Plugins and WholeBodyDynamics in Simulation

[diogofbsilva]

Hello everyone!

I am working on my thesis in ISR Lisbon with the iCub where I have the goal to control the dexterous robot hands using tactile and force sensors. In order to have good testing I need to simulate grasping activities. The work done here https://github.com/robotology/icub-gazebo-grasping-sandbox/tree/master creates a grasping activity. which is ideal to my work, being that the reason that I am using it. As I mentioned earlier, in this thesis I am using tactile and FT sensors so the use of plugins is important. However, as of right now, in my simulations, I do not have ports that are active giving FT readings.

So, here is my first question: what might be the problem I am facing, provided that I have in the robotology pipeline the force torque plugins.

The second problem is regarding the use of the wholeBodyDynamics device in simulation as well. Do you have any pointers that may help me using this device in the simulation? In this device, calibration of the FT sensors is made which is very helpful to my thesis to not take into account the weight of the arm in the sensor measurements.

Thank you
Diogo Silva

[pattacini]

Hi @diogofbsilva

The sandbox makes use of the model iCubGazeboV2_5_visuomanip, which has been specifically designed to focus on the hands but, on the other side, it does not expose FT sensors.

Therefore, you would need to extend the model taking inspiration from a more "standard" one.

Here's a list of useful pointers:

• https://github.com/robotology/icub-models/blob/836440918c72a0484eff069ef03d61dca3a3eeb6/iCub/robots/iCubGazeboV2_5/model.urdf#L2650-L2689
• https://github.com/robotology/icub-models/blob/836440918c72a0484eff069ef03d61dca3a3eeb6/iCub/conf/icub.xml#L22-L31

For the wholebodydynamics, our warm recommendation is to rely on the idyntree-based service.
Take a look at how it's called for a physical robot:

• https://github.com/robotology/robots-configuration/blob/c6a298257827bb08de3c6b734ea7144399531df8/ergoCubSN001/ergocub_wbd.xml#L141

CC'ing @robotology/iit-artificial-mechanical-intelligence to get better pointers for how to configure the WBD service on Gazebo.

[diogofbsilva]

Hello!
Thank you for the very useful pointers, especially the ones regarding the FT sensors as I was able to have them working on my simulation. Thank you also for the pointers regarding the WBD and for CC'ing more people to help me! I really apprecciate it!

[traversaro]

The second problem is regarding the use of the wholeBodyDynamics device in simulation as well. Do you have any pointers that may help me using this device in the simulation? In this device, calibration of the FT sensors is made which is very helpful to my thesis to not take into account the weight of the arm in the sensor measurements.

So, in simulation, the FT sensors already provide you the correct measurement without any offset, so there is no need to calibrate an additional offset. To disable the calibration of offset, you can pass the startWithZeroFTSensorOffsets option, see the wholebodydynamics device documentation (https://github.com/robotology/whole-body-estimators/tree/v0.10.0/devices/wholeBodyDynamics) or robotology/whole-body-estimators#72 and robotology/whole-body-estimators#67 for more details.

Then, in simulation you can either run the wbd externally, or run it as part of the robotinterface of the simulated robot itself (see the wbd-ext and wbd-int use cases described in icub-tech-iit/ergocub-software#35). At the moment in ergocub-software we are using the wbd-ext workflow, see https://github.com/icub-tech-iit/ergocub-software/blob/v0.7.3/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml and https://github.com/icub-tech-iit/ergocub-software/blob/v0.7.3/urdf/ergoCub/conf/estimators/wbd_ecub_sim.xml for the related configuration files. Similar configuration files for simulated iCub are https://github.com/robotology/whole-body-estimators/blob/master/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-six-fts-sim.xml and https://github.com/robotology/whole-body-estimators/blob/master/devices/wholeBodyDynamics/app/launch-wholebodydynamics-icub-six-fts-sim.xml .

[diogofbsilva]

Hello @traversaro! Thank you so much for your help!

Yes, in simulation the calibration of an offset is not needed, however I have another doubt for you.
Is in simulation the computations done in https://github.com/robotology/icub-main/blob/9f14d25946fdf22e8efc342f23ff781af6def5b0/src/modules/wholeBodyDynamics/observerThread.cpp#L841-L842 still useful?
Sorry for sending a link of the WBD in the iCub-main however I navigate better in the module than in the device for now.

As for how to run the wbd in simulation I will look into what sent and if I get into any trouble I will get in contact.

Thank you so much for your help!!
Happy Sunday!

[diogofbsilva]

Hello @traversaro @pattacini !

For the past few weeks I have been working with the FT plugins as they are working well in my simulation, however the information that is online is confusing me a little bit as the axes of the FT sensors of the image below are not matching with the results I am obtaining:

I am saying this because when analyzing the force values for different positions of the left arm of the iCub the values did not make sense to me taking into account the axes of the image. Take for example, when the arm of the iCub is up: It would be expected that for this position |Fx| would be the biggest, however that does not happen (|Fz| is the biggest value):

When analyzing the force values when the arm is down it seems that the same thing happens (the axis x seems switched with the axis z), as the force |Fz| should be the bigger value:

Are the axis represented in the first image not correct? What seems to be my problem?

Thank you for your help!!

Diogo

[Nicogene]

Hi @diogofbsilva,

We are aware that the FT sensors in iCub URDF have problems:

• iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong – Stint 1 icub-models-generator#233
• iCubGazeboV2_* and iCubGenova11 : the frames of the ft sensors in the arms seems wrong – Stint 2 icub-models-generator#271

this could explain the inconsistency you are seeing, we will let you know once we find a solution to this problem

dd @traversaro @pattacini @martinaxgloria

[traversaro]

@diogofbsilva which models are you using in your simulation?

[diogofbsilva]

Hi @traversaro !

I am using the model iCubGazeboV2_5_visuomanip, with slight modifications in the URDF such as the addition of the FT sensors located in the arms, which I got from the URDF model of iCubGazeboV2_5.

[diogofbsilva]

@Nicogene thank you for your help!
You have come up with a fix that can be quite helpful for me. robotology/icub-models-generator#233 (comment)
I will look into it!

Thanks again!

[traversaro]

Ok, thanks for the details! If you need further help, probably it is useful if you push somewhere the model you are using if it is not one of the one availably by default in icub-models .