Skip to content

Task-space Admittance Control

System and Controller Configurations

IndySDK provides several libraries such as position controller, admittance trajectory generator, several filters, and the shared memory. In this example, task-space admittance control is implemented with such libraries.

This example covers the following items. How to...

  • Transform F/T sensor coordinate.
  • Use H-infinity position controller.
  • Initialize the nominal system.
  • Use forward dynamic position controller.
  • Realize three dimensional admittance trajectory in the task-space.
  • Use low-pass filter.

Example Code

Header File

As a first step, generate the task control component. Then, add several header files to use the shared memory and filters as follows:

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

// for the use of lowpass filter and filtered derviative
#include <Util/Filter.h>
#include <math.h>

Before designing the task-space admittance controller, define several member functions and variables in the TaskControlComponent class as follows:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
// degree of freedom (6dof)
enum
{
  JOINT_DOF = AbstractRobot6D::JOINT_DOF
};

// new type definitaions
typedef typename ROBOT::JointVec JointVec;
typedef typename ROBOT::JointMat JointMat;
typedef NRMKFoundation::FilterLowPass<double> LowPassFilter;
typedef NRMKFoundation::FilterDerivative<double> FilteredDerivative;

1
2
3
4
5
// member functions
void readFTSensor(ROBOT & robot);
void initNominalSystem(ROBOT & robot);
void computeJDet(ROBOT & robot);
void setBias() {_ftBias = _ftSensor;}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
private:
    // getting nominal robot dynamics
    JointMat            _Mn, _Cn;
    JointVec            _Gn;

private:
    // getting admittance trajectory
    TaskPosition        _tposPxy;
    TaskVelocity        _tvelPxy;
    TaskAcceleration    _taccPxy;

private:
    // with respect to ft sensor
    LieGroup::Wrench    _ftBias;
    LieGroup::Wrench    _ftSensor;
    LieGroup::Rotation  _ft_sensor_orientation;
    LieGroup::Wrench    _ftTransformed, _ftTransformed_filtered;
    LowPassFilter       _ftLowPass[6];

private:
    // reset control, instaneous time, control gains
    bool                _reset_ctrl;
    double              _delT;
    TaskVec             _kp, _kv, _ki;

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
TaskControlComponent::TaskControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    indyShm = new NRMKFramework::ShmemManager(INDY_SHM_NAME, INDY_SHM_LEN);

    for(int i=0; i<6; i++) _ftBias[i] = 0;
}

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

1
2
3
4
5
6
7
TaskControlComponent::~TaskControlComponent()
{
    if (_robotNom != NULL) delete _robotNom;

    if (indyShm != NULL)
        delete indyShm;
}

Source File

Based on position control libraries (i.e., H-infinity and Forward Dynamic controllers), design admittance controller to track generated admittance trajectories.

To this end, fill in the initialize() function as follows:

  • set the coordinate of the F/T sensor to be aligned with the coordinate of the end-effector.
  • Initialize the position controller to be used.
  • Update the sampling time (delt) to the global variable.
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
void TaskControlComponent::initialize(ROBOT & robot, double delt)
{
  // create a nominal robot
    _robotNom = AbstractController::createRobotObj();

  // 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 position controller
#if defined(HINF_CTR)
    robot.initHinfController(delt);
  robot.resetHinfController();
#else
    robot.initForwardDynController(delt);
  robot.resetForwardDynController();
#endif

  // update sampling time
  _delT = delt;

  //initialize task-space error
  robot.initTaskErr(delt);
}

In the setGains() function, update the control gains to use them globally as follows:

1
2
3
4
5
6
void TaskControlComponent::setGains(TaskVec const & kp, TaskVec const & kv, TaskVec const & ki)
{
    _kp = kp;
    _kv = kv;
    _ki = ki;
}

For each control task, to reset the controller and the bias value of F/T sensor, implement the following code into the reset() function.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
void TaskControlComponent::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();

    for(int j=0; j<JOINT_DOF; j++){
        for(int i=0; i<2; i++){
            _ftLowPass[j].filtered(i) = 0.0;
            _ftLowPass[j].raw(i) = 0.0;
        }
    }

    // reset flag defined for controller reset
    _reset_ctrl = true;
}

In the computeControlTorq() function, the admittance controller is implemented like 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
int TaskControlComponent::computeControlTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir, TaskPosition const & posDesired, TaskVelocity const & velDesired, TaskAcceleration const & accDesired, JointVec & torque)
{
    if (indyShm == NULL)
    {
        printf("Shared memory is missed\n");
    }

  // reset admittance trajectory and position controller
    if (_reset_ctrl)
    {
      printf("initialized\n");

      //reset nominal system "robotNom"; the function is defined below
      initNominalSystem(robot);
      //reset admittancd trajectory
      robot.resetAdmittanceTraj(_delT);
      //reset position controllers
#if defined(HINF_CTR)
      robot.resetHinfController();
#else
      robot.resetForwardDynController();
#endif
      _reset_ctrl = false;
    }

    // read ft sensor value; the function is defined below
    readFTSensor(robot);

    // change ft sensor direction x,y,z,u,v,w to u,v,w,x,y,z
    for (int i=0; i<3; i++) _ftTransformed[i] = _ftSensor[i+3];
    for (int i=0; i<3; i++) _ftTransformed[i+3] = _ftSensor[i];

    // lowpass filtering of ft sensor values
    LieGroup::Wrench _ftTransFiltered, _ftTransFilteredLow;
    for (int i=0; i<INDY_NUM_TASK_AXES; i++)
    {
      _ftLowPass[i].filter(_ftTransformed[i], _ftTransformed_filtered[i]);
    }

    // calculate admittance trajecotry
    // based on desired trajectories and ft sensor values
    robot.updateAdmittanceTraj(posDesired, velDesired, accDesired, _tposPxy,
    _tvelPxy, _taccPxy, _ftTransformed, _ftTransformed_filtered, -1);

    // set control gains and update admittance trajecotry to controllers
#if defined(HINF_CTR)
    robot.setHinfControlGain(_kp, _kv, _ki);
    robot.HinfController(gravDir, _tposPxy, _tvelPxy, _taccPxy);
#else
    robot.setForwardDynControlGain(_kp, _kv, _ki);
    robot.ForwardDynController(*_robotNom, _Mn, _Cn, _Gn, _tposPxy, _tvelPxy,
    gravDir);
#endif

    torque = robot.tau();

    return 0;   //return 1: Close to Singularity
}

resetNominalSystem() and readFTSensor() functions used in the computeControlTorq() are respectively given as follows:

First, define resetNominalSystem() function to update the values of the robot object to those of the robotNom object. Note that resetNominalSystem() is used when restarting the task.

1
2
3
4
5
6
7
8
9
void TaskControlComponent::initNominalSystem(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();
}

Moreover, define the readFTSensor() function to transform the coordinate of the F/T sensor to be aligned with the global coordinate of the robot.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
void TaskControlComponent::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];
}