Skip to content

Commit

Permalink
add working code for impedance spring control
Browse files Browse the repository at this point in the history
  • Loading branch information
@minhnh
minhnh committed Jul 2, 2020
1 parent 9d9b441 commit 64764d0
Show file tree
Hide file tree
Showing 3 changed files with 118 additions and 37 deletions.
8 changes: 4 additions & 4 deletions generated/abag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,10 +91,10 @@ void gainAdapater_sched(double const * e_bar, double const * gain_threshold, dou
/* definitions of root schedules */
void abag_sched(abagState_t * abagState, double const * error, double * actuation, double const * alpha) {
/* data block declarations */
const double bias_threshold_xi = 0.000457;
const double bias_step_delta = 0.000500;
const double gain_threshold_xi = 0.602492;
const double gain_step_delta = 0.003552;
const double bias_threshold_xi = 0.000407;
const double bias_step_delta = 0.000400;
const double gain_threshold_xi = 0.502492;
const double gain_step_delta = 0.002;

/* fixed data flow schedule */
errSign(error, &(abagState->signedErr_access));
Expand Down
145 changes: 113 additions & 32 deletions src/control_kinova.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,10 @@
#include <chrono>
#include <time.h>

#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include "kinova_util.h"
#include "abag.h"
#include "constants.hpp"
Expand All @@ -25,6 +29,7 @@ namespace sc = std::chrono;

#define PORT 10000
#define PORT_REAL_TIME 10001
#define PI 3.14159265358979323846
#define DEG_TO_RAD(x) (x) * PI / 180.0
#define RAD_TO_DEG(x) (x) * 180.0 / PI
#define ACTUATOR_COUNT 7
Expand All @@ -41,6 +46,22 @@ void example_cyclic_torque_control (
std::cout << "loading URDF file at: " << UrdfPath << std::endl;
KDL::Tree kinovaTree; KDL::Chain kinovaChain;
loadUrdfModel(UrdfPath, kinovaTree, kinovaChain);
// Initialize solvers
const KDL::JntArray ZERO_ACCEL_ARRAY(7);
const KDL::Wrenches ZERO_WRENCHES(kinovaChain.getNrOfSegments(), KDL::Wrench::Zero());

KDL::Jacobian jacobianEndEff(kinovaChain.getNrOfJoints());
KDL::Frame endEffPose;
Eigen::Matrix<double, 6, 1> endEffForce;
endEffForce.setZero();

int returnFlag = 0;
KDL::ChainJntToJacSolver jacob_solver(kinovaChain);
KDL::ChainFkSolverPos_recursive fk_solver_pos(kinovaChain);
std::shared_ptr<KDL::ChainIdSolver_RNE> idSolver = std::make_shared<KDL::ChainIdSolver_RNE>(kinovaChain,
KDL::Vector(0.0, 0.0, -9.81289));

KDL::JntArray jntCmdTorques(7), jntPositions(7), jntVelocities(7), jnt_torque(7), jntImpedanceTorques(7);

// Clearing faults
try
Expand Down Expand Up @@ -81,17 +102,27 @@ void example_cyclic_torque_control (
auto control_mode_message = k_api::ActuatorConfig::ControlModeInformation();
control_mode_message.set_control_mode(k_api::ActuatorConfig::ControlMode::TORQUE);

int last_actuator_device_id = 7;
actuator_config->SetControlMode(control_mode_message, last_actuator_device_id);
// int last_actuator_device_id = 7;
// actuator_config->SetControlMode(control_mode_message, last_actuator_device_id);
for (int actuator_id = 1; actuator_id < ACTUATOR_COUNT + 1; actuator_id++)
actuator_config->SetControlMode(control_mode_message, actuator_id);


const std::vector<double> joint_torque_limits {39.0, 39.0, 39.0, 39.0, 9.0, 9.0, 9.0};
const double alpha = 0.8;
const std::vector<double> cart_force_limit {5.0, 5.0, 5.0, 5.0, 5.0, 5.0}; // N
const double alpha = 0.75;
double desired_vel = 6.0, current_vel = 0.0; //deg/s
double error = 0.0;
double abag_command = 0.0;
abagState_t abagState;
initialize_abagState(&abagState);
double desiredPosX = 0.0, desiredPosY = 0.0, desiredPosZ = 0.0;
double errorX = 0.0, errorY = 0.0, errorZ = 0.0;
double abagCommandX = 0.0, abagCommandY = 0.0, abagCommandZ = 0.0;

abagState_t abagStateX;
initialize_abagState(&abagStateX);
abagState_t abagStateY;
initialize_abagState(&abagStateY);
abagState_t abagStateZ;
initialize_abagState(&abagStateZ);

FILE * fp = fopen("simulated_data.csv", "w+");
fprintf(fp, "error, signed, bias, gain, e_bar, e_bar_prev, actuation, currentVel\n");

Expand All @@ -102,12 +133,14 @@ void example_cyclic_torque_control (
const sc::microseconds TASK_TIME_LIMIT_MICRO(TASK_TIME_LIMIT_SEC * SECOND_IN_MICROSECONDS);
const sc::microseconds LOOP_DURATION(SECOND_IN_MICROSECONDS / RATE_HZ);

int iterationCount = 0;
int slowLoopCount = 0;
sc::time_point<sc::steady_clock> controlStartTime = sc::steady_clock::now();;
sc::time_point<sc::steady_clock> loopStartTime;
sc::duration<int64_t, nano> totalElapsedTime;
while (loopStartTime - controlStartTime < TASK_TIME_LIMIT_MICRO)
{
iterationCount++;
loopStartTime = sc::steady_clock::now();
totalElapsedTime = loopStartTime - controlStartTime;

Expand All @@ -122,41 +155,87 @@ void example_cyclic_torque_control (
throw;
}

// for (int i = 0; i < ACTUATOR_COUNT; i++)
// {
// jnt_position(i) = DEG_TO_RAD(base_feedback.actuators(i).position()); //deg
// jnt_velocity(i) = DEG_TO_RAD(base_feedback.actuators(i).velocity()); // deg
// jnt_torque(i) = base_feedback.actuators(i).torque(); //nm
// }
for (int i = 0; i < ACTUATOR_COUNT; i++)
{
jntPositions(i) = DEG_TO_RAD(base_feedback.actuators(i).position()); //deg
jntVelocities(i) = DEG_TO_RAD(base_feedback.actuators(i).velocity()); // deg
jnt_torque(i) = base_feedback.actuators(i).torque(); //nm
}

// Kinova API provides only positive angle values
// This operation is required to align the logic with our safety monitor
// We need to convert some angles to negative values
// if (jnt_position(1) > DEG_TO_RAD(180.0)) jnt_position(1) = jnt_position(1) - DEG_TO_RAD(360.0);
// if (jnt_position(3) > DEG_TO_RAD(180.0)) jnt_position(3) = jnt_position(3) - DEG_TO_RAD(360.0);
// if (jnt_position(5) > DEG_TO_RAD(180.0)) jnt_position(5) = jnt_position(5) - DEG_TO_RAD(360.0);

// for (int i = 0; i < ACTUATOR_COUNT; i++)
// {
// base_command.mutable_actuators(i)->set_position(base_feedback.actuators(i).position());
// base_command.mutable_actuators(i)->set_torque_joint(jnt_command_torque(i));
if (jntPositions(1) > DEG_TO_RAD(180.0)) jntPositions(1) = jntPositions(1) - DEG_TO_RAD(360.0);
if (jntPositions(3) > DEG_TO_RAD(180.0)) jntPositions(3) = jntPositions(3) - DEG_TO_RAD(360.0);
if (jntPositions(5) > DEG_TO_RAD(180.0)) jntPositions(5) = jntPositions(5) - DEG_TO_RAD(360.0);

// current_vel = base_feedback.actuators(6).velocity();
// if (totalElapsedTime > TASK_TIME_LIMIT_MICRO * 0.1 && totalElapsedTime < TASK_TIME_LIMIT_MICRO * 0.9) {
// error = desired_vel - current_vel;
// } else {
// error = 0.0 - current_vel;
// }

current_vel = base_feedback.actuators(6).velocity();
if (totalElapsedTime > TASK_TIME_LIMIT_MICRO * 0.1 && totalElapsedTime < TASK_TIME_LIMIT_MICRO * 0.9) {
error = desired_vel - current_vel;
} else {
error = 0.0 - current_vel;
// gravity compensation
returnFlag = idSolver->CartToJnt(jntPositions, ZERO_ACCEL_ARRAY, ZERO_ACCEL_ARRAY, ZERO_WRENCHES, jntCmdTorques);
if (returnFlag != 0) break;
returnFlag = jacob_solver.JntToJac(jntPositions, jacobianEndEff);
if (returnFlag != 0) break;
returnFlag = fk_solver_pos.JntToCart(jntPositions, endEffPose);
if (returnFlag != 0) break;

// returnFlag = fk_solver_vel.JntToCart(jntPositions, jnt_vel_, end_eff_twist);
// if (returnFlag != 0) break;

if (iterationCount == 1)
{
desiredPosX = endEffPose.p(0) + 0.03;
desiredPosY = endEffPose.p(1) + 0.03;
desiredPosZ = endEffPose.p(2) + 0.03;
std::cout << "desired: " << desiredPosX << ", " << desiredPosY << ", " << desiredPosZ << std::endl;
}

errorX = desiredPosX - endEffPose.p(0);
errorY = desiredPosY - endEffPose.p(1);
errorZ = desiredPosZ - endEffPose.p(2);

abag_sched(&abagStateX, &errorX, &abagCommandX, &alpha);
abag_sched(&abagStateY, &errorY, &abagCommandY, &alpha);
abag_sched(&abagStateZ, &errorZ, &abagCommandZ, &alpha);

endEffForce(0) = abagCommandX * cart_force_limit[0];
endEffForce(1) = abagCommandY * cart_force_limit[1];
endEffForce(2) = abagCommandZ * cart_force_limit[2];
// std::cout << end_eff_force.transpose() << std::endl;

jntImpedanceTorques.data = jacobianEndEff.data.transpose() * endEffForce;

// std::cout << errorX << ", " << errorY << ", " << errorZ << std::endl;
for (int i = 0; i < ACTUATOR_COUNT; i++)
{
jntCmdTorques(i) = jntCmdTorques(i) + jntImpedanceTorques(i);
if (jntCmdTorques(i) >= joint_torque_limits[i]) jntCmdTorques(i) = joint_torque_limits[i] - 0.001;
else if (jntCmdTorques(i) <= -joint_torque_limits[i]) jntCmdTorques(i) = -joint_torque_limits[i] + 0.001;
base_command.mutable_actuators(i)->set_position(base_feedback.actuators(i).position());
base_command.mutable_actuators(i)->set_torque_joint(jntCmdTorques(i));
}

// std::cout << jntCmdTorques.data.transpose() << std::endl;
// control
// fprintf(fp, "%.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\n",
// errorX, abagStateX.signedErr_access, abagStateX.bias_access, abagStateX.gain_access, abagStateX.eBar_access,
// abagCommandX, endEffPose.p(0));
// fprintf(fp, "%.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\n",
// errorY, abagStateY.signedErr_access, abagStateY.bias_access, abagStateY.gain_access, abagStateY.eBar_access,
// abagCommandY, endEffPose.p(1));
fprintf(fp, "%.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\n",
error, abagState.signedErr_access, abagState.bias_access, abagState.gain_access,abagState.eBar_access,
abag_command, current_vel);
abag_sched(&abagState, &error, &abag_command, &alpha);
errorZ, abagStateZ.signedErr_access, abagStateZ.bias_access, abagStateZ.gain_access, abagStateZ.eBar_access,
abagCommandZ, endEffPose.p(2));
// abag_sched(&abagState, &error, &abag_command, &alpha);

base_command.mutable_actuators(6)->set_position(base_feedback.actuators(6).position());
// base_command.mutable_actuators(6)->set_position(base_feedback.actuators(6).position());

base_command.mutable_actuators(6)->set_torque_joint(abag_command * joint_torque_limits[6] * 0.8);
// base_command.mutable_actuators(6)->set_torque_joint(abag_command * joint_torque_limits[6] * 0.8);
// base_command.mutable_actuators(6)->set_torque_joint(1.0);

// Incrementing identifier ensures actuators can reject out of time frames
Expand Down Expand Up @@ -187,7 +266,9 @@ void example_cyclic_torque_control (
std::cout << "Number of loops which took longer than specified duration: " << slowLoopCount << std::endl;
// Set first actuator back in position
control_mode_message.set_control_mode(k_api::ActuatorConfig::ControlMode::POSITION);
actuator_config->SetControlMode(control_mode_message, last_actuator_device_id);
for (int actuator_id = 1; actuator_id < ACTUATOR_COUNT + 1; actuator_id++)
actuator_config->SetControlMode(control_mode_message, actuator_id);
// actuator_config->SetControlMode(control_mode_message, last_actuator_device_id);
std::cout << "Torque control example clean exit" << std::endl;

// Set the servoing mode back to Single Level
Expand Down
2 changes: 1 addition & 1 deletion src/kinova_util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ void loadUrdfModel(const std::string &pUrdfPath, KDL::Tree &pKinovaTree, KDL::Ch
}

// Extract KDL chain from KDL tree
pKinovaTree.getChain(constants::kinova::FRAME_JOINT_0, constants::kinova::FRAME_JOINT_6, pKinovaChain);
pKinovaTree.getChain(constants::kinova::FRAME_JOINT_0, constants::kinova::FRAME_END_EFFECTOR, pKinovaChain);
}

void handleKinovaException(k_api::KDetailedException& ex) {
Expand Down

0 comments on commit 64764d0

Please sign in to comment.