Add support for reading contacts from the Skin in the wholebodydynamics YARP device

[traversaro]

While replying to https://github.com/robotology/codyco-modules/issues/226 , I noticed that the wholebodydynamics YARP device is lacking support for reading the contact points from the skin [1].

Such a support needs to be implemented before we can deprecated wholeBodyDynamicsTree, see https://github.com/robotology/codyco-modules/issues/223 .

[1] : https://github.com/robotology/codyco-modules/blob/master/src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L1327

[traversaro]

cc @iron76 @DanielePucci @gabrielenava

[traversaro]

cc @fjandrad

Note that the conversion logic is already partially implemented in https://github.com/robotology/idyntree/blob/master/src/icub/include/iDynTree/iCub/skinDynLibConversions.h

The conversion logic is actually part of a test: https://github.com/robotology/idyntree/blob/2470a71cee49ee3a611195e36b8dc447eca9f154/src/tests/icub_consistency/iCubExternalWrenchesEstimationConsistencyTest.cpp#L418 but it was never used in the actual device. :D

[traversaro]

To have an idea of the logic currently implemented, you can check the relevant code in the very old module wholeBodyDynamics : https://github.com/robotology/icub-main/blob/cd2028daac5afcddb220f3b6a7bfe6caa4abdf7b/src/modules/wholeBodyDynamics/observerThread.cpp#L1552

and in the not so old module wholeBodyDynamicsTree : https://github.com/robotology/codyco-modules/blob/8d45db4b8613ea8de85c7e58502d8e84c05162c8/src/modules/wholeBodyDynamicsTree/src/wholeBodyDynamicsStatesInterfaces.cpp#L330

[traversaro]

Once the support has been implemented, in can be tested on the real robot, by launching the skinManager application, and ensuring that the following connections are active:

<connection>
    <from>/skinManager/skin_events:o</from>
    <to>/wholeBodyDynamics/skin_contacts:i</to>
    <protocol>udp</protocol>
</connection>

for the skinManager ---> wholeBodyDynamics connection, and

  <connection>
    <from>/wholeBodyDynamics/contacts:o</from>
    <to>/iCubGui/forces</to>
    <protocol>udp</protocol>
</connection>

for the wholeBodyDynamics ---> iCubGui

[fjandrad]

it seems to me that some functions from https://github.com/robotology/idyntree/blob/master/src/icub/src/skinDynLibConversions.cpp are replicated in https://github.com/robotology/idyntree/blob/master/src/icub-kdl/include/iCub/iDynTree/TorqueEstimationTree.h. Is this on purpose? Is there a relevant difference that I'm missing?

[diegoferigo]

Let's wait @traversaro reply, but I think I understood that slowly we're going to deprecate kdl stuff.

[traversaro]

Yes, the stuff in icub-kdl folder will be eventually removed, that's there is "duplicated" code.

[fjandrad]

this issue should be almost solved in the branch skinForceSensing, I just need to test on the robot.

[fjandrad]

Considering that now is possible to have more than one contact per submodel I remember discussing that when there are more than one contact in a submodel we can not estimate the external wrench, following the code I did not find a condition when number_contacts >number_of_submodels . So where and how should we deal with this situation? It would be my understanding that the estimator should deal with this.

maybe averaging the contact position and estimate the external wrench there?

[traversaro]

Fixed by @fjandrad in https://github.com/robotology/codyco-modules/pull/239 .