Repository files navigation

TheDynamic Force Field Controller framework - DForC or D4C in short - isbased on the idea of employing virtual force field in order to perform areaching task. It takes inspiration from theories conceived by O. Khatib durig80's (Oussama Khatib,Real-time obstacle avoidance for manipulators and mobilerobots, Int. J. Rob. Res., Vol. 5, No. 1. (1986), pp. 90-98) and revised byP. Morasso and V. Mohan within the Passive Motion Paradigm (PMP) (Mohan. V.,Morasso P.,Metta G., Sandini G.A biomimetic, force-field based computationalmodel for motion planning and bimanual coordination in humanoid robots,Autonomous Robots, Volume 27, Issue 3, (2009) pp. 291-301). The novelty of theD4C approach chiefly consists in replacing the use of the transposed Jacobian(as proposed in PMP) with theCartesian Interfacetool. Thereby, D4C automatically takes into account the robot's joints limitsand it makes the generated trajectory smooth. The D4C library allows describingobjects as obstacles or as targets, and to consequently build a virtual forcefield 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 tochoose 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 implementsome 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 trajectorygenerated from the combination of the virtual force fields associated to targetand obstacles. It is also possible to add a Gaussian attractor on the targetwhich will be summed to the mass-spring-damper force field.Obstacles modeled as Gaussians with tails in the first image, and without tailsin 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 casethe trajectory bypass the obstacles correctly.

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

The D4C library exploits theCartesian Interface,and it is built as follows:

There is a server, with the aim to instantiate, modify or delete objects, generateforce 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 thatcan send a request in order to have information on objects, or can ask to add ordelete objects.

Notably, the D4C server is capable of sending proper informationto theiCubGuiin order to display a pictorial representation of the set of targets, obstaclesand generated trajectories as depicted below.

• YARP
• iCub
• icub-contrib-common

For online documentation of all the methods please refer to theDoxygen Documentation.

In order to use the D4C Library it is required to add the Cartesian Interfaceto theCMakeLists.txt file. It is possible to do it by following the CartesianInterfacetutorial.It is also required to include in theCMakeLists.txt the following line:

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

In order to open ad4c_server:

...#include<iCub/d4c/d4c_server.h>...D4CServer server;Property options;options.put("verbosity",0);//print log messagesoptions.put("period",20);//period of the RateThreadoptions.put("device","cartesiancontrollerclient");options.put("name","d4c_server");options.put("robot","icub");options.put("part","right_arm");server.open(options);

In order to open ad4c_client:

...#include<iCub/d4c/d4c_client.h>...D4CClient client;Property options;options.put("verbosity",0);options.put("remote","/d4c_server");options.put("local","/d4c_client");client.open(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;obstacleOpt.put("type","obstacle_gaussian");obstacleOpt.put("active","on");obstacleOpt.put("G",5.0);obstacleOpt.put("name","obstacle");obstacleOpt.put("center",centerOb);obstacleOpt.put("radius",radiusOb);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;targetOpt.put("type","target_msd");targetOpt.put("active","on");targetOpt.put("K",1.5);targetOpt.put("D",3.0);targetOpt.put("name","target");targetOpt.put("center",centerTg);targetOpt.put("radius",radiusTg);int target; client.addItem(targetOpt,target);

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

client.setPointState(x,o,xdot,odot);

wherex represents the position ando the orientation, so asxdot andodot the corresponding velocities.

The user might want to enable the force field generation without enabling robot movements:

client.enableField();

In order to enable also robot movements:

client.enableControl();

Inmodules/d4cServerandmodules/d4cExamplethe user will find further info on how to open ad4c_server component and ad4c_client component, respectively, and how to write the correspondingCMakeLists.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.

• EFAA 1st Year Review Demo: thelibrary at work within the EFAA EU project.
• Humanoids2012 Video: the officialDForC video recorded for Humanoids2012 conference.

Material included here is Copyright ofiCub Facility - Istituto Italiano di Tecnologiaand is released under the terms of the GPL v2.0 or later. See the file LICENSE for details.