dartsim: fix handling inertia matrix pose rotation (#351)

When loading a model from SDF, the moment of inertia matrix is currently applying any rotations in the //inertial/pose two times, since the rotations are applied explicitly, but they are already applied in math::Inertial::Moi. Signed-off-by: Steve Peters <[email protected]> Signed-off-by: Louise Poubel <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
gazebosim · Nov 9, 2023 · 4c60522 · 4c60522
1 parent c4629f6
commit 4c60522
Showing 1 changed file with 1 addition and 9 deletions.
diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
@@ -417,15 +417,7 @@ Identity SDFFeatures::ConstructSdfLink(
   const math::Inertiald &sdfInertia = _sdfLink.Inertial();
   bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass());
 
-  // TODO(addisu) Resolve the pose of inertials when frame information is
-  // availabile for math::Inertial
-  const Eigen::Matrix3d R_inertial{
-        math::eigen3::convert(sdfInertia.Pose().Rot())};
-
-  const Eigen::Matrix3d I_link =
-      R_inertial
-      * math::eigen3::convert(sdfInertia.Moi())
-      * R_inertial.inverse();
+  const Eigen::Matrix3d I_link = math::eigen3::convert(sdfInertia.Moi());
 
   bodyProperties.mInertia.setMoment(I_link);