D4C Framework

From Wiki for iCub and Friends
Revision as of 15:32, 9 October 2012 by VVV12 (talk | contribs) (The Code)
Jump to: navigation, search

Theoretical Background

The Pmp library is based on the idea of employing virtual force field in order to perform a reaching task. It takes inspiration from theories conceived by Oussama Khatib durig 80's (Oussama Khatib, Real-time obstacle avoidance for manipulators and mobile robots, Int. J. Rob. Res., Vol. 5, No. 1. (1986), pp. 90-98) and revised by Vishwanathan Mohan (Mohan. V., Morasso P.,Metta G., Sandini G. A biomimetic, force-field based computational model for motion planning and bimanual coordination in humanoid robots, Autonomous Robots, Volume 27, Issue 3, (2009) pp. 291-301). We have developed a different version of pmp which, instead of using the transposed Jacobian, employs the Cartesian Interface; indeed, it automatically takes into account iCub's joints limits and it makes the generated trajectory smooth. The pmp library allows to describe objects as obstacles or as targets, and to consequently build a virtual force field to reach the target avoiding obstacles. For the target a mass-spring-damper force field is generated


Each obstacle is modeled as a Gaussian repulsive force field. It is possible to choose between a canonical multivariate Gaussian force field


and a Gaussian force field without tails


Since the tuning of the parameters is a critical point, we’re going to implement some further methods in order to avoid local minima.


Many tests with the following Simulink model have been performed


It allows you to modify parameters and to compute easily the virtual trajectory generated from the combination of the virtual force fields associated to target and obstacles. It is also possible to add a Gaussian attractor on the target which will be summed to the mass-spring-damper force field.

Obstacles modeled as Gaussians with tails in the first image, and without tails in the second

As it is possible to notice from the figure, local minima can be reached. These situations can be avoided with the method explained in

  • Randazzo M., Sgorbissa A. and Zaccaria R. µNav: Navigation without localization, .
In the first case the trajectory stops in a local minima, in the second case the trajectory bypass the obstacles correctly

It is possible to download the two MATLAB models (the one without methods for local minima, and the other one with methods for local minima) from here.

The Cartesian I/F Implementation

The pmp library exploits the Cartesian Interface, and it is built as follow


There is a server, with the aim to instantiate, modify or delete objects, generate force fields and compute the trajectory, which is transmitted to the Cartesian Interface. Then there is a client which is automatically connected to the server and that can send a request in order to have information on objects, or can ask to add or delete objects.

Notably, the pmp server is capable of sending proper information to the iCubGui in order to display a pictorial representation of the set of targets, obstacles and generated trajectories as depicted below.


The Code

In order to use the Pmp Library it is required to add the Cartesian Interface to the CMakeLists.txt file. It is possible to do it by following the Cartesian Interface Tutorial. It is also required to include in the CMakeLists.txt the following line:

target_link_libraries(${PROJECTNAME} ${YARP_LIBRARIES} pmp)

In order to open a pmp_server:

#include <iCub/pmp/pmp_server.h>
PmpServer server;
Property options;
options.put("verbosity",0); //print log messages
options.put("period",20); //period of the RateThread

In order to open a pmp_client:

#include <iCub/pmp/pmp_client.h>
PmpClient client;
Property options;

In order to add an obstacle:

Value centerOb; centerOb.fromString(("(0.2 0.2 0.2)");
Value radiusOb; radiusOb.fromString("(0.05 0.1 0.05)");
Property obstacleOpt;
int obstacle; client.addItem(obstacleOpt,obstacle);

In order to add a target:

Value centerTg; centerTg.fromString(("(0.4 0.4 0.4)");
Value radiusTg; radiusTg.fromString("(0.05 0.05 0.05)");
Property targetOpt;
int target; client.addItem(targetOpt,target);

In order to make the trajectory start from a specific point with a given initial velocity it is possible to use the method


where 'x' represents the position and 'o' the orientation, so as 'xdot' and 'odot' the corresponding velocities. The user might want to enable the force field generation without enabling robot movements:


In order to enable also robot movements:


For online documentation of all the methods composing the Pmp Library, please refer to the Doxygen Documentation


Under $ICUB_ROOT/main/src/modules/pmpServer and $ICUB_ROOT/main/src/examples/pmpExample user may find further info on how to open a pmp_server and a pmp_client, and how to write the related CMakeLists.txt


Gori I., Pattacini U., Nori F., Metta G. & Sandini G., DForC: a Real-Time Method for Reaching, Tracking and Obstacle Avoidance in Humanoid Robots, IEEE-RAS International Conference on Humanoid Robots, Osaka, Japan, November 29 - December 1, 2012.