diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index a2c346c2d..977aab5d0 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -724,6 +724,53 @@ TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) link->Inertial().MassMatrix().DiagonalMoments()); } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDefaultDensity) +{ + std::string sdf = "" + "" + " " + " 0 0 1.0 0 0 0" + " " + " " + " " + " 0 0 -0.5 0 0 0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + // Set enforcement policy to store warnings as error. + // Because density is missing so we expect that a warning is generated about + // using the default density when resolving auto inertia. + sdfParserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_FALSE(errors.empty()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + + // The collision should use the default density value of 1000 + // Mass of cube(volume * density) + double expectedMass = 1.0 * 1000.0; + + EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(166.667, 166.667, 166.667), + link->Inertial().MassMatrix().DiagonalMoments()); +} + ///////////////////////////////////////////////// TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) {