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)
{