diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index c3ee3042f..216b0ac20 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -457,6 +457,204 @@ TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity) } } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDifferentAutoInertiaParams) +{ + std::string sdf = "" + "" + " " + " 0 0 1.0 0 0 0" + " " + " 0 1.0 0 0 0 0" + " " + " " + " 12" + " 1 1 1" + " " + " " + " " + " 1.0 0 0 0 0 0" + " " + " " + " uri" + " " + " " + " " + " " + " " + " 0 2.0 0 0 0 0" + " " + " " + " 2.0 0 0 0 0 0" + " " + " 24" + " 1 1 1" + " " + " " + " " + " uri" + " " + " " + " " + " " + " " + " 0 3.0 0 0 0 0" + " " + " " + " 12" + " 1 1 1" + " " + " " + " " + " 3.0 0 0 0 0 0" + " " + " 24" + " 1 1 1" + " " + " " + " " + " uri" + " " + " " + " " + " " + " " + " 0 4.0 0 0 0 0" + " " + " " + " 4.0 0 0 0 0 0" + " " + " " + " uri" + " " + " " + " " + " " + " " + ""; + + // Lambda function for registering as custom inertia calculator + auto customMeshInertiaCalculator = []( + sdf::Errors &_errors, + const sdf::CustomInertiaCalcProperties &_inertiaProps + ) -> std::optional + { + auto autoInertiaParams = _inertiaProps.AutoInertiaParams(); + if (!autoInertiaParams || + !autoInertiaParams->HasElement("gz:density") || + !autoInertiaParams->HasElement("gz:box_size")) + { + // return default inertial values + gz::math::Inertiald meshInertial; + + meshInertial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + + return meshInertial; + } + + gz::math::Inertiald meshInerial; + + double gzDensity = autoInertiaParams->Get("gz:density"); + gz::math::Vector3d gzBoxSize = + autoInertiaParams->Get("gz:box_size"); + gz::math::Material material = gz::math::Material(gzDensity); + + gz::math::MassMatrix3d massMatrix; + massMatrix.SetFromBox(gz::math::Material(gzDensity), gzBoxSize); + + meshInerial.SetMassMatrix(massMatrix); + + return meshInerial; + }; + + + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + sdfParserConfig.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ(4u, model->LinkCount()); + + // ResolveAutoInertials should run by default during Root::Load. + + { + const sdf::Link *link = model->LinkByName("link_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //inertial/auto_inertia_params + // * gz:density == 12 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(2, 2, 2), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = model->LinkByName("collision_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //collision/auto_inertia_params + // * gz:density == 24 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(2, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = + model->LinkByName("collision_auto_inertia_params_overrides"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //collision/auto_inertia_params + // * gz:density == 24 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(3, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = + model->LinkByName("default_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify default inertial values + auto inertial = link->Inertial(); + EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(1, 1, 1), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(4, 0, 0, 0, 0, 0), inertial.Pose()); + } +} + ///////////////////////////////////////////////// TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) {