diff --git a/src/Geometry.cc b/src/Geometry.cc index ab169905d..9afb20212 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -312,6 +312,7 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) this->dataPtr->polylines = _polylines; } +///////////////////////////////////////////////// std::optional Geometry::CalculateInertial( sdf::Errors &_errors, const ParserConfig &_config, double _density, sdf::ElementPtr _autoInertiaParams) diff --git a/src/Link.cc b/src/Link.cc index d56792a06..40a64012f 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -191,9 +191,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { Error err( ErrorCode::WARNING, - "Inertial was used with auto=true for link, " + - this->dataPtr->name + " but user defined inertial values " - "were found. They would be overwritten by computed ones." + "Inertial was used with auto=true for the link named " + + this->dataPtr->name + ", but user-defined inertial values were " + "found, which will be overwritten by the computed inertial values." ); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), err, errors); @@ -610,7 +610,8 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, { _errors.push_back({ErrorCode::ELEMENT_MISSING, "Inertial is set to auto but there are no " - " elements for the link."}); + " elements for the link named " + + this->Name() + "."}); return; } diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 524d003f5..5e0968b75 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -293,18 +293,13 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) sdf::Root root; const sdf::ParserConfig sdfParserConfig; sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); - EXPECT_TRUE(errors.empty()); + ASSERT_EQ(1u, errors.size()) << errors; + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "Inertial is set to auto but there are no elements " + "for the link named link.")); EXPECT_NE(nullptr, root.Element()); - - const sdf::Model *model = root.Model(); - const sdf::Link *link = model->LinkByIndex(0); - root.ResolveAutoInertials(errors, sdfParserConfig); - ASSERT_FALSE(errors.empty()); - - // Default Inertial values set during load - EXPECT_EQ(1.0, link->Inertial().MassMatrix().Mass()); - EXPECT_EQ(gz::math::Vector3d::One, - link->Inertial().MassMatrix().DiagonalMoments()); } ///////////////////////////////////////////////// diff --git a/src/Mesh.cc b/src/Mesh.cc index 5e598345b..a3d4d1274 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -209,6 +209,22 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, const auto &customCalculator = _config.CustomInertiaCalc(); + if (!customCalculator) + { + Error err( + sdf::ErrorCode::WARNING, + "Custom moment of inertia calculator for meshes not set via " + "sdf::ParserConfig::RegisterCustomInertiaCalc, using default " + "inertial values."); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, _errors); + + using namespace gz::math; + return Inertiald( + MassMatrix3d(1, Vector3d::One, Vector3d::Zero), + Pose3d::Zero); + } + sdf::CustomInertiaCalcProperties calcInterface = CustomInertiaCalcProperties( _density, *this, _autoInertiaParams); diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index 209de15c5..32211f812 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -45,10 +45,10 @@ class sdf::ParserConfig::Implementation public: std::optional deprecatedElementsPolicy; /// \brief Configuration that is set for the CalculateInertial() function - /// By default it is set to SKIP_CALCULATION_IN_LOAD which would cause - /// Root::Load() to not call CalculateInertial() + /// By default it is set to SAVE_CALCULATION to preserve the behavior of + /// Root::Load() generating complete inertial information. public: ConfigureResolveAutoInertials resolveAutoInertialsConfig = - ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD; + ConfigureResolveAutoInertials::SAVE_CALCULATION; /// \brief Collection of custom model parsers. public: std::vector customParsers; diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 65719e4ef..6d1e681a9 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -57,11 +57,11 @@ TEST(ParserConfig, Construction) EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy()); - EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD, + EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION, config.CalculateInertialConfiguration()); config.SetCalculateInertialConfiguration( - sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); - EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION, + sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); + EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD, config.CalculateInertialConfiguration()); EXPECT_FALSE(config.URDFPreserveFixedJoint()); EXPECT_FALSE(config.StoreResolvedURIs()); diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index a7c5cacdd..d656f2efe 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -1889,7 +1889,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file from the same folder by passing a relative path { - std::string path = "inertial_stats.sdf"; + std::string path = "inertial_stats_auto.sdf"; std::string output = custom_exec_str("cd " + pathBase + " && " + diff --git a/test/integration/root_dom.cc b/test/integration/root_dom.cc index 69d682dd8..3b5d7c0ca 100644 --- a/test/integration/root_dom.cc +++ b/test/integration/root_dom.cc @@ -300,3 +300,13 @@ TEST(DOMRoot, CreateMulipleWorlds) testFunc(root); testFunc(root2); } + +///////////////////////////////////////////////// +TEST(DOMRoot, LoadWithoutMeshAutoInertialCalculator) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "mesh_auto_inertial.sdf"); + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; +} diff --git a/test/sdf/inertial_stats.sdf b/test/sdf/inertial_stats.sdf index e2504807f..a708f6300 100644 --- a/test/sdf/inertial_stats.sdf +++ b/test/sdf/inertial_stats.sdf @@ -1,25 +1,25 @@ - - + + 0 0 0 0 0 0 diff --git a/test/sdf/inertial_stats_auto.sdf b/test/sdf/inertial_stats_auto.sdf new file mode 100644 index 000000000..010313b5d --- /dev/null +++ b/test/sdf/inertial_stats_auto.sdf @@ -0,0 +1,107 @@ + + + + + + 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 + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + 0 5 0 0 0 0 + + + 6 + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + 0 -5 0 0 0 0 + + + 6 + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + diff --git a/test/sdf/mesh_auto_inertial.sdf b/test/sdf/mesh_auto_inertial.sdf new file mode 100644 index 000000000..ece976a41 --- /dev/null +++ b/test/sdf/mesh_auto_inertial.sdf @@ -0,0 +1,17 @@ + + + + + + + + + + box.dae + + + + + + +