Skip to content

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

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 <Indy/DynamicAnalysis6D.h>
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;

//for the use of shared memeory
#include <NRMKFramework/Indy/Sharedmemory/SharedData.h>
NRMKFramework::ShmemManager * indyShm = NULL;
NRMKIndy::SharedData::ExtraIOData extioData; // extra input-output data
NRMKIndy::SharedData::RobotControlSharedData ctrData; // control data
NRMKIndy::SharedData::RobotControlStatusSharedData ctrStatusData;

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
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();
}

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
45
46
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
    JointVec tauPD;

    kp.setZero();
    kd.setZero();
    tauPD.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; i<JOINT_DOF; i++)
    {
        switch(i)
        {
        case 0:
        case 1:
            if(sim_mode) Mhat(i,i) = M(i,i);
            else Mhat(i,i) = M(i,i) + 40;
            break;
        case 2:
            if(sim_mode) Mhat(i,i) = M(i,i);
            else Mhat(i,i) = M(i,i) + 20;
            break;
        case 3:
        case 4:
        case 5:
            if(sim_mode) Mhat(i,i) = M(i,i);
            else  Mhat(i,i) = M(i,i) + 10;
            break;
        }
    }

    robot.idyn_gravity(LieGroup::Vector3D(0, 0, -GRAV_ACC));
    robot.computeFK(_tpos, _tvel);
    robot.computeJacobian(_tpos, _tvel, _J, _Jdot);

    _robotNom->idyn_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.2;
        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 = 1.0*KC.cwiseProduct(err) + 1.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
void JointControlComponent::reset()
{
    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;
}