[featherstone] Publish JointFeedback forces.

[Fixit-Davide]

🎉 New feature

Related to the PR: gazebosim/gz-sim#2369

Partially solves: gazebosim/gz-sim#883

Summary

Allow the GetJointForce() method in bullet-featherston to return the joint feedback Force and not 0. This allow the joint_state_publisher to publish data containing the force field.

Test it

• Launch gz sim --physics-engine gz-physics-bullet-featherstone-plugin lift_drag.sdf simulation.
• Send a command to the model (gz topic -t "/model/lift_drag_demo_model/joint/rod_1_joint/cmd_force" -m gz.msgs.Double -p "data: 0.8").
• Echo the topic: gz topic -e -t /world/lift_drag/model/lift_drag_demo_model/joint_state

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

[iche033]

Is the results / 2.0 done to workaround the bug? Note that it was recently fixed upstream: bulletphysics/bullet3#4583. I think we also found that total force is not always 2 times than it should be (based on example in #565 and comment: bulletphysics/bullet3#3713 (comment)).

Anyways, best to guard this with a bullet version check e.g. for users using latest bullet version built from source.

#if BT_BULLET_VERSION < 326
  // not sure if this is always true
  return results / 2.0; 
#else
  return results;
#endif

[Fixit-Davide]

Yes, the results / 2.0 part is used as an ugly workaround in order to retrieve a data that is reasonable. I didn't know that the PR was merged when I have write the code.
Probably another workaround (for bullet version < 3.26) could be measured_torque = measured_torque - applied_torque ? But I don't know how to retrieve the information about the applied torque from the jointInfo class.

[iche033]

I just did some quick tests and saw that bullet clears the applied joint forces and torques after each step so I think we're not able to retrieve the applied torque at this point (e.g. model->body->getJointTorqueMultiDof just returns 0). We could cache the applied torque in JointInfo struct but we would need to clear them before every step. For now, I think we can just add a comment about this bug affecting bullet versions < 326 - basically what you had before, a link to bulletphysics/bullet3#3713 or #565.

[iche033]

CI picked up a few cpplint warnings:

  /github/workspace/bullet-featherstone/src/JointFeatures.cc:87:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
  /github/workspace/bullet-featherstone/src/JointFeatures.cc:90:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/bullet-featherstone/src/JointFeatures.cc:95:  Lines should be <= 80 characters long  [whitespace/line_length] [2]

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 24 lines in your changes missing coverage. Please review.

Project coverage is 78.67%. Comparing base (92e02c3) to head (9aee181).
Report is 21 commits behind head on gz-physics7.

❗ Current head 9aee181 differs from pull request most recent head beaacc7

Please upload reports for the commit beaacc7 to get more accurate results.

Files	Patch %	Lines
bullet-featherstone/src/JointFeatures.cc	0.00%	24 Missing ⚠️

Additional details and impacted files

@@               Coverage Diff               @@
##           gz-physics7     #628      +/-   ##
===============================================
+ Coverage        78.32%   78.67%   +0.34%     
===============================================
  Files              140      140              
  Lines             8069     8158      +89     
===============================================
+ Hits              6320     6418      +98     
+ Misses            1749     1740       -9     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[Fixit-Davide]

Any changes needed in order to merge? I can write the test for gazebosim/gz-sim#2369 afterward.

[iche033]

I'll go ahead and merge this first