diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 62eeebd03..8c9375fd1 100644 --- 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);