Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[featherstone] Publish JointFeedback forces. #628

Merged
merged 7 commits into from
Jun 18, 2024

Conversation

Fixit-Davide
Copy link
Contributor

@Fixit-Davide Fixit-Davide commented Apr 19, 2024

🎉 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

// BUG: The total force is 2 times the cmd one see:
// https://github.com/bulletphysics/bullet3/discussions/3713
results += axis_converted.Dot(angularTorque);
return results / 2.0;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

bullet-featherstone/src/JointFeatures.cc Outdated Show resolved Hide resolved
bullet-featherstone/src/JointFeatures.cc Outdated Show resolved Hide resolved
bullet-featherstone/src/JointFeatures.cc Outdated Show resolved Hide resolved
@iche033 iche033 added the Bullet Bullet engine label Apr 23, 2024
Signed-off-by: Davide Graziato <[email protected]>
@iche033
Copy link
Contributor

iche033 commented Apr 24, 2024

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]

Copy link

codecov bot commented May 2, 2024

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
Copy link
Contributor Author

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

@iche033
Copy link
Contributor

iche033 commented Jun 18, 2024

I'll go ahead and merge this first

@iche033 iche033 merged commit 3323de5 into gazebosim:gz-physics7 Jun 18, 2024
8 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Bullet Bullet engine 🎵 harmonic Gazebo Harmonic
Projects
Archived in project
Development

Successfully merging this pull request may close these issues.

None yet

2 participants