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 + + + + + + +