diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index de0be3dca..6cf0918b5 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -85,14 +85,10 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( { const auto *model = this->ReferenceInterface(_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()) @@ -100,9 +96,18 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( return this->GenerateInvalidId(); } } + // Reject if the model has a world joint + if (std::size_t(joint.second->model) == std::size_t(_modelID)) + { + const auto *identifier = + std::get_if(&joint.second->identifier); + if (identifier) + { + return this->GenerateInvalidId(); + } + } } - return _modelID; } @@ -110,12 +115,9 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( Identity FreeGroupFeatures::FindFreeGroupForLink( const Identity &_linkID) const { + // Free groups in bullet-featherstone are currently represented by ModelInfo const auto *link = this->ReferenceInterface(_linkID); - const auto *model = this->ReferenceInterface(link->model); - if (model->body->hasFixedBase()) - return this->GenerateInvalidId(); - - return link->model; + return this->FindFreeGroupForModel(link->model); } ///////////////////////////////////////////////// diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index a896efcb7..a4dcd7c8d 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -316,6 +316,121 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset) } } +TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticAndFixedModel) +{ + const std::string modelStaticStr = R"( + + + true + 1 2 3.0 0 0 0 + + + + + 0.1 + + + + + + )"; + + const std::string modelWorldFixedStr = R"( + + + false + 3 2 3.0 0 0 0 + + + + + 0.1 + + + + + + world + link + + + )"; + + 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::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);