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.

Sign up for GitHub

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

bullet-featherstone: Fix finding free group for a body with fixed base #700

Merged
merged 6 commits into from
Dec 13, 2024
Merged

bullet-featherstone: Fix finding free group for a body with fixed base #700

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
5c200b2
Fix setting world pose to static free group
iche033 Nov 1, 2024
538ea4b
fix build
iche033 Nov 1, 2024
949b20e
Merge branch 'gz-physics8' into bullet_free_group_static
iche033 Nov 6, 2024
a7e6eb5
fix for model with world joint, add test
iche033 Nov 11, 2024
e7a7ccb
remove debug print
iche033 Nov 11, 2024
ded1e0a
rename model str
iche033 Dec 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 14 additions & 12 deletions bullet-featherstone/src/FreeGroupFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,37 +85,39 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
{
const auto *model = this->ReferenceInterface<ModelInfo>(_modelID);

// Reject if the model has fixed base
if (model->body->hasFixedBase())
return this->GenerateInvalidId();

// Also reject if the model is a child of a fixed constraint
// (detachable joint)
for (const auto & joint : this->joints)
{
// Also reject if the model is a child of a fixed constraint
// (detachable joint)
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))
Copy link
Contributor

Choose a reason for hiding this comment

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

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.

Copy link
Member

Choose a reason for hiding this comment

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

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

ok sounds like a new feature to add so I'll keep this PR as is.

{
const auto *identifier =
std::get_if<RootJoint>(&joint.second->identifier);
if (identifier)
{
return this->GenerateInvalidId();
}
}
}


return _modelID;
}

/////////////////////////////////////////////////
Identity FreeGroupFeatures::FindFreeGroupForLink(
const Identity &_linkID) const
{
// Free groups in bullet-featherstone are currently represented by ModelInfo
const auto *link = this->ReferenceInterface<LinkInfo>(_linkID);
const auto *model = this->ReferenceInterface<ModelInfo>(link->model);
if (model->body->hasFixedBase())
return this->GenerateInvalidId();

return link->model;
return this->FindFreeGroupForModel(link->model);
}

/////////////////////////////////////////////////
Expand Down
115 changes: 115 additions & 0 deletions test/common_test/free_joint_features.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -316,6 +316,121 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset)
}
}

TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticAndFixedModel)
{
const std::string modelStaticStr = R"(
<sdf version="1.11">
<model name="sphere">
<static>true</static>
<pose>1 2 3.0 0 0 0</pose>
<link name="link">
<collision name="coll_sphere">
<geometry>
<sphere>
<radius>0.1</radius>
</sphere>
</geometry>
</collision>
</link>
</model>
</sdf>)";

const std::string modelWorldFixedStr = R"(
<sdf version="1.11">
<model name="sphere_world_fixed">
<static>false</static>
<pose>3 2 3.0 0 0 0</pose>
<link name="link">
<collision name="coll_sphere">
<geometry>
<sphere>
<radius>0.1</radius>
</sphere>
</geometry>
</collision>
</link>
<joint name="world_fixed" type="fixed">
<parent>world</parent>
<child>link</child>
</joint>
</model>
</sdf>)";

for (const std::string &name : pluginNames)
{
std::cout << "Testing plugin: " << name << std::endl;
gz::plugin::PluginPtr plugin = loader.Instantiate(name);

auto engine = gz::physics::RequestEngine3d<TestFeatureList>::From(plugin);
ASSERT_NE(nullptr, engine);

sdf::Root root;
sdf::Errors errors = root.Load(
common_test::worlds::kGroundSdf);
EXPECT_EQ(0u, errors.size()) << errors;

EXPECT_EQ(1u, root.WorldCount());
const sdf::World *sdfWorld = root.WorldByIndex(0);
ASSERT_NE(nullptr, sdfWorld);

auto world = engine->ConstructWorld(*sdfWorld);
ASSERT_NE(nullptr, world);

// create the static model
errors = root.LoadSdfString(modelStaticStr);
ASSERT_TRUE(errors.empty()) << errors;
ASSERT_NE(nullptr, root.Model());
world->ConstructModel(*root.Model());

auto modelStatic = world->GetModel("sphere");
ASSERT_NE(nullptr, modelStatic);
auto link = modelStatic->GetLink("link");
ASSERT_NE(nullptr, link);
auto frameDataLink = link->FrameDataRelativeToWorld();
EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0),
gz::math::eigen3::convert(frameDataLink.pose));

// get free group and set new pose
auto freeGroup = modelStatic->FindFreeGroup();
ASSERT_NE(nullptr, freeGroup);
gz::math::Pose3d newPose(4, 5, 6, 0, 0, 1.57);
freeGroup->SetWorldPose(
gz::math::eigen3::convert(newPose));
frameDataLink = link->FrameDataRelativeToWorld();
// static model should move to new pose
EXPECT_EQ(newPose,
gz::math::eigen3::convert(frameDataLink.pose));

// create the model with world joint
errors = root.LoadSdfString(modelWorldFixedStr);
ASSERT_TRUE(errors.empty()) << errors;
ASSERT_NE(nullptr, root.Model());
world->ConstructModel(*root.Model());

auto modelFixed = world->GetModel("sphere_world_fixed");
ASSERT_NE(nullptr, modelFixed);
link = modelFixed->GetLink("link");
ASSERT_NE(nullptr, link);
frameDataLink = link->FrameDataRelativeToWorld();
gz::math::Pose3d origPose(3, 2, 3, 0, 0, 0);
EXPECT_EQ(origPose,
gz::math::eigen3::convert(frameDataLink.pose));

// get free group and set new pose
freeGroup = modelFixed->FindFreeGroup();
ASSERT_NE(nullptr, freeGroup);
freeGroup->SetWorldPose(
gz::math::eigen3::convert(newPose));
frameDataLink = link->FrameDataRelativeToWorld();
// world fixed model should not be able to move to new pose
if (this->PhysicsEngineName(name) != "tpe")
{
EXPECT_EQ(origPose,
gz::math::eigen3::convert(frameDataLink.pose));
}
}
}

int main(int argc, char *argv[])
{
::testing::InitGoogleTest(&argc, argv);
Expand Down
Loading