Skip to content

Task-space Impedance Control

System and Controller Configurations

This example is the extension of the Joint-space Impedance Control Example. More specifically, Task-space impedance control is implemented by using joint-space impedance controller and jacobian inverse method. As for the system configuration briefly, 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, please refer to FT Sensor-based Friction Compensation Example)

This example is interested in...

  • How to implement task-space control using joint impedance control
  • How to calculate "Jacobian inverse"

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, let's generate the task control component. Then, add the SharedData.h and define ShmemManager so as to handle shared memory (which is used for getting F/T sensor values).

1
2
3
4
//for the use of shared memeory
#include "NRMKFramework/Indy/Sharedmemory/SharedData.h"
NRMKFramework::ShmemManager * indyShm = NULL;
NRMKIndy::SharedData::RobotControlSharedData ctrData; // control data

Next, modify the header file (i.e., TaskControlComponent class) as follows:

  • Define JointVec and JointMat
  • Add the impedance control function computeImpedanceControlTorq
  • Add resetNominalSystem to initialize the system model for every control sequence
  • Add setBias to compensate for F/T sensor bias
  • Add readFTSensor to transform the end-effector's wrench measured from F/T sensor
  • Define several variables (see the comments below)
 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
class TaskControlComponent : public AbstractTaskController<AbstractRobot6D>
{
    enum
    {
        JOINT_DOF = AbstractRobot6D::JOINT_DOF
    };
    typedef AbstractTaskController<AbstractRobot6D> AbstractController;
    typedef Eigen::Matrix<double, 6, 1> TaskVec;
    typedef Eigen::Matrix<double, 6, 6> TaskJacobian;
    typedef typename ROBOT::JointVec JointVec;
    typedef typename ROBOT::JointMat JointMat;

public:
    TaskControlComponent();
    virtual ~TaskControlComponent();

    void initialize(ROBOT & robot, double delt);
    void setPassivityMode(bool enable);
    void setGains(TaskVec const & kp, TaskVec const & kv, TaskVec const & ki);
    void setImpedanceParams(TaskVec const & iMass, TaskVec const & iDamping,
    TaskVec const & iStiffness, TaskVec const & iKi = TaskVec::Zero());

    void setImpedanceGains(TaskVec const & iKp, TaskVec const & iKv, TaskVec
    const & iKi, TaskVec const & iKf = TaskVec::Zero());

    void reset();
    int computeControlTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir,
    TaskPosition const & posDesired, TaskVelocity const & velDesired,
    TaskAcceleration const & accDesired, JointVec & torque);

    void computeImpedanceControlTorq(TaskPosition const & posDesired,
    TaskVelocity const & velDesired, JointVec const & q, JointVec const &
    qdot, JointVec & torque);

    void resetNominalSystem(ROBOT & robot);
    void setBias() {_ftBias = _ftSensor;}
    void readFTSensor(ROBOT & robot);

private:
    ROBOT * _robotNom;
    // flag to initialize nominal robot
    bool _reset_ctrl;
    // flag to set control mode (true: simulation mode, false: real mode)
    bool _sim_mode;
    // sampling time
    double _delT;
private:
    //Task space position (R,r) and velocity (v,w)
    TaskPosition _tpos, _tposNom;
    TaskVelocity _tvel, _tvelNom;
    // Task jacobian
    TaskJacobian _J, _JNom;
    TaskJacobian _Jdot, _JdotNom;
    // Robot dynamic parameters
    JointMat _Mn, _Cn, _Dn, _Mhat;
    JointVec _gn;
    // control input for gravity compensation of real and nominal robots
    JointVec _tauGrav, _tauGravNom;

private:
    // with respect to ft sensor
    JointVec _tauExt;
    LieGroup::Wrench    _ftBias;
    LieGroup::Wrench    _ftSensor;
    LieGroup::Rotation  _ft_sensor_orientation;
};  

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
TaskControlComponent::TaskSpaceComplianceControl()
: 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;

    // initialization
    _delT = 0.00025;
    _Dn.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
TaskControlComponent::~TaskSpaceComplianceControl()
{
  // destroy nominal robot
    if (_robotNom != NULL) delete _robotNom;

    // destroy shared memory
    if (indyShm != NULL)
        delete indyShm;
}

As mentioned above, the computeImpedanceControlTorq function is defined to implement the joint-space impedance control while performing task-space regulation. To this end, we used the PD (Proportional-Derivative)-type impedance control 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
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
void TaskControlComponent::computeImpedanceControlTorq(TaskPosition const
& posDesired, TaskVelocity const & velDesired, JointVec const & q, JointVec
const & qdot, JointVec & torque)
{

    // calculate jacobian inverse
    TaskJacobian JJtrans;
    JJtrans.setZero();
    JJtrans = _J*_J.transpose();
    Eigen::ColPivHouseholderQR<TaskJacobian> qr(JJtrans);

    TaskJacobian Jinv;
    Jinv.setZero();
    Jinv = _J.transpose()*qr.inverse();

    // calculate task error
    TaskVec e, edot;
    e.setZero();
    edot.setZero();

    _robotNom->computeTaskErr(_tposNom, _tvelNom, posDesired, velDesired,
                            e, edot);

    JointMat kp, kd;
    kp.setZero();
    kd.setZero();

    for (int i=0; i < JOINT_DOF; i++)
    {
        switch(i)
        {
        case 0:
        case 1:
            kp(i,i) = 70;
            kd(i,i) = 5;
//          kp(i,i) = 150;
//          kd(i,i) = 25;
            break;
        case 2:
            kp(i,i) = 25;
            kd(i,i) = 5;
//          kp(i,i) = 150;
//          kd(i,i) = 15;
            break;
        case 3:
        case 4:
            kp(i,i) = 25;
            kd(i,i) = 5;
//          kp(i,i) = 150;
//          kd(i,i) = 15;
            break;
        case 5:
            kp(i,i) = 15;
            kd(i,i) = 5;
            break;
        }
    }

    torque = kp*(Jinv*e) - kd*qdot;
}

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 ControlData structure. 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
int TaskControlComponent::computeControlTorq(ROBOT & robot, LieGroup::Vector3D
const & gravDir, TaskPosition const & posDesired, TaskVelocity const &
velDesired, TaskAcceleration const & accDesired, JointVec & torque)
{
    // read ft sensor value; the function is defined below
    readFTSensor(robot);

    // update initial states of nominal robot with those of real robot
    if(_reset_ctrl)
    {
        printf("initialized\n");

        //reset nominal system "robotNom"; the function is defined below
        resetNominalSystem(robot);

        //init_robotNom = false;
        _reset_ctrl = false;
    }

    // calculate robot dynamics
    _robotNom->computeDynamicsParams(gravDir, _Mn, _Cn , _gn);
    _Mhat = _Mn;

    for (int i = 0; i < JOINT_DOF; i++)
    {
        switch(i)
        {
        case 0:
        case 1:
            if(_sim_mode) _Mhat(i,i) = _Mn(i,i);
            else _Mhat(i,i) = _Mn(i,i) + 40;
            break;
        case 2:
            if(_sim_mode) _Mhat(i,i) = _Mn(i,i);
            else _Mhat(i,i) = _Mn(i,i) + 20;
            break;
        case 3:
        case 4:
        case 5:
            if(_sim_mode) _Mhat(i,i) = _Mn(i,i);
            else _Mhat(i,i) = _Mn(i,i) + 10;
            break;
        }
    }

    robot.idyn_gravity(gravDir);
    robot.computeFK(_tpos, _tvel);
    robot.computeJacobian(_tpos, _tvel, _J, _Jdot);

    _robotNom->idyn_gravity(gravDir);
    _robotNom->computeFK(_tposNom, _tvelNom);
    _robotNom->computeJacobian(_tposNom, _tvelNom, _JNom, _JdotNom);

    _tauExt = _J.transpose()*_ftSensor;
    _tauGrav = robot.tau();
    _tauGravNom = _robotNom->tau();

    //calculate impedance control input
    JointVec tauImp, tauAux;
    tauImp.setZero();
    tauAux.setZero();

    computeImpedanceControlTorq(posDesired, velDesired, _robotNom->q(),
                                _robotNom->qdot(), tauImp);

    JointVec K, KC, KD;
    if (_sim_mode)
    {
        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;
    }

    JointVec qddotNom, err, err_dot;
    qddotNom.setZero();
    err.setZero();
    err_dot.setZero();

    qddotNom = _Mhat.inverse()*(-(_Cn + 3.0*_Dn)*_robotNom->qdot()
              + K.cwiseProduct(tauImp + _tauExt));

    // update nominal states
    _robotNom->qdot() += (float) _delT*qddotNom;
    _robotNom->q() += (float) _delT*_robotNom->qdot();
    _robotNom->update();
    _robotNom->updateJacobian();

    // define errors between real and nominal robots
    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);

  // final robust impedance 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
22
23
void TaskControlComponent::readFTSensor(ROBOT & robot)
{

    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);
    TaskPosition _tposT;
    TaskVelocity _tvelT;
    LieGroup::Wrench _ftSensorTemp;
    _ftSensorTemp.setZero();

    // to transform the coordinate from F/T sensor frame to global frame
    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];
}

resetNominalSystem function is used to update the states of nominal robot to the states of real robot when starting motions.

1
2
3
4
5
6
7
8
9
void TaskControlComponent::resetNominalSystem(ROBOT & robot)
{
    _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();
}

initialize and reset functions are used to prepare the robot control.

  • createRobotObj is used to allocate the memory to _robotNom
  • setFTSensor defines the coordinate transformation between the end-effector and F/T sensor
  • In reset function, F/T sensor data is obtained to compensate for F/T sensor bias
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
void TaskControlComponent::initialize(ROBOT & robot, double delt)
{
    // create nominal robot
    _robotNom = AbstractController::createRobotObj();

    // check shared memory
    if (indyShm == NULL)
    {
        printf("Shared memory is missed\n");
    }

    // align ft sensor coordinates with robot's tool frame's coordinates
    _ft_sensor_orientation = LieGroup::Rotation(1,0,0,0,1,0,0,0,1);
    robot.setTFTSensor(_ft_sensor_orientation, LieGroup::Displacement::Zero());

    // initialize task error calculator
    robot.initTaskErr(delt);
}
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
void TaskControlComponent::reset()
{
    // to initialize nominal robot
    _reset_ctrl = true;

    // sim_mode/real_mode
    _sim_mode = true;

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