search

robotology / whole-body-estimators

YARP devices that implement estimators for humanoid robots.
27 stars 12 forks source link

Allow opening and using the contacts:o port without skinDynLib #110

Open S-Dafarra opened 3 years ago

S-Dafarra commented 3 years ago

At the moment, the contacts:o port is opened only if useSkinForContacts is set to true. See https://github.com/robotology/whole-body-estimators/blob/353985f3997e89dbc6744105077765457e2e0227/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L1469-L1482 and https://github.com/robotology/whole-body-estimators/blob/353985f3997e89dbc6744105077765457e2e0227/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L569-L575

I tried to set useSkinForContacts to true without setting the IDYNTREE_SKINDYNLIB_LINKS, but then I had a lot of these errors related to https://github.com/robotology/whole-body-estimators/blob/353985f3997e89dbc6744105077765457e2e0227/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L2534 coming from https://github.com/robotology/idyntree/blob/db312bfe9f490f991250603f05cea84a8c156899/src/icub/include/iDynTree/skinDynLibConversionsImplementation.h#L338.

Even if the error seems harmless, this is not sustainable since the wholeBodyDynamics device runs as a part of the robot main yarprobotinterface, thus filling up the logger in a very short time.

I have tried understanding how to set the IDYNTREE_SKINDYNLIB_LINKS group, but I could not figure out a correct configuration. In addition, it requires to specify a set of numbers that I have no clue how to get right, especially for a robot like iCub3 that is covered only in a small portion by the skin.

Any hint @traversaro @prashanthr05?

prashanthr05 commented 3 years ago

I have never used the skin with WBD so far. But maybe looking into skinDynLib and related configuration files could possibly help?

S-Dafarra commented 3 years ago

I have never used the skin with WBD so far. But maybe looking into skinDynLib and related configuration files could possibly help?

Would you have any input on how to get the numbers to put in there, like https://github.com/robotology/robots-configuration/blob/f3fef4277717ae87d825a9a46d994ccd21fef3ec/iCubGenova04/estimators/wholebodydynamics.xml#L24-L40?

S-Dafarra commented 3 years ago

After a F2F discussion with @traversaro, a possibility would be to open yet another port publishing the content of estimateExternalContactWrenches.

prashanthr05 commented 3 years ago

I have never used the skin with WBD so far. But maybe looking into skinDynLib and related configuration files could possibly help?

Would you have any input on how to get the numbers to put in there, like https://github.com/robotology/robots-configuration/blob/f3fef4277717ae87d825a9a46d994ccd21fef3ec/iCubGenova04/estimators/wholebodydynamics.xml#L24-L40?

On some reading, I think this file might have some relevant information, https://github.com/robotology/icub-main/blob/master/src/libraries/skinDynLib/include/iCub/skinDynLib/common.h#L43 related to the body part enums.

S-Dafarra commented 3 years ago

I have never used the skin with WBD so far. But maybe looking into skinDynLib and related configuration files could possibly help?

Would you have any input on how to get the numbers to put in there, like https://github.com/robotology/robots-configuration/blob/f3fef4277717ae87d825a9a46d994ccd21fef3ec/iCubGenova04/estimators/wholebodydynamics.xml#L24-L40?

On some reading, I think this file might have some relevant information, https://github.com/robotology/icub-main/blob/master/src/libraries/skinDynLib/include/iCub/skinDynLib/common.h#L43 related to the body part enums.

Ah nice finding!

S-Dafarra commented 3 years ago

I have never used the skin with WBD so far. But maybe looking into skinDynLib and related configuration files could possibly help?

Would you have any input on how to get the numbers to put in there, like https://github.com/robotology/robots-configuration/blob/f3fef4277717ae87d825a9a46d994ccd21fef3ec/iCubGenova04/estimators/wholebodydynamics.xml#L24-L40?

On some reading, I think this file might have some relevant information, https://github.com/robotology/icub-main/blob/master/src/libraries/skinDynLib/include/iCub/skinDynLib/common.h#L43 related to the body part enums.

Ah nice finding!

While the first number is more or less clear, I am not sure what the second number is :thinking:

traversaro commented 3 years ago

I have never used the skin with WBD so far. But maybe looking into skinDynLib and related configuration files could possibly help?

Would you have any input on how to get the numbers to put in there, like https://github.com/robotology/robots-configuration/blob/f3fef4277717ae87d825a9a46d994ccd21fef3ec/iCubGenova04/estimators/wholebodydynamics.xml#L24-L40?

On some reading, I think this file might have some relevant information, https://github.com/robotology/icub-main/blob/master/src/libraries/skinDynLib/include/iCub/skinDynLib/common.h#L43 related to the body part enums.

Ah nice finding!

While the first number is more or less clear, I am not sure what the second number is 🤔

It used to be a reference to the link index in the corresponding iKin Chain, so for LEFT_ARM https://github.com/robotology/icub-main/blob/4215771441724b7ece6412d56d7f884f70cca703/src/libraries/iKin/include/iCub/iKin/iKinFwd.h#L1081, LEFT_LEG https://github.com/robotology/icub-main/blob/4215771441724b7ece6412d56d7f884f70cca703/src/libraries/iKin/include/iCub/iKin/iKinFwd.h#L1235, etc etc. In reality they refer to the equivalent data structures in iDyn . See https://github.com/robotology/icub-main/blob/master/src/libraries/iDyn/src/iDynContact.cpp#L93 for the code that used it.

traversaro commented 3 years ago

In https://github.com/robotology/icub-main/pull/462 you can find a PR that we should revive to finally end this confusion.