From 5c200b2abe12c4694d1f5e650a9f7dbd5d460004 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 1 Nov 2024 17:18:24 +0000 Subject: [PATCH 1/5] Fix setting world pose to static free group Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 12 +--- test/common_test/free_joint_features.cc | 65 ++++++++++++++++++++ 2 files changed, 67 insertions(+), 10 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index de0be3dca..2e29bd668 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -85,10 +85,6 @@ 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) @@ -102,7 +98,6 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( } } - return _modelID; } @@ -110,12 +105,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 FindFreeGroupForModel(link->model) } ///////////////////////////////////////////////// diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index a896efcb7..7d647b405 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -316,6 +316,71 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset) } } +TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel) +{ + const std::string modelStr = R"( + + + true + 1 2 3.0 0 0 0 + + + + + 0.1 + + + + + + )"; + + 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 model + errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("sphere"); + ASSERT_NE(nullptr, model); + auto link = model->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 = model->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(); + EXPECT_EQ(newPose, + gz::math::eigen3::convert(frameDataLink.pose)); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From 538ea4b1769c424232aba1e9255e07ee9f659e1e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 1 Nov 2024 17:48:19 +0000 Subject: [PATCH 2/5] fix build Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 2e29bd668..852136f82 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -107,7 +107,7 @@ Identity FreeGroupFeatures::FindFreeGroupForLink( { // Free groups in bullet-featherstone are currently represented by ModelInfo const auto *link = this->ReferenceInterface(_linkID); - return FindFreeGroupForModel(link->model) + return this->FindFreeGroupForModel(link->model); } ///////////////////////////////////////////////// From a7e6eb51548c990acee13913b9811fa19d03a59b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 11 Nov 2024 22:20:39 +0000 Subject: [PATCH 3/5] fix for model with world joint, add test Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 15 ++++- bullet-featherstone/src/SDFFeatures.cc | 1 + test/common_test/free_joint_features.cc | 63 ++++++++++++++++++-- 3 files changed, 71 insertions(+), 8 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 852136f82..fadc806a1 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -85,10 +85,11 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( { const auto *model = this->ReferenceInterface(_modelID); - // Also reject if the model is a child of a fixed constraint - // (detachable joint) + std::cerr << "joints " << this->joints.size() << std::endl; 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()) @@ -96,6 +97,16 @@ 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; diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index ebb7c686c..3faed764b 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -578,6 +578,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( // Add world joint if (structure.rootJoint) { + std::cerr << " adding root joint " << structure.rootJoint->Name() << std::endl; const auto &parentInfo = structure.parentOf.at(structure.rootLink); this->AddJoint( JointInfo{ diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 7d647b405..2fc818a2c 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -316,7 +316,7 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset) } } -TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel) +TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticAndFixedModel) { const std::string modelStr = R"( @@ -335,6 +335,28 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel) )"; + 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; @@ -355,29 +377,58 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel) auto world = engine->ConstructWorld(*sdfWorld); ASSERT_NE(nullptr, world); - // create the model + // create the static model errors = root.LoadSdfString(modelStr); ASSERT_TRUE(errors.empty()) << errors; ASSERT_NE(nullptr, root.Model()); world->ConstructModel(*root.Model()); - auto model = world->GetModel("sphere"); - ASSERT_NE(nullptr, model); - auto link = model->GetLink("link"); + 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 = model->FindFreeGroup(); + 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)); + } } } From e7a7ccb9a9c8978fc8cf3aa3599026d3fa979bed Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 11 Nov 2024 22:22:27 +0000 Subject: [PATCH 4/5] remove debug print Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 1 - bullet-featherstone/src/SDFFeatures.cc | 1 - test/common_test/free_joint_features.cc | 1 - 3 files changed, 3 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index fadc806a1..6cf0918b5 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -85,7 +85,6 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( { const auto *model = this->ReferenceInterface(_modelID); - std::cerr << "joints " << this->joints.size() << std::endl; for (const auto & joint : this->joints) { // Also reject if the model is a child of a fixed constraint diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 3faed764b..ebb7c686c 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -578,7 +578,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( // Add world joint if (structure.rootJoint) { - std::cerr << " adding root joint " << structure.rootJoint->Name() << std::endl; const auto &parentInfo = structure.parentOf.at(structure.rootLink); this->AddJoint( JointInfo{ diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 2fc818a2c..3562ec6c8 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -356,7 +356,6 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticAndFixedModel) )"; - for (const std::string &name : pluginNames) { std::cout << "Testing plugin: " << name << std::endl; From ded1e0a7efcdb9c61d69e917a5c242a4aa423bbc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 7 Dec 2024 14:41:21 +0800 Subject: [PATCH 5/5] rename model str Signed-off-by: Ian Chen --- test/common_test/free_joint_features.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 3562ec6c8..a4dcd7c8d 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -318,7 +318,7 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset) TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticAndFixedModel) { - const std::string modelStr = R"( + const std::string modelStaticStr = R"( true @@ -377,7 +377,7 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticAndFixedModel) ASSERT_NE(nullptr, world); // create the static model - errors = root.LoadSdfString(modelStr); + errors = root.LoadSdfString(modelStaticStr); ASSERT_TRUE(errors.empty()) << errors; ASSERT_NE(nullptr, root.Model()); world->ConstructModel(*root.Model());