Add support for reading contacts from the Skin in the wholebodydynamics YARP device about codyco-modules HOT 10 CLOSED

cc @iron76 @DanielePucci @gabrielenava

from codyco-modules.

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

from codyco-modules.

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

from codyco-modules.

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

from codyco-modules.

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?

from codyco-modules.

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

from codyco-modules.

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

from codyco-modules.

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

from codyco-modules.

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?

from codyco-modules.

Fixed by @fjandrad in #239 .

from codyco-modules.