From aaadeeaa29e0c9a19bf67dc296905e72d91106ea Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 21 Dec 2024 09:25:07 +0800 Subject: [PATCH] Resolve auto inertia based on input mass (#1513) Support auto-inertia computation using mass and density. Implemented based on the suggestions in #1482. Inertia is first auto resolved from all collisions as usual. If mass is specified, we normalize the inertia to get unit inertia, then scaling is applied to match the desired mass. --------- Signed-off-by: Steve Peters Signed-off-by: Ian Chen Co-authored-by: Steve Peters --- include/sdf/Collision.hh | 5 +- include/sdf/Link.hh | 10 +- python/test/pyLink_TEST.py | 164 +++++++++++++- src/Collision.cc | 28 ++- src/Link.cc | 31 ++- src/Link_TEST.cc | 310 +++++++++++++++++++++++++- src/gz_TEST.cc | 13 ++ test/integration/link_dom.cc | 38 +++- test/sdf/inertial_stats_auto_mass.sdf | 140 ++++++++++++ 9 files changed, 713 insertions(+), 26 deletions(-) create mode 100644 test/sdf/inertial_stats_auto_mass.sdf diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index e2285a93c..18169ac99 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -170,12 +170,15 @@ namespace sdf /// \param[in] _autoInertiaParams An ElementPtr to the auto_inertia_params /// element to be used if the auto_inertia_params have not been set in this /// collision. + /// \param[in] _warnIfDensityIsUnset True to generate a warning that + /// the default density value will be used if it is not explicitly set. public: void CalculateInertial( sdf::Errors &_errors, gz::math::Inertiald &_inertial, const ParserConfig &_config, const std::optional &_density, - sdf::ElementPtr _autoInertiaParams); + sdf::ElementPtr _autoInertiaParams, + bool _warnIfDensityIsUnset = true); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index d692476fb..af4d00687 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -351,9 +351,13 @@ namespace sdf const std::string &_resolveTo = "") const; /// \brief Calculate & set inertial values(mass, mass matrix - /// & inertial pose) for the link. Inertial values can be provided - /// by the user through the SDF or can be calculated automatically - /// by setting the auto attribute to true. + /// & inertial pose) for the link. This function will calculate + /// the inertial values if the auto attribute is set to true. + /// If mass is not provided by the user, the inertial values will be + /// determined based on either link density or collision density if + /// explicitly set. Otherwise, if mass is specified, the inertia matrix + /// will be scaled to match the desired mass, while respecting + /// the ratio of density values between collisions. /// \param[out] _errors A vector of Errors object. Each object /// would contain an error code and an error message. /// \param[in] _config Custom parser configuration diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index 154b085b9..41391f897 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -483,12 +483,14 @@ def test_resolveauto_inertialsWithMultipleCollisions(self): link.inertial().mass_matrix().diagonal_moments()) def test_inertial_values_given_with_auto_set_to_true(self): + # The inertia matrix is specified but should be ignored. + # is not speicifed so the inertial values should be computed + # based on the collision density value. sdf = "" + \ "" + \ " " + \ " " + \ " " + \ - " 4.0" + \ " 1 1 1 2 2 2" + \ " " + \ " 1" + \ @@ -519,11 +521,169 @@ def test_inertial_values_given_with_auto_set_to_true(self): root.resolve_auto_inertials(errors, sdfParserConfig) self.assertEqual(len(errors), 0) - self.assertNotEqual(4.0, link.inertial().mass_matrix().mass()) + self.assertEqual(2.0, link.inertial().mass_matrix().mass()) self.assertEqual(Pose3d.ZERO, link.inertial().pose()) self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333), link.inertial().mass_matrix().diagonal_moments()) + def test_resolveauto_inertialsWithMass(self): + # The inertia matrix is specified but should be ignored. + # is speicifed - the auto computed inertial values should + # be scaled based on the desired mass. + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 4.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + self.assertEqual(4.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Pose3d.ZERO, link.inertial().pose()) + self.assertEqual(Vector3d(0.66666, 0.66666, 0.66666), + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsWithMassAndMultipleCollisions(self): + # The inertia matrix is specified but should be ignored. + # is speicifed - the auto computed inertial values should + # be scaled based on the desired mass. + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 12.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 0.0 0.0 0.5 0 0 0" + \ + " 4.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0.0 0.0 -1.0 0 0 0" + \ + " 1.0" + \ + " " + \ + " " + \ + " 1 1 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + "" + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + self.assertEqual(12.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Pose3d.ZERO, + link.inertial().pose()) + self.assertEqual(Vector3d(9.0, 9.0, 2.0), + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsWithMassAndDefaultDensity(self): + # The inertia matrix is specified but should be ignored. + # is speicifed - the auto computed inertial values should + # be scaled based on the desired mass. + # Density is not specified for the bottom collision - it should + # use the default value + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 12000.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 0.0 0.0 0.5 0 0 0" + \ + " 4000.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0.0 0.0 -1.0 0 0 0" + \ + " " + \ + " " + \ + " 1 1 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + "" + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + self.assertEqual(12000.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Pose3d.ZERO, + link.inertial().pose()) + self.assertEqual(Vector3d(9000.0, 9000.0, 2000.0), + link.inertial().mass_matrix().diagonal_moments()) + def test_resolveauto_inertialsCalledWithAutoFalse(self): sdf = "" + \ " " + \ diff --git a/src/Collision.cc b/src/Collision.cc index ca944712d..07ad12805 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -272,7 +272,8 @@ void Collision::CalculateInertial( gz::math::Inertiald &_inertial, const ParserConfig &_config, const std::optional &_density, - sdf::ElementPtr _autoInertiaParams) + sdf::ElementPtr _autoInertiaParams, + bool _warnIfDensityIsUnset) { // Order of precedence for density: double density; @@ -291,18 +292,21 @@ void Collision::CalculateInertial( // 3. DensityDefault value. else { - // If density was not explicitly set, send a warning - // about the default value being used + // If density was not explicitly set, default value is used + // Send a warning about the default value being used if needed density = DensityDefault(); - Error densityMissingErr( - ErrorCode::ELEMENT_MISSING, - "Collision is missing a child element. " - "Using a default density value of " + - std::to_string(DensityDefault()) + " kg/m^3. " - ); - enforceConfigurablePolicyCondition( - _config.WarningsPolicy(), densityMissingErr, _errors - ); + if (_warnIfDensityIsUnset) + { + Error densityMissingErr( + ErrorCode::ELEMENT_MISSING, + "Collision is missing a child element. " + "Using a default density value of " + + std::to_string(DensityDefault()) + " kg/m^3. " + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), densityMissingErr, _errors + ); + } } // If this Collision's auto inertia params have not been set, then use the diff --git a/src/Link.cc b/src/Link.cc index 19c3569fe..57a46eba5 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -211,7 +211,6 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) // If auto is to true but user has still provided // inertial values if (inertialElem->HasElement("pose") || - inertialElem->HasElement("mass") || inertialElem->HasElement("inertia")) { Error err( @@ -670,6 +669,11 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, return; } + auto inertialElem = this->dataPtr->sdf->GetElement("inertial"); + bool massSpecified = inertialElem->HasElement("mass"); + // Warn about using default collision density value if mass is not specified + bool warnUseDefaultDensity = !massSpecified; + gz::math::Inertiald totalInertia; for (sdf::Collision &collision : this->dataPtr->collisions) @@ -677,11 +681,31 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, gz::math::Inertiald collisionInertia; collision.CalculateInertial(_errors, collisionInertia, _config, this->dataPtr->density, - this->dataPtr->autoInertiaParams); + this->dataPtr->autoInertiaParams, + warnUseDefaultDensity); totalInertia = totalInertia + collisionInertia; } - this->dataPtr->inertial = totalInertia; + // If mass is specified, scale inertia to match desired mass + if (massSpecified) + { + double mass = inertialElem->Get("mass"); + const gz::math::MassMatrix3d &totalMassMatrix = totalInertia.MassMatrix(); + // normalize to get the unit mass matrix + gz::math::MassMatrix3d unitMassMatrix(1.0, + totalMassMatrix.DiagonalMoments() / totalMassMatrix.Mass(), + totalMassMatrix.OffDiagonalMoments() / totalMassMatrix.Mass()); + // scale the final inertia to match specified mass + this->dataPtr->inertial = gz::math::Inertiald( + gz::math::MassMatrix3d(mass, + mass * unitMassMatrix.DiagonalMoments(), + mass * unitMassMatrix.OffDiagonalMoments()), + totalInertia.Pose()); + } + else + { + this->dataPtr->inertial = totalInertia; + } // If CalculateInertial() was called with SAVE_CALCULATION // configuration then set autoInertiaSaved to true @@ -695,7 +719,6 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, { this->dataPtr->autoInertiaSaved = true; // Write calculated inertia values to //link/inertial element - auto inertialElem = this->dataPtr->sdf->GetElement("inertial"); inertialElem->GetElement("pose")->GetValue()->Set( totalInertia.Pose()); inertialElem->GetElement("mass")->GetValue()->Set( diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 7a9207dd0..8f32db944 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -389,6 +389,24 @@ TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity) )"; + // Parse first with enforcement policy set to ERR to detect warnings. + { + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + // Set enforcement policy to ERR so we can detect warnings in sdf::Errors. + sdfParserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); + // Expect 1 warning due to an unset collision density. + EXPECT_EQ(1u, errors.size()) << errors; + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "Collision is missing a child element. Using a " + "default density value of")) << errors; + EXPECT_NE(nullptr, root.Element()); + } + + // Parse again with default enforcement policy and expect no warnings. sdf::Root root; const sdf::ParserConfig sdfParserConfig; sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); @@ -727,12 +745,15 @@ TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) ///////////////////////////////////////////////// TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) { + // A model with link inertial auto set to true. + // The inertia matrix is specified but should be ignored. + // is not speicifed so the inertial values should be computed + // based on the collision density value. std::string sdf = "" "" " " " " " " - " 4.0" " 1 1 1 2 2 2" " " " 1" @@ -752,6 +773,27 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) " " " "; + // Parse first with enforcement policy set to ERR to detect warnings. + { + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + // Set enforcement policy to ERR so we can detect warnings in sdf::Errors. + sdfParserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + // Expect 1 warning due to user-specified inertial values when using + // inertial auto=true. + EXPECT_EQ(1u, errors.size()) << errors; + EXPECT_EQ(sdf::ErrorCode::WARNING, errors[0].Code()) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "Inertial was used with auto=true for the link named " + "compound_link, but user-defined inertial values were " + "found, which will be overwritten by the computed inertial " + "values")) << errors; + EXPECT_NE(nullptr, root.Element()); + } + + // Parse again with default enforcement policy and expect no warnings. sdf::Root root; const sdf::ParserConfig sdfParserConfig; sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); @@ -763,12 +805,276 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) root.ResolveAutoInertials(errors, sdfParserConfig); EXPECT_TRUE(errors.empty()); - EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(2.0, link->Inertial().MassMatrix().Mass()); EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose()); EXPECT_EQ(gz::math::Vector3d(0.33333, 0.33333, 0.33333), link->Inertial().MassMatrix().DiagonalMoments()); } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithMass) +{ + // A model with link inertial auto set to true. + // The inertia matrix is specified but should be ignored. + // is specified - the auto computed inertial values should + // be scaled based on the desired mass. + std::string sdf = "" + "" + " " + " " + " " + " 4.0" + " 1 1 1 2 2 2" + " " + " 1" + " 1" + " 1" + " " + " " + " " + " 2.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + + EXPECT_DOUBLE_EQ(4.0, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose()); + EXPECT_EQ(gz::math::Vector3d(0.66666, 0.66666, 0.66666), + link->Inertial().MassMatrix().DiagonalMoments()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithMassAndMultipleCollisions) +{ + // A model with link inertial auto set to true. + // The inertia matrix is specified but should be ignored. + // is specified - the auto computed inertial values should + // be scaled based on the desired mass. + // The model contains two collisions with different sizes and densities + // and a lumped center of mass at the link origin. + std::string sdf = "" + "" + " " + " " + " " + " 12.0" + " 1 1 1 2 2 2" + " " + " 1" + " 1" + " 1" + " " + " " + " " + " 0.0 0.0 0.5 0 0 0" + " 4.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " 0.0 0.0 -1.0 0 0 0" + " 1.0" + " " + " " + " 1 1 2" + " " + " " + " " + " " + " " + ""; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + + // Expect mass value from //inertial/mass + EXPECT_DOUBLE_EQ(12.0, link->Inertial().MassMatrix().Mass()); + + // Inertial values based on density before scaling to match specified mass + // + // Collision geometries: + // + // --------- z = 1 + // | | + // | c | cube on top, side length 1.0, density 4.0 + // | | + // |-------| z = 0 + // | | + // | | + // | | + // | c | box on bottom, size 1x1x2, density 1.0 + // | | + // | | + // | | + // --------- z = -2 + // + // Top cube: volume = 1.0, mass = 4.0, center of mass z = 0.5 + // Bottom box: volume = 2.0, mass = 2.0, center of mass z = -1.0 + // + // Total mass from density: 6.0 + // Lumped center of mass z = (4.0 * 0.5 + 2.0 * (-1.0)) / 6.0 = 0.0 + EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose()); + + // Moment of inertia at center of each shape + // Top cube: Ixx = Iyy = Izz = 4.0 / 12 * (1*1 + 1*1) = 8/12 = 2/3 + // Bottom box: Ixx = Iyy = 2.0 / 12 * (1*1 + 2*2) = 10/12 = 5/6 + // Izz = 2.0 / 12 * (1*1 + 1*1) = 4/12 = 1/3 + // + // Lumped moment of inertia at lumped center of mass + // Ixx = sum(Ixx_i + mass_i * center_of_mass_z_i^2) for i in [top, bottom] + // Iyy = Ixx + // Izz = Izz_top + Izz_bottom + // + // Ixx = Iyy = (2/3 + 4.0 * 0.5 * 0.5) + (5/6 + 2.0 * (-1.0) * (-1.0)) + // = (2/3 + 1) + (5/6 + 2.0) + // = 5/3 + 17/6 + // = 27/6 = 9/2 = 4.5 + // + // Izz = 2/3 + 1/3 = 1.0 + + // Then scale the inertias according to the specified mass of 12.0 + // mass_ratio = 12.0 / 6.0 = 2.0 + // Ixx = Iyy = mass_ratio * Ixx_unscaled = 2.0 * 4.5 = 9.0 + // Izz = mass_ratio * Izz_unscaled = 2.0 * 1.0 = 2.0 + EXPECT_EQ(gz::math::Vector3d(9.0, 9.0, 2.0), + link->Inertial().MassMatrix().DiagonalMoments()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithMassAndDefaultDensity) +{ + // A model with link inertial auto set to true. + // is specified - the auto computed inertial values should + // be scaled based on the desired mass. + // The model contains two collisions with different sizes. Density + // is specified for the top collision but not the bottom collision. + // There should be no parser warnings. + // The model should have a lumped center of mass at the link origin. + std::string sdf = "" + "" + " " + " " + " " + " 12000.0" + " " + " " + " 0.0 0.0 0.5 0 0 0" + " 4000" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " 0.0 0.0 -1.0 0 0 0" + " " + " " + " 1 1 2" + " " + " " + " " + " " + " " + ""; + + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + // Set enforcement policy to ERR so we can detect warnings in sdf::Errors. + sdfParserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + // Expect no warnings due to user-specified inertial values when using + // inertial auto=true. + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + + // Expect mass value from //inertial/mass + EXPECT_DOUBLE_EQ(12000.0, link->Inertial().MassMatrix().Mass()); + + // Inertial values based on density before scaling to match specified mass + // + // Collision geometries: + // + // --------- z = 1 + // | | + // | c | cube on top, side length 1.0, density 4000.0 + // | | + // |-------| z = 0 + // | | + // | | + // | | + // | c | box on bottom, size 1x1x2, default density 1000.0 + // | | + // | | + // | | + // --------- z = -2 + // + // Top cube: volume = 1.0, mass = 4000.0, center of mass z = 0.5 + // Bottom box: volume = 2.0, mass = 2000.0, center of mass z = -1.0 + // + // Total mass from density: 6000.0 + // Lumped center of mass z = (4000.0 * 0.5 + 2000.0 * (-1.0)) / 6000.0 = 0.0 + EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose()); + + // Moment of inertia at center of each shape + // Top cube: Ixx = Iyy = Izz = 4000.0 / 12 * (1*1 + 1*1) = 8000/12 + // Bottom box: Ixx = Iyy = 2000.0 / 12 * (1*1 + 2*2) = 10000/12 + // Izz = 2000.0 / 12 * (1*1 + 1*1) = 4000/12 + // + // Lumped moment of inertia at lumped center of mass + // Ixx = sum(Ixx_i + mass_i * center_of_mass_z_i^2) for i in [top, bottom] + // Iyy = Ixx + // Izz = Izz_top + Izz_bottom + // + // Ixx = Iyy = (8000/12 + 4000.0 * 0.5 * 0.5) + (10000/12 + 2000.0 + // * (-1.0) * (-1.0)) + // = (8000/12 + 1000) + (10000/12 + 2000.0) + // = 20000/12 + 34000/12 + // = 54000/12 = 9000/2 = 4500 + // + // Izz = 8000/12 + 4000/12 = 1000.0 + + // Then scale the inertias according to the specified mass of 12000.0 + // mass_ratio = 12000.0 / 6000.0 = 2.0 + // Ixx = Iyy = mass_ratio * Ixx_unscaled = 2.0 * 4500.0 = 9000.0 + // Izz = mass_ratio * Izz_unscaled = 2.0 * 1000.0 = 2000.0 + EXPECT_EQ(gz::math::Vector3d(9000.0, 9000.0, 2000.0), + link->Inertial().MassMatrix().DiagonalMoments()); +} + ///////////////////////////////////////////////// TEST(DOMLink, ResolveAutoInertialsCalledWithAutoFalse) { diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index dc54a45d2..864de685e 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -2021,6 +2021,19 @@ TEST(inertial_stats, SDF) EXPECT_EQ(expectedOutput, output); } + // Check a good SDF file with auto-inertials and explicit mass + // from the same folder by passing a relative path + { + std::string path = "inertial_stats_auto_mass.sdf"; + const auto pathBase = sdf::testing::TestFile("sdf"); + + std::string output = + custom_exec_str("cd " + pathBase + " && " + + GzCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } + expectedOutput = "Error Code " + std::to_string(static_cast( diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 6d875b223..0a3b2add7 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -948,7 +948,7 @@ TEST(DOMLink, InertialAuto) const sdf::Link *link = model->LinkByName("link_1"); ASSERT_NE(link, nullptr); - // Verify inertial values for link_1 match thos in inertial_stats.sdf + // Verify inertial values for link_1 match those in inertial_stats.sdf gz::math::Inertiald inertial = link->Inertial(); gz::math::MassMatrix3d massMatrix = inertial.MassMatrix(); EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose()); @@ -969,6 +969,40 @@ TEST(DOMLink, InertialAuto) EXPECT_FALSE(inertialElem->HasElement("pose")); } +///////////////////////////////////////////////// +TEST(DOMLink, InertialAutoExplicitMass) +{ + const std::string testFile = sdf::testing::TestFile("sdf", + "inertial_stats_auto_mass.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.Model(); + ASSERT_NE(model, nullptr); + + std::vector linkNames{"link_1", "link_2", "link_3", "link_4"}; + for (const std::string &linkName : linkNames) + { + const sdf::Link *link = model->LinkByName(linkName); + ASSERT_NE(link, nullptr); + + // Verify inertial values for link_i match those in inertial_stats.sdf + gz::math::Inertiald inertial = link->Inertial(); + gz::math::MassMatrix3d massMatrix = inertial.MassMatrix(); + EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose()); + EXPECT_DOUBLE_EQ(6.0, massMatrix.Mass()); + EXPECT_DOUBLE_EQ(1.0, massMatrix.Ixx()); + EXPECT_DOUBLE_EQ(1.0, massMatrix.Iyy()); + EXPECT_DOUBLE_EQ(1.0, massMatrix.Izz()); + EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixy()); + EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixz()); + EXPECT_DOUBLE_EQ(0.0, massMatrix.Iyz()); + } +} + ///////////////////////////////////////////////// TEST(DOMLink, InertialAutoSaveInElement) { @@ -990,7 +1024,7 @@ TEST(DOMLink, InertialAutoSaveInElement) const sdf::Link *link = model->LinkByName("link_1"); ASSERT_NE(link, nullptr); - // Verify inertial values for link_1 match thos in inertial_stats.sdf + // Verify inertial values for link_1 match those in inertial_stats.sdf gz::math::Inertiald inertial = link->Inertial(); gz::math::MassMatrix3d massMatrix = inertial.MassMatrix(); EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose()); diff --git a/test/sdf/inertial_stats_auto_mass.sdf b/test/sdf/inertial_stats_auto_mass.sdf new file mode 100644 index 000000000..b2b2c0381 --- /dev/null +++ b/test/sdf/inertial_stats_auto_mass.sdf @@ -0,0 +1,140 @@ + + + + + + 0 0 0 0 0 0 + + + + 5 0 0 0 0 0 + + 6 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + -5 0 0 0 0 0 + + 6 + + + 1000 + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + 0 5 0 0 0 0 + + 6 + + + 0 0 0.25 0 0 0 + 1000 + + + 1 1 0.5 + + + + + 0 0 -0.25 0 0 0 + 1000 + + + 1 1 0.5 + + + + + + + 1 1 1 + + + + + + + + + 0 -5 0 0 0 0 + + 6 + + + 0 0 0.25 0 0 0 + 123.456 + + + 1 1 0.5 + + + + + 0 0 -0.25 0 0 0 + 123.456 + + + 1 1 0.5 + + + + + + + 1 1 1 + + + + + + +