Skip to content

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

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

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];
}