From 98fe5a4d638af09c065cbca321315fe905a0715a Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Jun 2022 01:44:54 -0700 Subject: [PATCH 1/2] dartsim: fix handling inertia matrix pose rotation 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 --- dartsim/src/SDFFeatures.cc | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 0d5206187..4d1921dcb 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -596,10 +596,7 @@ Identity SDFFeatures::ConstructSdfLink( 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); From d56f652e1b9d51646403229ed45e7d2496d8fc99 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 1 Aug 2022 08:52:46 -0700 Subject: [PATCH 2/2] Removed unused variable and addressed TODO Signed-off-by: Louise Poubel --- dartsim/src/SDFFeatures.cc | 5 ----- 1 file changed, 5 deletions(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 4d1921dcb..65f713503 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -591,11 +591,6 @@ Identity SDFFeatures::ConstructSdfLink( const ignition::math::Inertiald &sdfInertia = _sdfLink.Inertial(); bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass()); - // TODO(addisu) Resolve the pose of inertials when frame information is - // availabile for ignition::math::Inertial - const Eigen::Matrix3d R_inertial{ - math::eigen3::convert(sdfInertia.Pose().Rot())}; - const Eigen::Matrix3d I_link = math::eigen3::convert(sdfInertia.Moi()); bodyProperties.mInertia.setMoment(I_link);