diff --git a/src/Cone.cc b/src/Cone.cc index e3c41daa5..7e094bdd0 100644 --- a/src/Cone.cc +++ b/src/Cone.cc @@ -159,6 +159,7 @@ std::optional Cone::CalculateInertial(double _density) { gz::math::Inertiald coneInertial; coneInertial.SetMassMatrix(coneMassMatrix.value()); + coneInertial.SetPose({0, 0, -0.25 * this->dataPtr->cone.Length(), 0, 0, 0}); return std::make_optional(coneInertial); } } diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc index d09e87da7..59c5dbf3e 100644 --- a/src/Cone_TEST.cc +++ b/src/Cone_TEST.cc @@ -219,7 +219,7 @@ TEST(DOMCone, CalculateInertial) gz::math::Inertiald expectedInertial; expectedInertial.SetMassMatrix(expectedMassMat); - expectedInertial.SetPose(gz::math::Pose3d::Zero); + expectedInertial.SetPose({0, 0, -l / 4.0, 0, 0, 0}); auto coneInertial = cone.CalculateInertial(density); EXPECT_EQ(cone.Shape().Mat().Density(), density);