-
Notifications
You must be signed in to change notification settings - Fork 38
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
Conversation
1632c9c
to
d1e4335
Compare
Signed-off-by: Davide Graziato <[email protected]>
d1e4335
to
3f72282
Compare
// 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; |
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
Signed-off-by: Davide Graziato <[email protected]>
c01ae96
to
1019f6e
Compare
CI picked up a few cpplint warnings:
|
Signed-off-by: Davide Graziato <[email protected]>
Codecov ReportAttention: Patch coverage is
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. |
Signed-off-by: Ian Chen <[email protected]>
Any changes needed in order to merge? I can write the test for gazebosim/gz-sim#2369 afterward. |
I'll go ahead and merge this first |
🎉 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 thejoint_state_publisher
to publish data containing the force field.Test it
gz sim --physics-engine gz-physics-bullet-featherstone-plugin lift_drag.sdf
simulation.gz topic -t "/model/lift_drag_demo_model/joint/rod_1_joint/cmd_force" -m gz.msgs.Double -p "data: 0.8"
).gz topic -e -t /world/lift_drag/model/lift_drag_demo_model/joint_state
Checklist
codecheck
passed (See contributing)