bullet-featherstone: Fix finding free group for a body with fixed base #700
Conversation
Signed-off-by: Ian Chen <[email protected]>
Signed-off-by: Ian Chen <[email protected]>
| @@ -316,6 +316,71 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset) | |||
| } | |||
| } | |||
|
|
|||
| TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel) | |||
There was a problem hiding this comment.
Do we need to check how it behaves with non-static models attached to the world with a fixed joint?
There was a problem hiding this comment.
updated code to make sure non-static model with a world joint can not be moved, also added a test a7e6eb5
Signed-off-by: Ian Chen <[email protected]>
iche033
force-pushed
the
bullet_free_group_static
branch
from
6362297 to
05368c7
Compare
Signed-off-by: Ian Chen <[email protected]>
iche033
force-pushed
the
bullet_free_group_static
branch
from
05368c7 to
e7a7ccb
Compare
| if (joint.second->fixedConstraint) | ||
| { | ||
| if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get()) | ||
| { | ||
| return this->GenerateInvalidId(); | ||
| } | ||
| } | ||
| // Reject if the model has a world joint | ||
| if (std::size_t(joint.second->model) == std::size_t(_modelID)) |
There was a problem hiding this comment.
I think this is good for matching the behavior of DART, but we might want to consider the allowing models with world joints to move. I think it would be more consistent with also allowing static models to move. I won't block on this, but I'd like to hear @scpeters thoughts on it.
There was a problem hiding this comment.
according to our documentation of FreeGroup in FreeGroup.hh:
the FreeGroup class, which represents a group of links that are not connected to the world with any kinematic constraints
I think it could be a nice user feature in gz-sim to be able to move objects that have a joint connecting them to the world, but by the definition of a FreeGroup, I don't think we can support that with this gz-physics API. I think we would need a gz-sim feature that detaches a joint, moves, and then reattaches the joint.
There was a problem hiding this comment.
ok sounds like a new feature to add so I'll keep this PR as is.
| if (joint.second->fixedConstraint) | ||
| { | ||
| if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get()) | ||
| { | ||
| return this->GenerateInvalidId(); | ||
| } | ||
| } | ||
| // Reject if the model has a world joint | ||
| if (std::size_t(joint.second->model) == std::size_t(_modelID)) |
There was a problem hiding this comment.
according to our documentation of FreeGroup in FreeGroup.hh:
the FreeGroup class, which represents a group of links that are not connected to the world with any kinematic constraints
I think it could be a nice user feature in gz-sim to be able to move objects that have a joint connecting them to the world, but by the definition of a FreeGroup, I don't think we can support that with this gz-physics API. I think we would need a gz-sim feature that detaches a joint, moves, and then reattaches the joint.
Signed-off-by: Ian Chen <[email protected]>
🦟 Bug fix
Fixes gazebosim/gz-sim#2663
Summary
Finding free group for a body that has a fixed base results an invalid ID, which causes set world pose from gz-sim to fail, see gazebosim/gz-sim#2663. This PR removes the fixed base body check and I verified that set world pose works for a body with fixed base.
Another minor change - I noticed that finding free group for links has the same check. Currently free groups only works for models in bullet-featherstone so I updated the function to just call
FindFreeGroupForModel.Checklist
codecheckpassed (See contributing)Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining
Signed-off-bymessages.