Force Control

From Wiki for iCub and Friends
Revision as of 07:17, 3 August 2011 by Vvv11 (talk | contribs) (Force Control on torso joints)
Jump to: navigation, search

Latest updates (Monday 01, August 2011)

  • New firmware available:
  • firmware build 57 for brushless control boards
  • firmware build 42 for 4DC control boards
  • force control of torso joints (3DOFs)
  • fixed a safety issue related to zero velocity commands sent in torque control mode
  • improved timeout watchdog for both 4DC/BLL boards
  • minor bug fixes
  • In order to enable force control on torso joints, also the robot .ini files have to be updated, as explained here.

Previous updates (Friday 04, March 2011)

  • New firmware available:
  • firmware build 55 for brushless control boards
  • firmware build 41 for 4DC control boards
  • safe checks on the joint limits for force/impedance control.
  • integral control added
  • back-emf compensation added.
  • faster routine for trajctory generation.
  • better decoupling for arm joints.
  • minor bug fixes
  • A new feature has been added in iCubInterface to inform you when a new firmware is available and verify the consistency of your system respect to the latest updates. To enable this feature, turn on the flag ICUB_CANPROTOCOL_STRICT in the cMake options when compiling the iCub repository.

Minimum Requirements for iCub v1.x

Hardware Requirements:

In order to run force control, your iCub must be equipped with the four 6-axis F/T Sensors. These sensors have been installed on the robot since v1.1 of the platform.

Firmware Requirements:

The PID control algorithm responsible of tracking the commanded torque at joint level is executed inside the DSP control boards. This means that the firmware of your robot needs to be updated in order to run joint level torque control. The minimum requirements are:

  • firmware build 50 for brushless control boards
  • firmware build 38 for 4DC control boards

However it is strongly recommended to use the latest firmware available, since it implements a better control algorithm respect to these old versions. Please refer to the firmware section for a description of the firmware update procedure.

Software Requirements:

Force control is one of the latest added feature of iCub. This means that even if your robot is equipped with the 6-axis F/T Sensors, maybe your .ini configuration files do not contains all the required parameters in order to run force control. In fact, new features have been recently added, and new configuration parameters have become mandatory. In order to check if the configuration files of your robot are properly updated, please refer to this section. One final remark: since iCub software is continuosly evolving, new features/improvements/bug fixes are constantly released, so it's a good idea to periodically update both your iCub and Yarp repository using SVN.

Joint-level torque Control in iCub v2.0 (future iCub)

The new robotic platform iCub v2.0 (the future release of iCub hardware) will use integrated joint torque sensors (strain gauges mounted on individual joints) in order to obtain joint-level torque control. This hardware is currently under development and is not available yet.

Joint-level torque Control in iCub v1.x (current iCub)

iCub v1.x is not equipped with joint-level torque sensors, but only with four 6-axis F/T sensors mounted on the arms and on the legs. iCub v1.x thus exploits a model-based approach based on a modified Newton-Euler algorithm (Ref: iDyn library) in order to estimate joint-level torques from the four proximal sensors. The controller is thus distributed in three different levels:

Control paths.jpg
  • wholeBodyTorqueObserver (application level): the modules takes the measurements from the 3DOF orientation tracker placed inside iCub's head and from the four F/T sensors of the robot limbs to make a model-based estimation of joint torques, with the hypothesis that external forces are applied only on the end-effector (wrist/ankle joints). For further information refer also to the wholeBodyTorqueObserver documentation

  • iCubInterface (middleware): it sends (through yarp ports) the 6-axis F/T sensors measurements to the wholeBodyTorqueObserver module and receives from it the computed joint torques. The estimated joint torque measurements are sent through the CAN bus to the DSP boards which perform the control.

  • motor control boards (firmware level): The control boards receive the computed estimation of the joint torques from iCubInterface and implement different PID control algorithms in order to track the desired position/torque commands. The type of control (i.e. position/torque/impedance control etc.) of a specific joint can be changed runtime by sending an appropriate command to the control board.

Ports and connections

wholeBodyTorqueObserver and iCubInterface communicate through yarp ports:

iCub ports are:

  • /icub/inertial provides 3DOF orientation tracker measurements
  • /icub/<part>/analog:o provides calibrated F/T measurements (an offset is present, due to the stresses of mounting)
  • /icub/joint_vsens/<part>:i acquires joint torque estimation from wholeBodyTorqueObserver module

wholeBodyTorqueObserver ports are:

  • /wholeBodyTorqueObserver/inertial:i receive inertial data
  • /wholeBodyTorqueObserver/<part>/FT:i reads F/T data from the analog ports
  • /wholeBodyTorqueObserver/<part>/Torques:o provides joint torque measurements

The port connections required to run force control are thus:

  • /icub/inertial -> /wholeBodyTorqueObserver/inertial:i
  • /icub/<part>/analog:o -> /wholeBodyTorqueObserver/<part>/FT:i
  • /icub/joint_vsens/<part>:i -> /wholeBodyTorqueObserver/<part>/Torques:o

Connecting all these ports for all the robot parts (left_arm, right_arm, left_leg, right_leg) is a laborious process and it's easy to make mistakes while typing the port names. For this reason, it's not recommended to make the connections manually, using the yarp connect command. Instead you can easily make all the necessary connections using the provided automated scripts. Refer to section How to start torque control

Available control modes

The key concept to comprehend how force control works and how to use it is the concept of control mode. The control mode represents the current control algorithm that is running on the firmware of the control boards to control a specific joint. For example, the position control modes implements a PID control that tracks the commanded trajectories, while the impedance control realizes a compliant position control by computing the reference torque that an inner torque control loop has to track, given an equilibrium position and the stiffness of a simulated spring. The control mode of a joint can be changed online, during the execution of your application, using the apposite yarp interfaces. In this way you can assign different control modes to different joints in order to obtain the desired behaviour (e.g. you can set some joints in position control mode to obtain a 'stiff' behaviour and other joint in impedance control mode to obtain a compliant behaviour).

Five different control modes are currently implemented in the firmware of the control boards:

  • Position control mode ███
(Typical input parameters: desired position, trajectory velocity)
Position control is the standard control mode. In this control mode, the motors PWM is computed using a PID controller the receives in input the desired joint position and the current measurement from the joint encoders:
Pid pos.jpg
Note that when you command a new joint position, you are not instantaneously assigning the reference qd in the above formula. Instead, a mimum jerk trajectory generator takes in input your commanded position and the desired velocity, and produces a smooth movement creating a sequence of position references qd tracked by the PID controller.
  • Velocity control mode ███
(Typical input parameters: desired velocity, acceleration)
Velocity control mode allows you to control the robot by assigning a desired volocity/acceleration to a joint. The control law is the same of position control, but in this case qd is not directly controlled by the user, but it is obtained from the integration of the commanded user velocity. Also in this case a minimum jerk profile generator is used.
  • Torque control mode ███
(Typical input parameters: reference torque)
Torque control mode allows you to directly control the robot joints torque:
Pid trq.jpg
In this case the motors PWM is computed using a PID controller the receives in input the desired joint torque and the current measured joint torque. Additionally, a PWM offset can be added to the output of the control algorithm. If both the commanded reference torque and the PWM offset is set to zero, the robot joint will be free to be moved in the space (eventually it will move down as an effect of the gravity acting on that joint).
  • Impedance Position control mode ███
(Typical input parameters: desired position, trajectory velocity + desired joint stiffness and damping)
Impedance control mode allows you to control the joint position and its compliance. In particular, you can control the equilibrium position of a virtual spring (using the standard yarp::dev::IPositionControl interface) and its stiffness/damping (using the yarp::dev::IImpedanceControl interface).
The control is implemented in the DSP firmware as follows:
Pid imp.jpg
Firstly, a reference torque is computed, accordingly to the input position and the commanded stiffness/damping parameters (Hooke's law). Secondly, the reference torque is tracked by a PID algorithm (same gains used by the torque control mode). By tuning the stiffness parameters, you can thus make the robot joint feeling like a hard or soft spring, while maintaining control on the desired joint position (note that the same mimum-jerk trajectory generator used by the position control is also used when position impedance control is running).
  • Impedance Velocity control mode ███
(Typical input parameters: desired velocity, acceleration + desired joint stiffness and damping)
The impedance velocity control mode is the corresponding impedance mode using velocity control. The control law is the same of the impedance position control, but in this case qd is not directly controlled by the user, but it is obtained from the integration of the commanded user velocity (also in this case minimum jerk profile generator is used).
  • Idle ███
(Typical input parameters: none)
This is not a real control mode, but represents the status of a joint in which the control is currently disabled (both because PWM has been deliberately turned off by the user or because a fault (e.g. overcurrent) occurred).

NOTE 1: The control mode of a joint can be set using the yarp::dev::iControlMode interface.

NOTE 2: When you send movement commands (i.e. position/velocity commands) to a joint, the obtained behaviour will change depending on the current control mode of the joint (e.g. a position command in position control mode will generate the standard stiff trajectory, while the same command executed in impedance control mode will change the equilibrium point of the simulated spring). Moreover some commands can also implicitly change the current control mode, even if no requests have been made using the yarp::dev::iControlMode interface. The following table summarizes the implemented behaviour:

Send IPositionControl::positionMove command Send IVelocityControl::velocityMove command Send ITorqueControl::setRefTorque command Send "enable PWM" command
position control mode
  • move to the desired position.
  • move with the desired velocity.
  • Automatically switch to velocity control mode.
  • command ignored.
  • command ignored.
velocity control mode
  • move to the desired position.
  • Automatically switch to position control mode.
  • move with the desired velocity.
  • command ignored.
  • Automatically switch to position control mode.
impedance position control mode
  • move to the desired position with compliant behaviour.
  • move with the desired velocity with compliant behaviour.
  • Automatically switch to impedeance velocity control mode.
  • command ignored.
  • Automatically switch to position control mode.
impedance velocity control mode
  • move to the desired position with compliant behaviour.
  • Automatically switch to impedance position control mode.
  • move with the desired velocity with compliant behaviour.
  • command ignored.
  • Automatically switch to position control mode.
torque control mode
  • command ignored.
  • command ignored.
  • Move the joint with the desired torque.
  • Automatically switch to position control mode.
motor idle
  • command ignored.
  • command ignored.
  • command ignored.
  • PWM on.
  • Automatically switch to position control mode.

How to start torque control

Running the wholeBodyTorqueObserver

The first step to achieve joint level torque control / playing with torque & impedance interfaces / running the demoForceControl module is to start the wholeBodyTorqueObserver module and connect the corresponding ports. Note that if this module is not running and the ports are not properly connected, any attempt to change the control mode of the joints to force/impedance control mode will result in a protection fault (motor going in idle state).

To run wholeBodyTorqueObserver, be sure iCubInterface is running. Then, open the wholeBodyTorqueObserver manager using the script: wholeBodyTorqueObserver.xml

the .xml configuration file used by is located in the path: $ICUB_ROOT/app/wholeBodyTorqueObserver/scripts. If this is the first time that you are running the module, rename the file wholeBodyTorqueObserver.xml.template to wholeBodyTorqueObserver.xml, edit it and set the console and stdio tags (by default, these tags are filled with the string 'console'/'icubsrv', change them with the name of the pc on your LAN where the module will run. Further information about running applications using the application manager can be found here

After having started the application manager, click on the button Run on the left of the wholeBodyTorqueObserver module. Do not run the gravityCompensator module yet. After this, be sure the robot is not touching or interacting with anything or anyone, and click on the Connect button. In this phase, the F/T measurements are aligned to the model of the robot. Do not touch the robot until this procedure has finished (almost 5 seconds after the Connect button has been pressed). After this, you will see the port connections becoming green on the bottom part of the window and you will be ready to run the force demo.



  • I'm clicking on the Run button in the application manager, but the module does not start. Why?
Typically this means that one of the dependencies indicated in top part of the application manager window is not fulfilled. In particular:
  • /icub/inertial port is not available (is iCubInterface running? check it)
  • yarp cannot find the machine (set in the 'nodes' section) on which the module has to run. Check the name of the machine and your network connection.

Additional notes:

  • If your iCub has no legs, do not use the standard script, but the following one: wholeBodyTorqueObserver_no_legs.xml

Running the gravityCompensator

The gravityCompensator is included in the same script used to launch the wholeBodyTorqueObserver module (i.e. wholeBodyTorqueObserver.xml). The module estimates the gravitational term acting on joints due to links weight and generates a feed-forward term which can be used to compensate the gravity when the joint is torque/impedance control mode. This module is particular useful if you want to control the robot in impedance position mode with low stiffness values. In this case, in fact, the accuracy of position control loop will be poor, because the gravity will act on the low-stiffness joint preventing it to reach the commanded position. On the contrary, if the gravity compensation module is running, the feed-forward term will help the joint to reach the commanded position even with low stiffness values.

Additional notes:

  • The torques estimation is perfomed by the iDyn library which includes a mechanical model of the iCub robot based on the CAD parameters. This means that a small drift can be present, due to the fact that the model obtained from the CAD slightly differs from the real iCub (consider the weight of the power cables, the additional skin etc.)
  • If the joint stiffness is zero and gravityCompensator is off, the gravity will make the joint to fall down regardless the commanded position. In this case, running the gravityCompensator is the only way to track to commanded position.
  • Be particular careful in this situation: consider a joint in impedance position control mode, with the gravity compensator turned off and low joint stiffness (e.g 0.01Nm/deg). In this case the position tracking error will be high: for example if you command a position of 30degrees, due to the gravity effect and the very low stiffness, the joint could able to reach only the position at 10 degrees (so the tracking error is 20 degrees). In this condition the impedance control loop is computing a command torque T=0.01*(30-10)=0.2Nm. Now, if you suddenly increase (with a step) the joint stiffness to a high value (for example i.e. 0.6Nm.deg) you obtain a command torque of T=0.5*(30-10)=12Nm which is a huge torque. The joint will move very fast (and joint tendons may be also damaged). In this case the solution is to increase the stiffness gradually, in order to make the position tracking error decrease before setting the joint in high-stiffness mode or turning on the gravity compensator. With the gravity compensator enabled the position error will be limited to few degrees even with low stiffness values.

Running the force control demo

Analogously to wholeBodyObserver, the demoForceControl module can be started using the application manager: demoForceControl.xml

In the opened application manager window, you will see four different modules (one for each part of the robot: left_arm, right_arm, left_leg, right_leg). You can run them individually in order to start the demo on a specific robot part. Additionally, in the bottom part of the window, you can open four terminal windows which you can use to send commands to the modules through a yarp rpc port (please refer to the demoForceControl manual for a list of the available commands).


Additional notes:

  • If your iCub has no legs, do not use the standard script, but the following one: demoForceControl_no_legs.xml
  • When the demoForceControl is launched, it sets in impedance position control mode the specified joints (the first five joints of the arm / all the six joints of the legs) and sets the stiffness/damping written in its configuration files.
  • The demoForceControl application uses the configuration (.ini) file located in iCub\main\app\demoForceControl\conf. You can change the default behaviour of the demoForceControl application by changing the stiffness/damping parameters written into these files. For example, the lines reported below are the contents of the iCub\main\app\demoForceControl\conf\leftArmFT.ini configuration file:
robot icub
part left_arm

Stiffness   0.5   0.5   0.5   0.2  0.1 //hard spring
Damping    0.06  0.06  0.06  0.02  0.0

The values reported above correspond to a pretty stiff spring. You can decrease the stiffness values if you want to obtain a more compliant behaviour (example follows).

robot icub
part left_arm

Stiffness  0.25  0.25  0.25   0.1  0.1 //soft spring
Damping    0.06  0.06  0.06  0.02  0.0

Regarding the damping, please play attention to the fact the high stiffness values with low (or zero) damping on the first shoulder joint may cause dangerous oscillations (and eventually break the tendons). So play with these values with particular care. The remaining small two joints are less critical.

  • If you want to interactively change stiffness/damping values, do not change the parameters of the demoForceControl configuration files, use the robotMotorGui instead.

Using the robotMotor Gui

The robotMotorGui module provides a graphical interface in order to control iCub joints. The status of every joint is displayed using color codes associated to the different control modes. You can change the current control mode of the joint by right-clicking on the corresponding joint (a pop-up window will appear). You can send position commands to joints by moving the position slider. If a joint is running the impedance control mode, you will able to control the position of the joint while maintaining a compliant behaviour. Furthermore, you can change the control parameters (joint stiffness etc) by clicking on the PID button.

robotMotorGui_torque1 robotMotorGui_impedance

Additional notes:

  • Please note that the parameter desired joint force shown in the "stiffness params" window is automatically controlled by the gravity compensator module ans displays 0 when the module is not running.

Using yarp interfaces to program your own application

Yarp provides a set of interfaces in order to have complete control on the behaviour of the robot joints. The typically used interfaces when running the force control on the robot are:

  • yarp::dev::IControlMode to set and retrieve the control mode of a specific joint.
  • yarp::dev::IPositionControl to send position commands to the robot joints.
  • yarp::dev::IVelocityControl to send velocity commands to the robot joints.
  • yarp::dev::ITorqueControl to obtain torque measurements and give explicit torque commands to a joint controlled in torque mode.
  • yarp::dev::IImpedanceControl to set the impedance parameters (stiffness/damping) of a joint controlled in impedance mode.

For more details, please refer to the following resources:

  • You can find a useful tutorial about making a simple application that uses the force control here
  • You can also find a complete reference to the available YARP interfaces here
  • You can also give a look at the source code of the force control demo

Configuration files

Mandatory parameters for force control

A complete reference on the robot configuration files can be found here. In this section only the parameters related to force control will be covered.

iCub\app\<your robot name>\conf directory contains the configuration files of the robot. The configuration of the robot is currently organized in two different levels.

  • icub.ini: the general configuration file, describing all the devices/parts available on the robot.
  • icub<robot_part>.ini: the configuration parameters for the specific part of the robot (arms/legs etc.)

In order to run force control, the following parameters are mandatory in the robot global configuration file icub.ini :

analog (rightarmanalog jointrightarmanalog leftarmanalog jointleftarmanalog righthandanalog lefthandanalog  leftleganalog jointleftleganalog rightleganalog jointrightleganalog)


network net_rarm
deviceId (right_arm)
period 10

network net_rarm
deviceId (joint_right_arm)
period 10
  • The analog(<list of analog devices>) parameter represents the list of all the analog sensors available on the robot. This list includes both the physical 6-axis F/T sensors (4 devices: rightarmanalog, leftarmanalog, rightleganalog, leftleganalog) both the emulated joint-level torque sensors (again 4 devices, the same names with the joint prefix). Two additional devices (righthandanalog lefthandanalog) are included on this list and correspond to the hall-effect position sensors of the fingers. These two entries are of course mandatory for the robot hands, but are unrelated to force control.
  • For each entry included in the list, a section [xxxanalog] is required. In this section it is specified the CAN bus network on which the sensor is located, the name of the device, and the period of the corresponding iCubInterface thread.

In order to run force control, the following sections are mandatory in the part-specific configuration files icub<robot_part>.ini :

  • The following section contains the torque pid gains for ALL the joints present in the specified robot part. The configuration parameters reported below are an example of tipical torque PID gains for an iCub arm. Note that the gains for ALL the joints are included in this section, including also the joints that cannot run torque control (in the example below: joint 5,6,7).
  //         Proportional  Derivative     Integral      IntegralLimit    PWMLimit    ScaleFactor>>   offset                                                                               
  TPid0          7             0             0             1333          1333          10            0                                                                                    
  TPid1          7             0             0             1333          1333          10            0                                                                                    
  TPid2          30            0             0             1333          1333          10            0                                                                                    
  TPid3          30            0             0             1333          1333          10            0                                                                                    
  TPid4          50            0             0             1333          1333          10            0                                                                                    
  TPid5          0             0             0             1333          1333          10            0                                                                                    
  TPid6          0             0             0             1333          1333          10            0                                                                                    
  TPid7          0             0             0             1333          1333          10            0
  • The following sections contain the configuration parameters for the force/torque sensors, both the real and the emulated ones. In the presented example right_arm is the real 6-axis force/torque sensors mounted on the robot right_arm. It is configured to transmit data with a Period of 2 ms. The output of the sensor will be avilable on /icub/right_arm/analog:o port. The second entry is named joint_right_arm. This is an emulated joint torque sensor, that opens an imput port /joint_vsens/right_arm:i. The module WholeBodyTorqueObserver will be responsible of: 1.reading data from the real sensor 2.computing robot dynamics. 3.Sending the output to joint_right_arm input port. In this way the joint torques are transmitted to the control boards through the CAN bus. If CanEcho flag is set to 1, you can also read back the transmitted CAN data on the port /icub/joint_right_arm/analog:o.
  [analog right_arm]                                                                                                                                                                                    
  CanAddress      0x0D                                                                                                                                                                     
  Format          16                                                                                                                                                                       
  Channels        6                                                                                                                                                                        
  Period          2                                                                                                                                                                        
  UseCalibration  1                                                                                                                                                                        
  [analog joint_right_arm]                                                                                                                                                                               
  CanAddress      0x0C                                                                                                                                                                    
  Format          16                                                                                                                                                                       
  Channels        6                                                                                                                                                                        
  Period          2                                                                                                                                                                        
  UseCalibration  1                                                                                                                                                                        
  CanEcho         1                                                                                                                                                                        
  PortName        /joint_vsens/right_arm:i
  • In order to associate the right channel of the virtual joint torque sensor to the corresponding motor channel, a map is required. You can find these information in [GENERAL] section. TorqueId the address used by the joint torque sensor to transmit data on the CAN bus, and TorqueChan is the corresponding channel. In order to transmit the torque data on the CAN bus, the floating point torque value is converted into a 16-bits value using a conversion factor, represented by the TorqueMax parameter.
  Joints        8             
  MaxDAC        100           100           100           100           100           100           100           100            
  AxisMap       0             1             2             3             4             5             6             7              
  Encoder       11.375        11.375        11.375        11.375        706.67        978.46        978.4         6.66       
  Zeros         178           33            262.7422      171           90            -20           -52           262.5          
  TorqueId      0x0C          0x0C          0x0C          0x0C          0x0C          0             0             0              
  TorqueChan    0             1             2             3             4             0             0             0              
  TorqueMax     8             8             8             8             2             2             2             2              

Basically the TorqueMax parameter represents the maximum torque value (in Newtons) that can be interpreted by the DSP using a 16 bits representation. For example if TorqueMax = 8 Newtons, the internal representation of the torque is:

Torque[N]     Torque[DSP representation 16-bits]
+8            32767
+4            16384
+2            8192
+1            4096
0             0
...           ...
-8            -32768

Meaning that the resolution of the sensor is 8/32767 = 0,000244 N. Of course if you decrease the value of the TorqueMax parameter the resolution of the sensor will increase but the maximum representable torque will decrease, and torque values above the specified parameter will be saturated before transmitting them on the bus. Please note this parameter only affects the representation of the torque insided the control board DSP but is not changing/adjusting the resolution of any physical sensor/ADC converter. One final remark: remember that that this value will also affect the representation of the maximum stiffness and damping of joint.

Back EMF compensation

This optional feature has been introduced in firmware build 55. In order to enable the back EMF compensation you have to:

  • enable the flag: ENABLE_icubmod_debugInterfaceClient in your cMake configuration.
  • add the following section in both your icub icub_right_arm.ini and icub_left_arm.ini configuration files:
Debug0        0 0 0 7 32000 0 0 3000
Debug1        0 0 0 7 27000 0 0 3000
Debug2        0 0 0 7 27000 0 0 3000
Debug3        0 0 0 7 20000 0 0 8000

This feature is still experimental. Please contact the rc-hackers mailing list for further informations.

Suggested PID gains

The following gains are currently used on iCub-Genova01:

Left arm:

//        Proportional  Derivative    Integral     Integral Limit    PWM Limit     scale factor >>    offset 
TPid0         -8            0             0             1333            1333           10              0              
TPid1         -8            0             0             1333            1333           10              0              
TPid2         -30           0             0             1333            1333           10              0              
TPid3         -30           0             0             1333            1333           10              0              
TPid4         -50           0             0             1333            1333           10              0              
TPid5         0             0             0             1333            1333           10              0              
TPid6         0             0             0             1333            1333           10              0              
TPid7         0             0             0             1333            1333           10              0              

Right arm:

//        Proportional  Derivative    Integral     Integral Limit    PWM Limit     scale factor >>    offset 
TPid0         8             0             0             1333            1333           10              0              
TPid1         8             0             0             1333            1333           10              0              
TPid2         30            0             0             1333            1333           10              0              
TPid3         30            0             0             1333            1333           10              0              
TPid4         50            0             0             1333            1333           10              0              
TPid5         0             0             0             1333            1333           10              0              
TPid6         0             0             0             1333            1333           10              0              
TPid7         0             0             0             1333            1333           10              0  

Left leg:

//        Proportional  Derivative    Integral     Integral Limit    PWM Limit     scale factor >>    offset 
TPid0         8             0             0             1333            1333           10              0              
TPid1         -8            0             0             1333            1333           10              0              
TPid2         8             0             0             1333            1333           10              0              
TPid3         -8            0             0             1333            1333           10              0              
TPid4         0             0             0             1333            1333           10              0              
TPid5         0             0             0             1333            1333           10              0              

Right leg:

//        Proportional  Derivative    Integral     Integral Limit    PWM Limit     scale factor >>    offset         
TPid0         -8            0             0             1333            1333           10              0              
TPid1         8             0             0             1333            1333           10              0              
TPid2         -8            0             0             1333            1333           10              0              
TPid3         8             0             0             1333            1333           10              0              
TPid4         0             0             0             1333            1333           10              0              
TPid5         0             0             0             1333            1333           10              0              


//        Proportional  Derivative    Integral     Integral Limit    PWM Limit     scale factor >>    offset         
TPid6         6             0             0             1333            1333           11              0              
TPid7         6             0             0             1333            1333           11              0              
TPid8         6             0             0             1333            1333           11              0      

If you are going to modify the PID gains of your robot, please pay attention to the signs and remember to change the gains little by little to avoid dangerous oscillations / unstable behaviours. Please note also that the torso PID gains use a different numbering respect to the other limbs (i.e: TPid6, TPid7, TPid8 instead of TPid0, TPid1 etc.)

Additional notes:

  • Derivative parameter: this parameter typically do not improves the quality of force control.
  • Integral parameter: this parameter can be used to improve the steady state error of control force loop. However, additional care should be used when modifying it because it works in a completely different range of values respect to the other parameters. Integral should be much smaller then the proportional one. In order to have a finer control of this parameter is suggested to increase the scale factor (and adjust the other parameters accordingly).
  • Scale Factor (aka shift factor): it represents a 2^Shift scaling factor for Kp, Kd, Ki. Increasing this parameter allows a finer tuning of the Kp,Kd,Ki gains.
For example, these are three different ways to set the same Kp gain with different scaling factors:
  • Kp= -8 and Shift=10 specifies a Kp=-8*2^(-10) = -8/1024=-0.0078.
  • Kp=-16 and Shift=11 specifies a Kp=-16*2^(-11) = -16/2048=-0.0078.
  • Kp= -4 and Shift=9 specifies a Kp=-4*2^(-9) = -4/512=-0.0078.
However Shift=11 allows a finer tuning of PID gains because, for example, it allows you to set Kp=-17 (-0.0083).

Please check also the configuration files section.

Force Control on torso joints

Since August 2011, force control is available also on torso joints. Requiriments:

  • Latest firmware (build 57 for BLL boards). In order to upgrade the firmware of your iCub you have to run on the PC104 the script $ICUB_ROOT/firmware/build/ followed by the .txt file describing the configuration of your robot (e.g: updateRobotv1.1.txt, updateRobotv1.2.txt etc). Additional info on the firmware upgrade procedure can be found here.
  • The icub.ini file has to be updated and it must include a jointtorsoanalog identifier in the analog device list + a corresponding [jointtorsoanalog] section. Here is an example (source: $ICUB_ROOT\main\app\robots\iCubGenova01\conf\icub.ini)
analog (... jointtorsoanalog ...)

network net_headtorso
deviceId (joint_torso)
period 10
  • The icub_head_torso.ini file must contain the analog device descriptor section:
[analog joint_torso]         
CanAddress                  0x0C                   
Format                      16             
Channels                    6              
Period                      2              
UseCalibration              1              
CanEcho                     1              
PortName                    /joint_vsens/torso:i   

and also the channel mapping for torso joints in the [GENERAL SECTION]



TorqueId 0 0 0 0 0 0 0x0C 0x0C 0x0C 0 TorqueChan 0 0 0 0 0 0 0 1 2 0 TorqueMax 0 0 0 0 0 0 8 8 8 0


  • The icub_head_torso.ini file must contain a suitable [TORQUE_PIDS] section, as indicated in the suggested PID gains section.

Known issues

Controlling iCub joints using force control is probably one of the most intriguing features of our robotic platform. However, this control modality has been only recently added, and some parts of the control software are still currently under development. This means that force control has still to be consider an *experimental* feature, and you are encouraged to report your problems/eventual erratic behaviour observed using your robot.

Currently (03/03/2011, BLL firmware build 55, 4DC firmware build 41) the following issues are known:

  • Torque control PIDs.

By default the torque control gains specified in the .ini configuration files use only a proportional gain (+ filter to improve control stability). This means the torque controller has a steady state error (that you can monitor for example using the modules:controlBoardDumper and portScope). In order to increase the tracking accuracy of the commanded torques you may add an integral gain. However you are warned that playing with PID gains (without knowing what you are going to do) may lead to control instability (and likely breaking of joint tendons). We do not suggest to change your torque PID gains without first asking suggestion about it.

  • Changing stiffness on-line, during a joint motion

Changing the joint impedance from a low stiffness to high stiffness value with a step function is typically dangerous. If the commanded position differs from the current joint position, increasing the joint stiffness to a high value is equivalent to load a spring and suddenly release it. For this reason it'is important to always keep the position error small when increasing the stiffness (or gradually increase it, avoiding step functions). In order to keep small enough the position error (typically caused by the gravity force acting on the joint) and avoid this kind of problems use the gravityCompensator module. This is a one of the most common reasons of tendon breaking, so keep particular attention when increasing the stiffness of a previously low-stiffness joint. Please also read carefully the example reported in the additional notes of the gravityCompensator section.


  • Which joints can be currently controlled using torque/impedance control mode?
Currently torque/impedance control is supported only on the following joints:
  • ARMS:
    • The complete shoulder (3DOF, joints: 0 1 2)
    • The elbow (joint: 3)
    • Wrist pronosupination (joint: 4)
  • LEGS:
    • The complete hip (3DOF, joints 0 1 2)
    • The knee (joint: 3)
  • TORSO:
    • The complete shoulder (3DOF, joints: 0 1 2), firmware update required, additional notes here
  • What are the measurement units employed by the YARP torque / impedance interfaces?
    • yarp::dev::ITorqueControl::getTorque() returns the current joint torque in [Nm]
    • yarp::dev::IImpedance Control::getImpedance() returns the current joint stiffness in [Nm/deg] and the currnt joint damping in [Nm/(deg/s)]
  • What is the maximum joint torque that can be commanded?
Depends on the joint. For example, typical maximum torque for leg joint is 12Nm, while for the arm is 8Nm.
You can modify this values in the .ini configuration files of your robot. Remember that since the torque is currently internally represented by a 16 bit value, if you increase the maximum allowed torque value, the resolution of the measurement will be reduced (For further information please refer to the configuration files section.)
You can also use the yarp method yarp::dev::ITorqueControl::getTorqueRange() in order to retrieve the max/min torque allowed for the specified joint (but you cannot currently modify these values).
  • What is the control frequency?
Torque PID control is performed by the DSP of the controller board, so the control frequency is fixed to 1KHz. However, other parameters affect the effective control speed. For example, torque measurements are obtained from the 6-axis force sensors every <x> millisecond, where <x> is the value of parameter 'period' specified in the section [analog <sensor_name>] of your .ini configuration files. Typically this parameter is set to 1 or 2 milliseconds depending on your hardware capabilities (iCub version). Furthermore, the performance of the wholeBodyTorqueObserver module, which is responsible of the dynamic computation, depends on the computational power of the machine on which it is executed (high performance machine suggested). The main thread of wholeBodyTorqueObserver tipically runs @ ten milliseconds.
  • What is the suggested stiffness value in order to start playing with impedance control?
A very soft spring behaviour can be obtained setting stiffness to 0.2[Nm/deg] and damping to 0.02[Nm/(deg/s)]. You can then gradually increase these values in order to simulate an harder spring. Always remember to set a damping in order to avoid joint oscillations (no damping = ideal spring that will oscillate forever!). Important: the higher is the stiffness, the higher must be the damping parameter.
  • What is the default stiffness value when I start the impedance control?
If you just start the iCubInterface + wholeBodyTorqueObserver and then you set a joint in impedance control mode, the stiffness/damping parameters will be zero by default and the joint will be free to move. This is made intentionally because it's not possible to know the optimal joint stiffness for your application. Moreover, if you start the robot with a high stiffness value and one joint is already pushing against an obstacle you may damage the joint tendons. Setting a default stiffness to zero is the safest mode to protect your robot.
If you run the demoForcecontrol, instead, the default stiffness/damping is taken is 0.5[Nm/deg] and 0.06[Nm/(deg/s)] respectively (these values are saved in your iCub\app\demoForceControl\conf\*.ini configuration files).
  • Should I care about the control mode when I close my application? And during the calibration of the robot, what happens if one joint is left in impedance control?
Yes, you should. Next iCub user may not know if you left one arm in position or impedance control mode! It's thus a good practice to close your application returning the control modes of the joint you used to their previous configuration. With great power comes great responsibility.
Regarding the second question, all the calibrators are allowed to change the control mode to position control (obviously!)
  • What is the difference between a impedance control with zero stiffness and torque control?
If the stiffness and damping parameters are both set to zero when using impedance control, you will obtain the same behaviour of running torque control and commanding a zero reference torque.
  • Is there any quick tool in order to change online the desired position of the joint/stiffness/control mode etc?
The robotMotorGui module provides a graphical interface that shows which boards are controlled in position/torque/impedance mode etc. You can change most of the control parameters using this Gui.
  • I want to dump / plot the torque data of the joints. Is there any tool available?
You can use the controlBoardDumper module and the portScope tool (located in iCub\contrib\src\modules\portScope).
  • My joint goes in idle mode when I change from position control to impedance control, why?
The control board (DSP) is not receiving the joint torque measurement (a watchdog is implemented in the firmware in order to disable the control if no torque measurements are received in 200ms). This probably means that:
  • WholeBodyTorqueObserver module is not running.
  • WholeBodyTorqueObserver ports are not connected with iCubInterface.
  • WholeBodyTorqueObserver module is running on a slow machine and the computation time exceeded 200ms.
  • During normal operation I can see a lot of messages similar to: **** PORT: /icub/joint_right_arm/analog:o **** TIMEOUT : 0.013 COUNT: 121. What does they mean?
iCubInterface integrates a check to verify that a fresh torque measurement is received from the estimator module (wholeBodyTorqueObserver) at least every 10 milliseconds. However, if the wholeBodyTorqueObserver module is running on a low-performance machine or if the connection between the wholeBodyTorqueObserver and iCubInterface experiences networking issues (bandwidth congestion, packet drops etc.), this throughput may not be achieved. If a measurement is not received every 12 milliseconds, the reported timeout message will be displayed. If a lot of timeout messages are displayed, please consider to solve this problem before continuing, because these delays can seriously worse the quality of the control. In most of the cases is sufficient to verify that the wholebodyTorqueObserver module is running in release mode (and not in debug) and make it run on a machine of adequate power (typically a desktop machine, not a centrino laptop). Additionally, remember that if these delays reach the threshold of 200 milliseconds, the internal watchdog of the control boards will detect a fault event and the torque control will be disabled (the motor will go in idle mode).
  • When a joint is in impedance position control mode, I can observe a small position error respect to the commanded position. Why?
This is a normal behaviour because gravity is acting on the joint and, unless you compensate for it, an accurate position tracking is not possible. In particular, if the joint stiffness is small (soft spring) the position error will be noticeable. On the contrary, the position error will be small when you use high stiffness values.
  • Can I change the stiffness of a joint in realtime during the execution of a commanded trajectory?
Yes, you can. Please consider the issues reported in the issues section.
  • I noticed that the power cables on the back of the elbow mechanically interfere with torque measurements of the arm joints.
Yes, if the power cables push on the elbow joint, the F/T sensor will measure the resulting forces and the control will react to them. Try to fix the cables in a way that they do not interact with the joint.
  • I set different control modes on different joints (i.e. impedance mode on joint x, position mode on joint y) but now I forgot everything. How can I retrieve the status of the joints?
  • From your software application: you can use yarp::dev::IControlMode::getControlMode() method.
  • In general: You can use the robotMotorGui application to monitor the status of your control boards.
  • Is there any tutorial application about how to write a module that uses force control?
  • Is gravity compensation available? And cartesian impedance control?
The module gravityCompensator provides gravity compensation (based on the CAD model of iCub). Cartesian impedance control is still in research phase.
  • I'm controlling my robot in impedance/torque control mode. I'm pushing on the shoulder and I noticed that... WTF! the forearm moves?!? Why?
The control algorithm implemented in your iCub v1.x assumes that you are applying external forces only on the end effector (wrists, ankles). Of course this because your robot does not have real joint torque sensors, and some assumptions are needed to perform the dynamic calculation (refer also to the iDyn page). Because of this, if you exert an external forces not on the end effector of your robot, some miscalculation of the joint torques will occur.
Of course you can change the external forces application point, but it will be always a fixed point.
  • Can I change the PID gains of torque control?
You are not recommended to do it, because wrong gains may cause control instability and eventually break joint tendons. Integral control is experimental and not currently recommended. Please ask before playing with PID gains and avoid random numbers.
  • Where are located the .ini configuration files?
In iCub\app\<your robot name>\conf directory. Read section
  • I found a bug! And now??
Please report bugs on rc-hackers mailing list.


EmbedVideo is missing a required parameter.

More coming soon...

Related modules


The iCub force control people: Matteo Fumagalli, Serena Ivaldi, Marco Randazzo, Francesco Nori