Getting accustomed with torque/impedance interfaces

This tutorial shows how to use the following interfaces:

yarp::dev::IControlMode

yarp::dev::ITorqueControl

yarp::dev::IImpedanceControl

yarp::dev::IInteractionControl

Introduction

Prerequisites, see Getting accustomed with motor interfaces

The code described in this tutorial can be found at src/motorControlImpedance/tutorial_arm_joint_impedance.cpp
The example is based on the previous tutorial src/motorControlBasic/tutorial_arm.cpp

The example shows how to open the ITorqueControl to show the arm joint torques and how to change the control mode of the elbow joint from position to impedance control.

A detailed reference of the yarp interfaces used in this tutorial (IControlMode, ITorqueControl, IImpedanceControl, IInteractionControl) can be found in the YARP doxygen documentation and in the YARP Control Modes documentation.

Initialization

Let us assume the module wholeBodyDynamics is up and running. The wholeBodyDynamics module, based on the iDyn library acts as a server and is responsibile for the calculation of the joint torques. In order to access to the torque values, our program has to declare and open the yarp ITorqueControl interface, in the same manner as you open the IPositionControl interface (see previous tutorial: Getting accustomed with motor interfaces). We can also open the IControlMode interface (resposibile to set the control mode of each joint (position/velocity/impedance etc.), the IImpedanceControl interface, used to set the impedance parameters (joint stiffness/ joint damping etc) and the IInteractionControl interface, used to set a joint in stiff or compliant interaction mode.

IPositionControl *pos;
IEncoders *encs;
IControlMode *ictrl;
IInteractionMode *iint;
IImpedanceControl *iimp;
ITorqueControl *itrq;

and:

bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
ok = ok && robotDevice.view(ictrl);
ok = ok && robotDevice.view(iint);
ok = ok && robotDevice.view(iimp);
ok = ok && robotDevice.view(itrq);
 
if (!ok) {
    printf("Problems acquiring interfaces\n");
    return 0;
}

We can thus start with checking how many axes we can control by doing:

int jnts = 0;
pos->getAxes(&jnts);

Let's create some vectors for later use:

Vector tmp;
Vector encoders;
Vector command_position;
Vector command_velocity;
 
tmp.resize(jnts);
encoders.resize(jnts);
...

Let's now initialize the controllers and start up the amplifiers:

// we need to set reference accelerations used to generate the velocity
// profile, here 50 degrees/sec^2
int i;
for (i = 0; i < jnts; i++) {
    tmp[i] = 50.0;
}
pos->setRefAccelerations(tmp.data());
for (i = 0; i < jnts; i++) {
    tmp[i] = 10.0;
    pos->setRefSpeed(i, tmp[i]);
}

if needed we can check the position of our axes by doing:

enc->getEncoders(encoders.data());

which reads the values of the motor encoders into a vector of doubles of size ''jnts''.

Position control

First do:

int i;
for (i = 0; i < jnts; i++) {
    tmp[i] = 40.0;
}
pos->setRefSpeeds(tmp.data());

which sets the reference speed for all axes to 40 degrees per second. The reference speed is used to interpolate the trajectory between the current and the desired position.

Now you can do:

bool ok = pos->positionMove(command_position.data());

where ''command_position'' is the desired position. This starts a movement of all axes toward the desired position.

Reading the torques of a joint

You can use the ITorqueControl interface to read the torque at a specific joint.

Vector torques;
torques.resize(nj);
itrq->getTorques(torques.data());

Using different control modes: Position Control

You can change the impedence parameters of a joint at any time:

double stiffness = 0.111;          // stiffness coefficient, units are Nm/deg
double damping =  0.014;           // damping coefficient,   units are Nm/(deg/s)
bool ok = iimp->setImpedance(joint, stiffness, damping);

You can use IInteractionMode interface to change the interaction mode of a specific joint to compliant interaction mode:

ictrl->setPositionMode(3);
iint->setInteractionMode(3,VOCAB_IM_COMPLIANT);

When in position control mode with compliant interaction mode, the low level controller (DSP) computes a refence torque Td as:

\[ T_d = K_{spring} * (q_d-q) - K_{damp} * \dot{q} + F_{offset} \]

which is tracked by the internal PID regulator.

You can specify a new equilibrium position using the usual position interface, because the control mode of the joint is still VOCAB_CM_POSITION even if the interaction mode has changed:

bool ok = pos->positionMove(command_position.data());

If you need to change the interaction mode back to stiff mode:

iint->setInteractionMode(3,VOCAB_IM_COMPLIANT);

An ImpedanceVelocity control mode is also available. In this control modality you can use vel->VelocityMove() command in order to command a velocity while mantaining the impedance specified with the iimp->setImpedance(joint, stiffness, damping, dorce_offset) command.

Using different control modes: Torque Control

Torque control mode can be used to directly specify a reference torque at a joint.

ictrl->setTorqueMode(jnt);
 
double jnt_torque= 0.0; //expressed in Nm
itrq->setRefTorque(jnt_torque); 

The gains of the PID controller resposibile of tracking the torque reference can be obtained using ITorqueControl::getTorquePid() method.

NOTE: the reference torque that you can specify on the icub joints is limited (clamped) to a given value. You can check this value in the yarprobotinterface configuration files for your robot.

NOTE2: near the hardware limits of the robot, the torque PID controlled is disabled, in order to prevent to command a torque againts the hardware limits. The PID controller is automatically re-enabled as soon as the joint is moved far the limits (You can view the value of the limites using the IControlLimits interface).

NOTE3: during calibration/parking of the robot, yarprobotinterface automatically sets the control mode of all the joints to position control mode with stiff interaction mode.

NOTE4: If your application changes the control mode/interaction mode of one joint, it's good practice to reset it to the previous modality when your application quits.

Closing the device

When you are done with controlling the robot don't forget to close the device:

robotDevice.close();

More interfaces

To read more about the interfaces that are available in YARP read the doxygen online documentation and in the YARP Control Modes documentation.

Code

See code in: src/motorControlImpedance/tutorial_arm_joint_impedance.cpp