WholeBodyDynamics Questions

[diogofbsilva]

Hi everyone!

I am currently working on my thesis where I have the goal to control the hands of the iCub in different physical tasks. For that, I am aiming to use the F/T sensors. To know when to actuate I am converting the force to the base reference frame. The goal in this is that the force on the base reference frame does not change when the joint angles of the arm suffer variation. However, when looking at some modules present on the robotology pipeline I stumbled upon the wholeBodyDynamics module where it seems that the internal wrench of the sensor is removed from the sensor measurements, being here my first question, the internal wrench takes into account the weight of the sensor and the arm or just the sensor?

My second question is about the calibrateOffset function. What is the offset that is obtained from that function and how is obtained? Did not quite understand the way is obtained.

I would really appreciate your help, especially from @randaz81 and Matteo Fumagalli.

Thank you for your attention!
Diogo Silva

[traversaro]

Hello @diogofbsilva, probably it may be useful that when you refer to "internal wrench of the sensor" and "removed from the sensor measurements", you refer to the actual lines of code or formula in the paper you refer to, to avoid the ambiguity intrinsic in the natural language. : )

Furthermore, just to let you know you seem to refer to the wholeBodyDynamics module from icub-main, in most modern deployments on the robot we are instead using the wholebodydynamics YARP device available in https://github.com/robotology/whole-body-estimators/tree/master/devices/wholeBodyDynamics, that is using logic from iDynTree's ExtWrenchesAndJointTorquesEstimator class.

[diogofbsilva]

@traversaro thank you for your help! I will investigate that YARP device.
Regarding the removal of the internal wrench of the sensor from the measurements I am referring to the lines 841 and 842 of the observerThread.cpp file of wholeBodyDynamics.

[traversaro]

Please add a link to the corresponding github page, for example:
https://github.com/robotology/icub-main/blob/9f14d25946fdf22e8efc342f23ff781af6def5b0/src/modules/wholeBodyDynamics/observerThread.cpp#L841-L842

In this way it is also clear to which version of the code you are referring to. Ok, by looking the code it is much more clear, and I can answer:

the internal wrench takes into account the weight of the sensor and the arm or just the sensor?

Both arm and (small part) of the sensor.

[diogofbsilva]

Thank you! Regarding my second question about the calibrateOffset function in line 1157, can you maybe explain me a little bit better what computation are being made there?
https://github.com/robotology/icub-main/blob/9f14d25946fdf22e8efc342f23ff781af6def5b0/src/modules/wholeBodyDynamics/observerThread.cpp#L841-L842

Thank you!

[traversaro]

Basically that computes the expected wrench that should be measured by the FT assuming that the only force acting on the robot is applied to the root link (as it happens for example when the robot is fixed to the pole). Then, if you have the raw measurement of the sensor and assuming that the raw measurement of the sensor is just the actual measurment plus a constant offset, you can compute this offset by substracting from the FT sensor measure the expected FT sensor measure.

For some more mathematical details, see:

• https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf Page 87, Section 4.7

Another useful reference can be the equivalent code in the device and in iDynTree, i.e. :

• https://github.com/robotology/whole-body-estimators/blob/v0.10.0/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L2587
• https://github.com/robotology/idyntree/blob/v12.1.0/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp

[diogofbsilva]

Thank you so much for your help!
I understand quite better the code and I will look into the references related to the equivalent code in the device!!