diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 22e219528..4fd79f57f 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -93,7 +93,8 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const auto *model = this->ReferenceInterface(_groupID); if (model) { - model->body->setBaseWorldTransform(convertTf(_pose)); + model->body->setBaseWorldTransform( + convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); } } diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 7fe3b2633..a896efcb7 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -240,6 +240,82 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) } } +TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset) +{ + const std::string modelStr = R"( + + + 1 2 3.0 0 0 0 + + + -0.00637 -0.008 0.13254 0 0 0 + + 0.01331127 + -0.00030365 + -0.00034148 + 0.01157659 + 0.00088073 + 0.00378028 + + 1.50251902 + + + + + 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("box"); + 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);