# Joint-space Impedance Control

## System and Controller Configurations

This example introduces the robust joint-level impedance control method which consists of two parts. First part is a robust control which enables to overcome the friction effect acting on robot joints and model uncertainties. Then, the impedance control is applied to the nominal robot that does not include the friction effect and model uncertainties. This control strategy can cope with the external force while ensuring the position control performance. As for the system configuration, the F/T sensor is attached to the end-effector and the interaction occurs only through the F/T sensor (for more details on the installation of F/T sensor, refer to FT Sensor-based Friction Compensation Example).

This example discusses about the following items.

• How to transform the coordinate of F/T sensor
• How to use robot dynamic models

Note

For safety, an emergency stop occurs when a certain amount of position error occurs in the actual robot. For this reason, the control algorithm introduced below may not work in certain control gain settings even if it works well in simulation.

## Example Code

To begin with, generate the joint control component. Then, add the following code in the header so as to use dynamic simulation libraries and shared memory:

  1 2 3 4 5 6 7 8 9 10 //for dynamic simulation of nominal robot #include typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis; //for the use of shared memeory #include NRMKFramework::ShmemManager * indyShm = NULL; NRMKIndy::SharedData::ExtraIOData extioData; // extra input-output data NRMKIndy::SharedData::RobotControlSharedData ctrData; // control data NRMKIndy::SharedData::RobotControlStatusSharedData ctrStatusData; // control status data 

Moreover, define the computeImpedanceControlTorq function and several relevant variables in the header as follows:

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 // type definition typedef typename ROBOT::JointMat JointMat; typedef typename ROBOT::TaskPosition TaskPosition; typedef typename ROBOT::TaskVelocity TaskVelocity; typedef typename ROBOT::TaskJacobian TaskJacobian; // To calculate the control input for impedance control int computeImpedanceControlTorq(JointVec const & qdes, JointVec const & q, JointVec const & qdot, JointVec const & tauGrav, JointVec & tauImp); void setBias() {_ftBias = _ftSensor;} void readFTSensor(ROBOT & robot); public: double _delT; double _t; int key; int flag_bias; private: ROBOT * _robotNom; bool init_robotNom; JointVec tauGrav, tauGrav_nom; JointVec fext, tauExt; JointMat M, C, D, Mhat; JointVec g; TaskPosition _tpos; TaskVelocity _tvel; TaskPosition _tpos_nom; TaskVelocity _tvel_nom; TaskJacobian _J, _Jdot, _J_nom, _Jdot_nom; private: // with respect to ft sensor LieGroup::Wrench _ftBias; LieGroup::Wrench _ftSensor; LieGroup::Rotation _ft_sensor_orientation; LieGroup::Wrench _ftTransformed, _ftTransformed_filtered; 

In the constructor, allocate the memory to the pointer variable, which allows to acquire the F/T sensor data from the shared memory (for more details, see IndyFramework or F/T Sensor and Gripper Example).

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 JointControlComponent::JointControlComponent() : AbstractController(USERNAME, EMAIL, SERIAL) , _robotNom(NULL) { // initialize shared memory // INDY_SHM_NAME: "indySHM" // INDY_SHM_LEN: 0x1000000 16MB == 0x000000~0xFFFFFF (~16777216) indyShm = new NRMKFramework::ShmemManager(INDY_SHM_NAME, INDY_SHM_LEN); for (int i=0; i<6; i++) _ftBias[i] = 0; // initialize sampling time: 4kHz control frequency _delT = 0.00025; // initialize variables D.setZero(); _J.setZero(); _Jdot.setZero(); Mhat.setZero(); tauExt.setZero(); } 

In the destructor, release the memory allocated for the nominal robot and shared memory.

 1 2 3 4 5 6 7 8 9 JointControlComponent::~JointControlComponent() { // destroy nominal robot if (_robotNom != NULL) delete _robotNom; // destroy shared memory if (indyShm != NULL) delete indyShm; } 

In the top of this example, the computeImpedanceControlTorq function was newly defined to implement the joint-space impedance controller. Here, let us implement the PD (Proportional-Derivative)-type impedance control as shown below (i.e., stiffness-damping control).

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 int JointControlComponent::computeImpedanceControlTorq(JointVec const & qdes, JointVec const & q, JointVec const & qdot, JointVec const & tauGrav, JointVec & tauImp) { // initialize parameters JointMat kp, kd; //p- and d-gains kp.setZero(); kd.setZero(); // set pd control gains for (int i=0; i < JOINT_DOF; i++) { switch(i) { //for 1st and 2nd joints case 0: case 1: kp(i,i) = 70; kd(i,i) = 5; break; //for 3rd joint case 2: kp(i,i) = 25; kd(i,i) = 3; break; //for 4th and 5th joints case 3: case 4: kp(i,i) = 25; kd(i,i) = 3; break; //for 6th joint case 5: kp(i,i) = 18; kd(i,i) = 1.5; break; } } tauImp = kp*(qdes - q) - kd*qdot; return 0; } 

The PD controller introduced above may not work well due to the effect of friction and gravity. To solve this problem, let construct the following robust controller into the computeControlTorque() function. This will attenuate the effect of friction and reduce the effect of inertia in the range of the stability margin.

• Get F/T sensor data from the shared memeory to detect the external force. As mentioned in F/T Sensor and Gripper Example, users can get F/T sensor data using either ExtraIOData or ControlData structures. This is defined in readFTSensor function to be introduced below.

• Change force configuration because the measured F/T sensor value is constructed in the order of (force, moment), while the Wrench (used in IndySDK) is constructed in the order of (moment, force). This is defined in readFTSensor function to be introduced below.

• Initialize the kinematic information of nominal robot with those of real robot when the new motion is started. Then, update the state variables and velocity kinematic information.

• Calculate the forward kinematics for both real and nominal robots. More specifically, calculate the acceleration of nominal robot using external forces, gravity forces, and impedance forces as inputs.

• Finally, calculate the auxiliary input based on the errors between nominal robot and real robot, which compensates the friction effect and model uncertainty.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 int JointControlComponent::computeControlTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir, JointVec const & qDesired, JointVec const & qdotDesired, JointVec const & qddotDesired, JointVec & torque) { // wait until servo is On NRMKIndy::SharedData::getControlStatusData(*indyShm, ctrStatusData); if(!ctrStatusData.isReady) { robot.idyn_gravity(gravDir); torque = robot.tau(); return 0; } // update time _t += _delT; // read ft sensor value; the function is defined below readFTSensor(robot); // update initial states of nominal robot with those of real robot if (init_robotNom == true) { //update target coordinates _robotNom->setTtarget(robot.Ttarget().R(), robot.Ttarget().r()); _robotNom->setTReference(robot.Tref().R(), robot.Tref().r()); _robotNom->q() = robot.q(); _robotNom->qdot() = robot.qdot(); _robotNom->update(); _robotNom->updateJacobian(); init_robotNom = false; } static bool sim_mode = true; // calculate real and nominal robot dynamics //DynamicAnalysis::JointDynamics(*_robotNom, M, C, g, // LieGroup::Vector3D(0, 0, -GRAV_ACC)); _robotNom->computeDynamicsParams(gravDir, M, C, g); Mhat = M; for (int i=0; iidyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC)); _robotNom->computeFK(_tpos_nom, _tvel_nom); _robotNom->computeJacobian(_tpos_nom, _tvel_nom, _J_nom, _Jdot_nom); // ** controller JointVec tauImp, tauAux; JointVec qddot_nom, err, err_dot; tauImp.setZero(); tauAux.setZero(); qddot_nom.setZero(); tauExt = _J.transpose()*_ftSensor; tauGrav = robot.tau(); tauGrav_nom = _robotNom->tau(); // set control gain JointVec K, KC, KD; if(sim_mode == true) { K << 1.0, 1.0, 1.0, 1.0, 1.0, 0.02; KC << 80, 80, 40, 25, 25, 25; KD << 55, 55, 30, 15, 15, 15; }else { K << 5.0, 5.0, 5.0, 5.0, 5.0, 5.0; KC << 80, 80, 40, 25, 25, 25; KD << 55, 55, 30, 15, 15, 15; } // calculate impedance control input computeImpedanceControlTorq(qDesired, _robotNom->q(), _robotNom->qdot(), tauGrav, tauImp); qddot_nom = Mhat.inverse()*(-(C + 3.0*D)*_robotNom->qdot() + K.cwiseProduct(tauImp + tauExt)); // update nominal states _robotNom->qdot() += (float) _delT*qddot_nom; _robotNom->q() += (float) _delT*_robotNom->qdot(); _robotNom->update(); _robotNom->updateJacobian(); // calcualte error err = _robotNom->q() - robot.q(); err_dot = _robotNom->qdot() - robot.qdot(); // calculate auxiliary control input tauAux = 4.0*KC.cwiseProduct(err) + 4.0*KD.cwiseProduct(err_dot); // calculate final control input torque = tauGrav - tauExt + K.cwiseProduct(tauImp + tauAux + tauExt); return 0; } 

readFTSensor function is used to get and transform F/T sensor data.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 void JointControlComponent::readFTSensor(ROBOT & robot) { NRMKIndy::SharedData::getControlData(*indyShm, ctrData); TaskPosition _tposT; TaskVelocity _tvelT; LieGroup::Wrench _ftSensorTemp; _ftSensorTemp.setZero(); robot.computeFK(_tposT, _tvelT); for (int i=0; i<6; i++) _ftSensorTemp[i] = ctrData.Fext[i]; _ftSensorTemp = _ftSensorTemp - _ftBias; LieGroup::HTransform T_ft_target = robot.Ttarget().icascade(robot.Tft()); LieGroup::Wrench _ftSensorTemp2 = T_ft_target.itransform(_ftSensorTemp); LieGroup::Wrench _ftSensorTransformed = LieGroup::Wrench(_tposT.R()*_ftSensorTemp2.f(), _ftSensorTemp2.n()); for (int i=0; i<3; i++) _ftSensor[i] = _ftSensorTransformed[i+3]; for (int i=0; i<3; i++) _ftSensor[i+3] = _ftSensorTransformed[i]; } 

Note that it is important to reset the controller. To this end, define the flag in the reset() function.

  1 2 3 4 5 6 7 8 9 10 11 12 13 void JointControlComponent::reset() { // get ft sensor values from shared memory NRMKIndy::SharedData::getControlData(*indyShm, ctrData); for (int i=0; i<6; i++) _ftSensor[i] = ctrData.Fext[i]; // compensate ft sensor bias setBias(); init_robotNom = true; } 

In addition to the joint position control mode, the direct teaching mode is supposed to be defined in the joint control component. In this example, apply the gravity compensation control.

  1 2 3 4 5 6 7 8 9 10 11 12 int JointControlComponent::computeGravityTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir, JointVec & torque) { // compute gravitational torque JointVec tauGrav; robot.idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC)); tauGrav = robot.tau(); // update torque control input torque = tauGrav; return 0; }