SimbodyLink: add and set force and torque #3262
Open
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
The following methods are now implemented in the SimbodyLink class: - AddForce - AddRelativeForce - AddForceAtWorldPosition - AddForceAtRelativePosition - AddLinkForce - AddTorque - AddRelativeTorque Additionally, the implementation of the SetForce, and SetTorque methods was modified to ensure that setting the force applied to the body do not alter the torque currently applied to the body and vice versa. In order to test the above methods, the AddForce test of the physics_link integration tests is now enabled for Simbody. Solves issue gazebosim#1478.
|
@scpeters Could you please review this PR? |
scpeters
reviewed
Sep 22, 2022
| { | ||
| gzerr << "Not implemented.\n"; | ||
| auto const worldForce = this->WorldPose().Rot().RotateVector(_force); | ||
| this->AddTorque(worldForce); |
There was a problem hiding this comment.
I'm confused why we would call AddTorque from the AddRelativeForce method
There was a problem hiding this comment.
I'm sorry, this is a mistake I made. Line 519 should be a call to this->AddForce. I fixed this in the latest commit.
The implementation of the SimbodyLink::AddRelativeForce was incorrectly calling the AddTorque method. This commit changes the implementation to call AddForce instead.
|
Any updates on this? |
sorry I haven't had a chance to finish my review, but I'll try to take a look this week |
|
@jeduardo211 Can you rebase to the latest gazebo11 dev? |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
This PR adds the implementation of methods for applying forces and torques to links when using the Simbody physics engine.
The following methods are now implemented in the SimbodyLink class:
Additionally, the implementation of the SetForce, and SetTorque methods was modified to ensure that setting the force applied to the body do not alter the torque currently applied to the body and vice versa.
In order to test the above methods, the AddForce test of the physics_link integration tests is now enabled for Simbody. Please note that Simbody required an additional step to update the pose of the link and its inertial origin, and thus the AddForce test was modified to reflect this.
Note: The OnWrenchMsg test is still disabled for Simbody.
There is an easy way to enable this test, by adding a "auto_reset_forces" parameter to the SimbodyPhysics and checking the value of this parameter in the SimbodyPhysics::UpdatePhysics method to determine if the forces and torques applied should be cleared before the next step.
However, the SimbodyPhysics class is not following the PIMPL pattern, and thus adding this parameter would introduce breaking changes in the API. Because of this these modifications were not included and the OnWrenchMsg test remains disabled for simbody.
If this change is acceptable please advise and I will create a new PR to enable this test.
Solves issue #1478.