bullet-featherstone: fix SetWorldPose with off-diagonal moment of ine…

…rtia (#623) Signed-off-by: Ian Chen <ichen@openrobotics.org> Signed-off-by: Davide Graziato <dgraziato10@gmail.com>
gazebosim · Apr 19, 2024 · 8a5769a · 8a5769a
1 parent 624dc27
commit 8a5769a
Show file tree

Hide file tree

Showing 2 changed files with 78 additions and 1 deletion.
diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -93,7 +93,8 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   const auto *model = this->ReferenceInterface<ModelInfo>(_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
@@ -240,6 +240,82 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose)
   }
 }
 
+TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset)
+{
+  const std::string modelStr = R"(
+    <sdf version="1.11">
+      <model name="box">
+        <pose>1 2 3.0 0 0 0</pose>
+        <link name="link">
+          <inertial>
+            <pose>-0.00637 -0.008 0.13254 0 0 0</pose>
+            <inertia>
+              <ixx>0.01331127</ixx>
+              <ixy>-0.00030365</ixy>
+              <ixz>-0.00034148</ixz>
+              <iyy>0.01157659</iyy>
+              <iyz>0.00088073</iyz>
+              <izz>0.00378028</izz>
+            </inertia>
+            <mass>1.50251902</mass>
+          </inertial>
+          <collision name="coll_sphere">
+            <geometry>
+              <sphere>
+                <radius>0.1</radius>
+              </sphere>
+            </geometry>
+          </collision>
+        </link>
+      </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 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);