[WBD] Add the option to select the type of the Wrench Contact

[HosameldinMohamed]

Summary

The wholeBodyDynamics device assumes that external wrench type is a full-wrench (all the components of the forces/torques are unknowns to be estimated). This PR will allow to set a-priori the assumptions about the wrench type (pure forces or pure forces with known direction), from the configuration file. In addition, it will allow to set the position of the applied external wrench. Also, the direction in the case of the pure force with known direction.

The configuration file wholebodydynamics-external.xml located in <installation-directory>/share/yarp/robots/iCubGazeboV2_5/estimator should be have the following lines added (after the parameter defaultContactFrames):

<param name="overrideContactFrames">(l_hand,r_hand,chest,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1)</param>
<param name="contactWrenchType">(       pureKnown   ,pureKnown       ,full    ,pureKnown     ,pureKnown     ,pure                 ,pureKnown                ,pureKnown              ,pureKnown)</param>
<param name="contactWrenchDirection">(1,0,0   ,0,5,0    ,0,0,1 ,0,0,1  ,0,0,1  ,0,0,1              ,0,0,1             ,0,0,1           ,0,0,1)</param>
<param name="contactWrenchPosition">(  0,0,0   ,8,1,12    ,10,5,-4 ,8,0,12 ,8,0,12 ,8,0,12             ,0,0,0             ,0,0,0           ,0,0,0)</param>

Details

See https://github.com/robotology/whole-body-estimators/issues/17#issue-558572450.

[HosameldinMohamed]

CC @gabrielenava

[prashanthr05]

The configuration file wholebodydynamics-external.xml located in <installation-directory>/share/yarp/robots/iCubGazeboV2_5/estimator should be have the following lines added (after the parameter defaultContactFrames):

<param name="overrideContactFrames">(l_hand,r_hand,chest,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1)</param>
<param name="contactWrenchType">(       pureKnown   ,pureKnown       ,full    ,pureKnown     ,pureKnown     ,pure                 ,pureKnown                ,pureKnown              ,pureKnown)</param>
<param name="contactWrenchDirection">(1,0,0   ,0,5,0    ,0,0,1 ,0,0,1  ,0,0,1  ,0,0,1              ,0,0,1             ,0,0,1           ,0,0,1)</param>
<param name="contactWrenchPosition">(  0,0,0   ,8,1,12    ,10,5,-4 ,8,0,12 ,8,0,12 ,8,0,12             ,0,0,0             ,0,0,0           ,0,0,0)</param>

Do you think using a bottle of bottles would offer better separability between two different entities than a single long vector, which is prone to error given there is no proper spacing?

Just a curiosity! if the existing choice is already convenient, we go with it.

[HosameldinMohamed]

I saw the changes in general seems alright. I just have doubts about the need to add the frame name. I could not find where do you use it. If its not needed then we could use the iDyntree unkownWrenchContact no?

@fjandrad thanks. In https://github.com/robotology/whole-body-estimators/pull/48#discussion_r395712695 I try to justify why I created another entity.

[prashanthr05]

In the future, I suggest you give more meaningful/descriptive commit messages than

Address @prashanthr05 comments

[prashanthr05]

In the future, I suggest you give more meaningful/descriptive commit messages than

Address @prashanthr05 comments

You could already do so by the following commands, git rebase -i HEAD~2 where 2 is the number of previous commits you want to be shown starting from the head. Then you would have multiple options to either squash commits s, reword the commit messages r, etc. You replace the tag pick against the respective commit with r to reword the commit.

However, by doing so, you are changing history, so you must decide on this carefully.

Alternately you could also use git amend

For more info, please see https://help.github.com/en/github/committing-changes-to-your-project/changing-a-commit-message

[HosameldinMohamed]

Hi guys @prashanthr05 @fjandrad , thank you for your comments. In the previous versions I wanted to have minimum modification to the existing code that's why I created unnecessary objects and had so much code duplicated.

In the latest commits (https://github.com/robotology/whole-body-estimators/pull/48/commits/38c21cdf2a12d33f0fd9a3f736d69353bb89e86e), I have removed the class wholeBodyDynamics::UnknownWrenchContact and used the members of the struct iDynTree::UnknownWrenchContact instead. At this stage. I don't think it's necessary to update iDynTree::UnknownWrenchContact to have the members we talked about (frameName, nrOfUnknowns and display()).

The duplication was removed by filling iDynTree::UnknownWrenchContact objects for each frame name, whether defaultContactFrames or overrideContactFrames are used, except for defaultContactFrames, we put the type=iDynTree::FULL_WRENCH.

I have also modified the publishing function according to https://github.com/robotology/whole-body-estimators/issues/17#issuecomment-612414675 and the preceding discussion. I have published the contact wrenches instead of the net-wrench if contactsReadFromSkin.empty() is satisfied. CC @traversaro .

[prashanthr05]

How do we agree on a valid test or check for merging this PR?

As usual wait for the tests on the robot? @traversaro @fjandrad

[traversaro]

As usual wait for the tests on the robot? @traversaro @fjandrad

In the current situation this is not doable. If it works on iCubGazeboV2_5 with the old configuration file, I think that is ok. If there are regressions on the real robot, we will fix when we will be able to test on a real robot.

[prashanthr05]

In the latest commits (38c21cd), I have removed the class wholeBodyDynamics::UnknownWrenchContact and used the members of the struct iDynTree::UnknownWrenchContact instead. At this stage. I don't think it's necessary to update iDynTree::UnknownWrenchContact to have the members we talked about (frameName, nrOfUnknowns and display()).

@HosameldinMohamed Why do you think it's not necessary? Technically, yes it wouldn't be necessary, but it would be nice. I thought it would lighten the number of additional variables you introduce here in WholeBodyDynamics.h and keep it in conatined within the relevant structure.

[HosameldinMohamed]

@HosameldinMohamed Why do you think it's not necessary? Technically, yes it wouldn't be necessary, but it would be nice. I thought it would lighten the number of additional variables you introduce here in WholeBodyDynamics.h and keep it in conatined within the relevant structure.

@prashanthr05 You're right. However, when I checked the source code in iDynTree, I saw that adding members nrOfUnknowns and frameName may not be good in terms of consistency of the code.

About the frameName variable, the struct UnknownWrenchContact represents A contact whose wrench is unknown without associating it to a frame. Then the class LinkUnknownWrenchContacts add the UnknownWrenchContacts to the link using addNewContactInFrame.

About the nrOfUnknowns variable, iDynTree computes it at a later stage, using the function countUnknowns, which is called when computing the matrices in this function.

And for debugging, the class LinkUnknownWrenchContacts has a toString() method that can be called after adding the contacts.

So I felt the additional members will not be necessary because they are implemented somehow iDynTree.

As we saw in our case in wholeBodyDynamics, I create the structs UnknownWrenchContact in openContactFrames() so that I can assign them directly to addNewContactInFrame in readContactPoints().

[prashanthr05]

About the frameName variable, the struct UnknownWrenchContact represents A contact whose wrench is unknown without associating it to a frame. Then the class LinkUnknownWrenchContacts add the UnknownWrenchContacts to the link using addNewContactInFrame.

True. However, my argument here is an unknown wrench needs to be identified at some point and needs to be provided with ceratin metadata (for comparisons). An use-case, for example in a contact localization scenario, if we have some contact location semantics (lets now simplify to name), we can just look at the name to match for newly identified unknownwrench and an already identified unknownwrench to see if they associate to the same contact location and associate them to the same name (more or less analogous to loop closure or place recognition in SLAM).

About the nrOfUnknowns variable, iDynTree computes it at a later stage, using the function countUnknowns, which is called when computing the matrices in this function.

then, in this case, it would have made more sense to factor out the counting code as a public function within the relevant class so that it can be reused elsewhere. Duplicating code is highly susceptible to errors in the core concepts when the core concepts tend to evolve (in one place and remain outdated in another). In this case since we are dependent on iDynTree this is all the more valid, in case of independent software, it does not matter a lot to reimplement things.

These are my thoughts.

Already by removing the extra structs we initially introduce within this PR, the refactored code commits look a lot more cleaner and easy to understand, looking at the gains vs time tradeoffs.

But we can keep these details in mind, if and when, we refactor the software.

[HosameldinMohamed]

If it works on iCubGazeboV2_5 with the old configuration file, I think that is ok.

I've tested it with a configuration file and it seems ok. I didn't have issues.

These are my thoughts.

Already by removing the extra structs we initially introduce within this PR, the refactored code commits look a lot more cleaner and easy to understand, looking at the gains vs time tradeoffs.

But we can keep these details in mind, if and when, we refactor the software.

They make sense. I would proceed with merging this PR for now. Then we can open issues/PRs for possible enhancements.

Thanks guys.

[HosameldinMohamed]

@traversaro

Ok for me for merging, except for the part on the publishing external wrenches that is not related to the PR,

I think it's related to the PR because the user can't find this useful if they get netExternalWrench of the whole subsystem. We need to publish each external link separately somehow.

and breaks who reads from that port

Do you think the condition contactsReadFromSkin.empty() that I put is enough? Because when it it's true, and defaultsContactsFrames is chosen, publishing the contact wrenches should be the same as netExternalWrenches. What do you think?

[traversaro]

and breaks who reads from that port

Do you think the condition contactsReadFromSkin.empty() that I put is enough? Because when it it's true, and defaultsContactsFrames is chosen, publishing the contact wrenches should be the same as netExternalWrenches. What do you think?

I do not think that it is not enough to avoid problems in the future, because it is quite a complex, coupled and undocumented behaviour. The semantics of that port in the documentation is clear, and if I read in the docs of the WBD_OUTPUT_EXTERNAL_WRENCH_PORTS that it publishes this port publishes the net external wrench applied on a link, it should continue to publish the net external wrench applied on a link. If you want to read as separated contact forces all the contacts that are estimated on a link, you can use the existing dynContacts:o port, or add some new functionality to stream this information in a port (for example for having it in a YARP port that publishes as a vector.

To explain why I think the modification in the current form is dangerous, thing about the following: the simulink controllers read the external wrench being applied on a sole using the /wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o port (relative config: https://github.com/robotology/robots-configuration/blob/v1.15.0/iCubGenova02/estimators/wholebodydynamics.xml#L39 ). As some point in the future, let's say that someone specifies via overrideContactFrames that instead of a single wrench (specified via defaultContactFrames) the force under the sole should be estimate as two pure forces in two different points of the sole. Without him suspecting anything, the semantics of the /wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o port will change from publishing the 6D force/torque vector applied on the sole, to another 6D vector that contains instead two 3D force vectors. The tricky aspect is that the size of the vector will not change, so on the simulink side no error will be raised, but the controller will read completly wrong data.

[traversaro]

@HosameldinMohamed we can discuss about this change of behavior of WBD_OUTPUT_EXTERNAL_WRENCH_PORTS when you prefer via Teams, I guess that also @prashanthr05 can join us if he wants.

[HosameldinMohamed]

@HosameldinMohamed we can discuss about this change of behavior of WBD_OUTPUT_EXTERNAL_WRENCH_PORTS when you prefer via Teams, I guess that also @prashanthr05 can join us if he wants.

I have implemented one recommendation we agreed on: Added a parameter:

<!-- "netWrench" or contactWrenches -->
        <param name="outputWrenchPortInfoType">   contactWrenches   </param>

It's an optional parameter with a default value netWrench which keeps the original behaviour. Selecting contactWrenches changes the output of the ports to publish the contact wrenches. Done in https://github.com/robotology/whole-body-estimators/pull/48/commits/a91ed5422a088256a65c7c27d838184f94cb28ba

---

If it works on iCubGazeboV2_5 with the old configuration file, I think that is ok.

I have run yarprobotinterface with the file installed in <robotology-superbuild_location>/robotology-superbuild/build/install/share/yarp/robots/iCubGazeboV2_5/. Here's a test by applying an external wrench to the left hand and checking the plot of /wholeBodyDynamics/left_arm/endEffectorWrench:o

---

If you think it's okay we may merge the PR. Thanks. @prashanthr05 @traversaro @fjandrad

[traversaro]

It seems ok for me!

[prashanthr05]

@HosameldinMohamed You could update the Changelog and proceed with the merge, thanks! :)

[fjandrad]

Should I rebase the temperature PR? @prashanthr05 @traversaro

[traversaro]

Should I rebase the temperature PR? @prashanthr05 @traversaro

I guess it is a good idea to avoid problems during the PR/merge.