From ed6f2d5714c9bde0746564f7d41878d8f96faf2d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 7 Oct 2023 00:26:03 -0700 Subject: [PATCH 01/11] Use //link/inertial/density for auto-inertials The density precedence for Collisions is now: 1. Density explicitly set in //collision/density or by Collision::SetDensity. 2. Density explicitly set in //link/inertial/density or by Link::SetDensity. 3. Default density in Collision::DensityDefault(). Signed-off-by: Steve Peters --- include/sdf/Collision.hh | 23 ++++- include/sdf/Link.hh | 10 ++ src/Collision.cc | 57 +++++++++--- src/Collision_TEST.cc | 1 + src/Link.cc | 24 ++++- src/Link_TEST.cc | 151 +++++++++++++++++++++++++++++++ test/sdf/inertial_stats_auto.sdf | 5 +- 7 files changed, 255 insertions(+), 16 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index d6b8470c9..6667896d6 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -18,6 +18,7 @@ #define SDF_COLLISION_HH_ #include +#include #include #include #include @@ -78,6 +79,11 @@ namespace sdf /// \param[in] _name Name of the collision. public: void SetName(const std::string &_name); + /// \brief Get the default density of a collision if its density is not + /// specified. + /// \return Default density. + public: static double DensityDefault(); + /// \brief Get the density of the collision. /// \return Density of the collision. public: double Density() const; @@ -145,13 +151,28 @@ namespace sdf /// \brief Calculate and return the MassMatrix for the collision /// \param[out] _errors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors - /// \param[in] _config Custom parser configuration /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values + /// \param[in] _config Custom parser configuration public: void CalculateInertial(sdf::Errors &_errors, gz::math::Inertiald &_inertial, const ParserConfig &_config); + /// \brief Calculate and return the MassMatrix for the collision + /// \param[out] _errors A vector of Errors objects. Each errors contains an + /// Error code and a message. An empty errors vector indicates no errors + /// \param[out] _inertial An inertial object which will be set with the + /// calculated inertial values + /// \param[in] _config Custom parser configuration + /// \param[in] _density An optional density value to override the default + /// collision density. This value is used instead of DefaultDensity() + // if this collision's density has not been explicitly set. + public: void CalculateInertial( + sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config, + const std::optional &_density); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index c92d8d18d..00a65b080 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -18,6 +18,7 @@ #define SDF_LINK_HH_ #include +#include #include #include #include @@ -79,6 +80,15 @@ namespace sdf /// \param[in] _name Name of the link. public: void SetName(const std::string &_name); + /// \brief Get the density of the inertial if it has been set. + /// \return Density of the inertial if it has been set, + /// otherwise std::nullopt. + public: std::optional Density() const; + + /// \brief Set the density of the inertial. + /// \param[in] _density Density of the inertial. + public: void SetDensity(double _density); + /// \brief Get the number of visuals. /// \return Number of visuals contained in this Link object. public: uint64_t VisualCount() const; diff --git a/src/Collision.cc b/src/Collision.cc index c2b2a15ec..cf03c5651 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -49,11 +49,8 @@ class sdf::Collision::Implementation /// \brief The collision's surface parameters. public: sdf::Surface surface; - /// \brief Density of the collision. Default is 1000.0 - public: double density{1000.0}; - - /// \brief True if density was set during load from sdf. - public: bool densitySetAtLoad = false; + /// \brief Density of the collision if it has been set. + public: std::optional density; /// \brief SDF element pointer to tag public: sdf::ElementPtr autoInertiaParams{nullptr}; @@ -130,7 +127,6 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("density")) { this->dataPtr->density = _sdf->Get("density"); - this->dataPtr->densitySetAtLoad = true; } // Load the auto_inertia_params element @@ -155,10 +151,20 @@ void Collision::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +double Collision::DensityDefault() +{ + return 1000.0; +} + ///////////////////////////////////////////////// double Collision::Density() const { - return this->dataPtr->density; + if (this->dataPtr->density) + { + return *this->dataPtr->density; + } + return DensityDefault(); } ///////////////////////////////////////////////// @@ -256,14 +262,41 @@ void Collision::CalculateInertial( gz::math::Inertiald &_inertial, const ParserConfig &_config) { - // Check if density was not set during load & send a warning - // about the default value being used - if (!this->dataPtr->densitySetAtLoad) + this->CalculateInertial(_errors, _inertial, _config, std::nullopt); +} + +///////////////////////////////////////////////// +void Collision::CalculateInertial( + sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config, + const std::optional &_density) +{ + // Order of precedence for density: + double density; + // 1. Density explicitly set in this collision, either from the + // `//collision/density` element or from Collision::SetDensity. + if (this->dataPtr->density) + { + density = *this->dataPtr->density; + } + // 2. Density passed into this function, which likely comes from the + // `//link/inertial/density` element or from Link::SetDensity. + else if (_density) + { + density = *_density; + } + // 3. DensityDefault value. + else { + // If density was not explicitly set, send a warning + // about the default value being used + density = DensityDefault(); Error densityMissingErr( ErrorCode::ELEMENT_MISSING, "Collision is missing a child element. " - "Using a default density value of 1000.0 kg/m^3. " + "Using a default density value of " + + std::to_string(DensityDefault()) + " kg/m^3. " ); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), densityMissingErr, _errors @@ -272,7 +305,7 @@ void Collision::CalculateInertial( auto geomInertial = this->dataPtr->geom.CalculateInertial(_errors, _config, - this->dataPtr->density, this->dataPtr->autoInertiaParams); + density, this->dataPtr->autoInertiaParams); if (!geomInertial) { diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 07f894a58..1f19f2acb 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -38,6 +38,7 @@ TEST(DOMcollision, Construction) EXPECT_EQ(nullptr, collision.Element()); EXPECT_TRUE(collision.Name().empty()); EXPECT_EQ(collision.Density(), 1000.0); + EXPECT_EQ(collision.DensityDefault(), 1000.0); collision.SetName("test_collison"); EXPECT_EQ(collision.Name(), "test_collison"); diff --git a/src/Link.cc b/src/Link.cc index 40a64012f..c3d772dd5 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -69,6 +69,10 @@ class sdf::Link::Implementation /// \brief The projectors specified in this link. public: std::vector projectors; + /// \brief Density of the inertial which will be used for auto-inertial + /// calculations if the collision density has not been set. + public: std::optional density; + /// \brief The inertial information for this link. public: gz::math::Inertiald inertial {{1.0, gz::math::Vector3d::One, gz::math::Vector3d::Zero}, @@ -180,6 +184,11 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); + if (inertialElem->HasElement("density")) + { + this->dataPtr->density = inertialElem->Get("density"); + } + if (inertialElem->Get("auto")) { this->dataPtr->autoInertia = true; @@ -309,6 +318,18 @@ void Link::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +std::optional Link::Density() const +{ + return this->dataPtr->density; +} + +///////////////////////////////////////////////// +void Link::SetDensity(double _density) +{ + this->dataPtr->density = _density; +} + ///////////////////////////////////////////////// uint64_t Link::VisualCount() const { @@ -620,7 +641,8 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - collision.CalculateInertial(_errors, collisionInertia, _config); + collision.CalculateInertial(_errors, collisionInertia, _config, + this->dataPtr->density); totalInertia = totalInertia + collisionInertia; } diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 5e0968b75..656d72491 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -125,6 +125,8 @@ TEST(DOMLink, Construction) EXPECT_EQ(inertial.BodyMatrix(), inertial.SpatialMatrix()); EXPECT_TRUE(inertial.MassMatrix().IsValid()); + EXPECT_FALSE(link.Density().has_value()); + EXPECT_EQ(0u, link.CollisionCount()); EXPECT_EQ(nullptr, link.CollisionByIndex(0)); EXPECT_EQ(nullptr, link.CollisionByIndex(1)); @@ -302,6 +304,155 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) EXPECT_NE(nullptr, root.Element()); } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity) +{ + std::string sdf = "" + "" + " " + " 0 0 1.0 0 0 0" + " " + " 0 1.0 0 0 0 0" + " " + " 12.0" + " " + " " + " 1.0 0 0 0 0 0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + " 0 2.0 0 0 0 0" + " " + " " + " 1.0 0 0 0 0 0" + " 24.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + " 0 3.0 0 0 0 0" + " " + " 12.0" + " " + " " + " 1.0 0 0 0 0 0" + " 24.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + " 0 4.0 0 0 0 0" + " " + " " + " 1.0 0 0 0 0 0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + ""; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + 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_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + ASSERT_TRUE(linkDensity.has_value()); + EXPECT_DOUBLE_EQ(12.0, *linkDensity); + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // 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()); + } + + { + const sdf::Link *link = model->LinkByName("collision_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + EXPECT_FALSE(linkDensity.has_value()); + // assume Collision density is properly set to 24 + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // 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()); + } + + { + const sdf::Link *link = + model->LinkByName("collision_density_overrides_link_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + ASSERT_TRUE(linkDensity.has_value()); + EXPECT_DOUBLE_EQ(12.0, *linkDensity); + // assume Collision density is properly set to 24 and overrides the link + // density + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // 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()); + } + + { + const sdf::Link *link = + model->LinkByName("default_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + EXPECT_FALSE(linkDensity.has_value()); + // assume density is the default value of 1000.0 + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(1000.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d::One * 500.0 / 3.0, + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } +} + ///////////////////////////////////////////////// TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) { diff --git a/test/sdf/inertial_stats_auto.sdf b/test/sdf/inertial_stats_auto.sdf index 010313b5d..239b9a340 100644 --- a/test/sdf/inertial_stats_auto.sdf +++ b/test/sdf/inertial_stats_auto.sdf @@ -25,9 +25,10 @@ 5 0 0 0 0 0 - - + 6 + + 1 1 1 From 13161514a0de89736872d444dd841a84815c1596 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 7 Oct 2023 22:58:11 -0700 Subject: [PATCH 02/11] Improve code coverage Signed-off-by: Steve Peters --- src/Link_TEST.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 656d72491..c3ee3042f 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -126,6 +126,10 @@ TEST(DOMLink, Construction) EXPECT_TRUE(inertial.MassMatrix().IsValid()); EXPECT_FALSE(link.Density().has_value()); + const double density = 123.0; + link.SetDensity(density); + ASSERT_TRUE(link.Density().has_value()); + EXPECT_DOUBLE_EQ(density, *link.Density()); EXPECT_EQ(0u, link.CollisionCount()); EXPECT_EQ(nullptr, link.CollisionByIndex(0)); From 865dd08dd77d13d316d6728a8b17885e7998e1d8 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 28 Oct 2023 01:35:48 -0700 Subject: [PATCH 03/11] Collision_TEST: fix variable name, comment Signed-off-by: Steve Peters --- src/Collision.cc | 2 +- src/Collision_TEST.cc | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index cf03c5651..7c29b7657 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -52,7 +52,7 @@ class sdf::Collision::Implementation /// \brief Density of the collision if it has been set. public: std::optional density; - /// \brief SDF element pointer to tag + /// \brief SDF element pointer to tag public: sdf::ElementPtr autoInertiaParams{nullptr}; /// \brief The SDF element pointer used during load. diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 1f19f2acb..c6076c859 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -59,12 +59,12 @@ TEST(DOMcollision, Construction) EXPECT_DOUBLE_EQ(collision.Density(), 1240.0); EXPECT_EQ(collision.AutoInertiaParams(), nullptr); - sdf::ElementPtr autoInertialParamsElem(new sdf::Element()); - autoInertialParamsElem->SetName("auto_inertial_params"); - collision.SetAutoInertiaParams(autoInertialParamsElem); - EXPECT_EQ(collision.AutoInertiaParams(), autoInertialParamsElem); + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + autoInertiaParamsElem->SetName("auto_inertia_params"); + collision.SetAutoInertiaParams(autoInertiaParamsElem); + EXPECT_EQ(collision.AutoInertiaParams(), autoInertiaParamsElem); EXPECT_EQ(collision.AutoInertiaParams()->GetName(), - autoInertialParamsElem->GetName()); + autoInertiaParamsElem->GetName()); collision.SetRawPose({-10, -20, -30, GZ_PI, GZ_PI, GZ_PI}); EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, GZ_PI, GZ_PI, GZ_PI), From 67bf92b0cbdfb2937b110f44903e504adc96a1a4 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 28 Oct 2023 01:39:17 -0700 Subject: [PATCH 04/11] Add Link auto_inertia_params to API Pass //link/inertial/auto_inertia_params ElementPtr to Collision::CalculateInertial and use it if the Collision does not have auto_inertia_params of its own. Signed-off-by: Steve Peters --- include/sdf/Collision.hh | 6 +++++- include/sdf/Link.hh | 12 ++++++++++++ src/Collision.cc | 15 ++++++++++++--- src/Link.cc | 25 ++++++++++++++++++++++++- 4 files changed, 53 insertions(+), 5 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 6667896d6..2ab4114ce 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -167,11 +167,15 @@ namespace sdf /// \param[in] _density An optional density value to override the default /// collision density. This value is used instead of DefaultDensity() // if this collision's density has not been explicitly set. + /// \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. public: void CalculateInertial( sdf::Errors &_errors, gz::math::Inertiald &_inertial, const ParserConfig &_config, - const std::optional &_density); + const std::optional &_density, + sdf::ElementPtr _autoInertiaParams); /// \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 00a65b080..3e5260e73 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -89,6 +89,18 @@ namespace sdf /// \param[in] _density Density of the inertial. public: void SetDensity(double _density); + /// \brief Get the ElementPtr to the element + /// This element can be used as a parent element to hold user-defined + /// params for the custom moment of inertia calculator. + /// \return ElementPtr object for the element. + public: sdf::ElementPtr AutoInertiaParams() const; + + /// \brief Function to set the auto inertia params using a + /// sdf ElementPtr object + /// \param[in] _autoInertiaParams ElementPtr to + /// element + public: void SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams); + /// \brief Get the number of visuals. /// \return Number of visuals contained in this Link object. public: uint64_t VisualCount() const; diff --git a/src/Collision.cc b/src/Collision.cc index 7c29b7657..05ca351bf 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -262,7 +262,8 @@ void Collision::CalculateInertial( gz::math::Inertiald &_inertial, const ParserConfig &_config) { - this->CalculateInertial(_errors, _inertial, _config, std::nullopt); + this->CalculateInertial( + _errors, _inertial, _config, std::nullopt, ElementPtr()); } ///////////////////////////////////////////////// @@ -270,7 +271,8 @@ void Collision::CalculateInertial( sdf::Errors &_errors, gz::math::Inertiald &_inertial, const ParserConfig &_config, - const std::optional &_density) + const std::optional &_density, + sdf::ElementPtr _autoInertiaParams) { // Order of precedence for density: double density; @@ -303,9 +305,16 @@ void Collision::CalculateInertial( ); } + // If this Collision's auto inertia params have not been set, then use the + // params passed into this function. + sdf::ElementPtr autoInertiaParams = this->dataPtr->autoInertiaParams; + if (!autoInertiaParams) + { + autoInertiaParams = _autoInertiaParams; + } auto geomInertial = this->dataPtr->geom.CalculateInertial(_errors, _config, - density, this->dataPtr->autoInertiaParams); + density, autoInertiaParams); if (!geomInertial) { diff --git a/src/Link.cc b/src/Link.cc index c3d772dd5..659b25cbe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -73,6 +73,9 @@ class sdf::Link::Implementation /// calculations if the collision density has not been set. public: std::optional density; + /// \brief SDF element pointer to tag + public: sdf::ElementPtr autoInertiaParams{nullptr}; + /// \brief The inertial information for this link. public: gz::math::Inertiald inertial {{1.0, gz::math::Vector3d::One, gz::math::Vector3d::Zero}, @@ -189,6 +192,13 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->density = inertialElem->Get("density"); } + // Load the auto_inertia_params element + if (inertialElem->HasElement("auto_inertia_params")) + { + this->dataPtr->autoInertiaParams = + inertialElem->GetElement("auto_inertia_params"); + } + if (inertialElem->Get("auto")) { this->dataPtr->autoInertia = true; @@ -330,6 +340,18 @@ void Link::SetDensity(double _density) this->dataPtr->density = _density; } +///////////////////////////////////////////////// +sdf::ElementPtr Link::AutoInertiaParams() const +{ + return this->dataPtr->autoInertiaParams; +} + +///////////////////////////////////////////////// +void Link::SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams) +{ + this->dataPtr->autoInertiaParams = _autoInertiaParams; +} + ///////////////////////////////////////////////// uint64_t Link::VisualCount() const { @@ -642,7 +664,8 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, { gz::math::Inertiald collisionInertia; collision.CalculateInertial(_errors, collisionInertia, _config, - this->dataPtr->density); + this->dataPtr->density, + this->dataPtr->autoInertiaParams); totalInertia = totalInertia + collisionInertia; } From 7a308a6c1efd69da61354aa5cefa319baf69714a Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 12 Dec 2023 00:00:06 -0800 Subject: [PATCH 05/11] Mesh: don't warn if FilePath is not set The FilePath refers to the SDFormat file that the Mesh object was parsed from, not the Mesh file itself, so remove the unrelated warning. Signed-off-by: Steve Peters --- src/Mesh.cc | 8 -------- src/Mesh_TEST.cc | 3 --- 2 files changed, 11 deletions(-) diff --git a/src/Mesh.cc b/src/Mesh.cc index a3d4d1274..fa180b2bb 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -199,14 +199,6 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, double _density, const sdf::ElementPtr _autoInertiaParams, const ParserConfig &_config) { - if (this->dataPtr->filePath.empty()) - { - _errors.push_back({ - sdf::ErrorCode::WARNING, - "File Path for the mesh was empty. Could not calculate inertia"}); - return std::nullopt; - } - const auto &customCalculator = _config.CustomInertiaCalc(); if (!customCalculator) diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 7a8c4c4ca..51c1751f4 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -201,9 +201,6 @@ TEST(DOMMesh, CalcualteInertial) double density = 0; sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); - // File Path needs to be considered as a valid mesh - mesh.SetFilePath("/some_path"); - // Test Lambda function for registering as custom inertia calculator auto customMeshInertiaCalculator = []( sdf::Errors &_errors, From 4eae4b8cf2a96512492a80acbbc4e4c9de3f255b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 12 Dec 2023 00:02:16 -0800 Subject: [PATCH 06/11] Add test for precedence of AutoInertiaParams Signed-off-by: Steve Peters --- src/Link_TEST.cc | 198 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 198 insertions(+) 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) { From 756ec5409d97a25108a6a09964c223f8cbf2a2f6 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 12 Dec 2023 13:25:31 -0800 Subject: [PATCH 07/11] Fix compiler warning Signed-off-by: Steve Peters --- src/Link_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 216b0ac20..a4fb29690 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -535,7 +535,7 @@ TEST(DOMLink, ResolveAutoInertialsWithDifferentAutoInertiaParams) // Lambda function for registering as custom inertia calculator auto customMeshInertiaCalculator = []( - sdf::Errors &_errors, + sdf::Errors &, const sdf::CustomInertiaCalcProperties &_inertiaProps ) -> std::optional { From 359875e766fcf87fbb68fc562c222bd82fc7ecd7 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 12 Dec 2023 20:11:59 -0800 Subject: [PATCH 08/11] Improve code coverage Signed-off-by: Steve Peters --- src/Link_TEST.cc | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index a4fb29690..253233905 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -131,6 +131,14 @@ TEST(DOMLink, Construction) ASSERT_TRUE(link.Density().has_value()); EXPECT_DOUBLE_EQ(density, *link.Density()); + EXPECT_EQ(link.AutoInertiaParams(), nullptr); + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + autoInertiaParamsElem->SetName("auto_inertia_params"); + link.SetAutoInertiaParams(autoInertiaParamsElem); + EXPECT_EQ(link.AutoInertiaParams(), autoInertiaParamsElem); + EXPECT_EQ(link.AutoInertiaParams()->GetName(), + autoInertiaParamsElem->GetName()); + EXPECT_EQ(0u, link.CollisionCount()); EXPECT_EQ(nullptr, link.CollisionByIndex(0)); EXPECT_EQ(nullptr, link.CollisionByIndex(1)); From b41b48b612db47a30409b80b4d1bb793ab734377 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 15 Dec 2023 15:06:57 -0800 Subject: [PATCH 09/11] Link_TEST: use string literals for model string Signed-off-by: Steve Peters --- src/Link_TEST.cc | 270 ++++++++++++++++++++++++----------------------- 1 file changed, 136 insertions(+), 134 deletions(-) diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 253233905..931fe0977 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -319,70 +319,71 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) ///////////////////////////////////////////////// TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity) { - std::string sdf = "" - "" - " " - " 0 0 1.0 0 0 0" - " " - " 0 1.0 0 0 0 0" - " " - " 12.0" - " " - " " - " 1.0 0 0 0 0 0" - " " - " " - " 1 1 1" - " " - " " - " " - " " - " " - " 0 2.0 0 0 0 0" - " " - " " - " 1.0 0 0 0 0 0" - " 24.0" - " " - " " - " 1 1 1" - " " - " " - " " - " " - " " - " 0 3.0 0 0 0 0" - " " - " 12.0" - " " - " " - " 1.0 0 0 0 0 0" - " 24.0" - " " - " " - " 1 1 1" - " " - " " - " " - " " - " " - " 0 4.0 0 0 0 0" - " " - " " - " 1.0 0 0 0 0 0" - " " - " " - " 1 1 1" - " " - " " - " " - " " - " " - ""; + const std::string sdfString = R"( + + + + 0 0 1.0 0 0 0 + + 0 1.0 0 0 0 0 + + 12.0 + + + 1.0 0 0 0 0 0 + + + 1 1 1 + + + + + + 0 2.0 0 0 0 0 + + + 1.0 0 0 0 0 0 + 24.0 + + + 1 1 1 + + + + + + 0 3.0 0 0 0 0 + + 12.0 + + + 1.0 0 0 0 0 0 + 24.0 + + + 1 1 1 + + + + + + 0 4.0 0 0 0 0 + + + 1.0 0 0 0 0 0 + + + 1 1 1 + + + + + + )"; sdf::Root root; const sdf::ParserConfig sdfParserConfig; - sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); EXPECT_TRUE(errors.empty()) << errors; EXPECT_NE(nullptr, root.Element()); @@ -468,78 +469,79 @@ 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" - " " - " " - " " - " " - " " - ""; + const std::string sdfString = R"( + + + + 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 = []( @@ -585,7 +587,7 @@ TEST(DOMLink, ResolveAutoInertialsWithDifferentAutoInertiaParams) sdf::Root root; sdf::ParserConfig sdfParserConfig; sdfParserConfig.RegisterCustomInertiaCalc(customMeshInertiaCalculator); - sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); EXPECT_TRUE(errors.empty()) << errors; EXPECT_NE(nullptr, root.Element()); From 8d91b6a962a90670bfafb64b41acb37e54c2040c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Jan 2024 22:57:52 -0800 Subject: [PATCH 10/11] Fix density handling in Collision::ToElement A density element was written unconditionally, even if it was not originally set by the user. This checks the optional density value before setting via ToElement(). Signed-off-by: Steve Peters --- src/Collision.cc | 8 ++++++-- src/Collision_TEST.cc | 15 +++++++++++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index 05ca351bf..ca944712d 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -351,6 +351,7 @@ sdf::ElementPtr Collision::Element() const return this->dataPtr->sdf; } +///////////////////////////////////////////////// sdf::ElementPtr Collision::ToElement() const { sdf::Errors errors; @@ -377,8 +378,11 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const poseElem->Set(_errors, this->RawPose()); // Set the density - sdf::ElementPtr densityElem = elem->GetElement("density", _errors); - densityElem->Set(this->Density()); + if (this->dataPtr->density.has_value()) + { + sdf::ElementPtr densityElem = elem->GetElement("density", _errors); + densityElem->Set(this->Density()); + } // Set the geometry elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true); diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index c6076c859..1ab3d1b18 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -421,6 +421,8 @@ TEST(DOMCollision, ToElement) sdf::ElementPtr elem = collision.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + EXPECT_FALSE(elem->HasElement("density")); sdf::Collision collision2; collision2.Load(elem); @@ -435,6 +437,19 @@ TEST(DOMCollision, ToElement) ASSERT_NE(nullptr, surface2->Friction()); ASSERT_NE(nullptr, surface2->Friction()->ODE()); EXPECT_DOUBLE_EQ(1.23, surface2->Friction()->ODE()->Mu()); + + // Now set density in collision + const double kDensity = 1234.5; + collision.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = collision.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + ASSERT_TRUE(elemWithDensity->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, elemWithDensity->Get("density")); + + sdf::Collision collision3; + collision3.Load(elem); + EXPECT_DOUBLE_EQ(kDensity, collision.Density()); } ///////////////////////////////////////////////// From 9460341a6eb3aa01b2ef023e0c3599bbb6e46b47 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Jan 2024 22:59:12 -0800 Subject: [PATCH 11/11] Write density data in Link::ToElement Signed-off-by: Steve Peters --- src/Link.cc | 5 +++++ src/Link_TEST.cc | 23 +++++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/src/Link.cc b/src/Link.cc index 659b25cbe..085feab93 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -987,6 +987,11 @@ sdf::ElementPtr Link::ToElement() const inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); + if (this->dataPtr->density.has_value()) + { + inertialElem->GetElement("density")->Set(*this->dataPtr->density); + } + if (this->dataPtr->inertial.FluidAddedMass().has_value()) { auto addedMass = this->dataPtr->inertial.FluidAddedMass().value(); diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 931fe0977..2182ff2e3 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -984,12 +984,18 @@ TEST(DOMLink, ToElement) sdf::ElementPtr elem = link.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + { + auto inertialElem = elem->FindElement("inertial"); + EXPECT_FALSE(inertialElem->HasElement("density")); + } sdf::Link link2; link2.Load(elem); EXPECT_EQ(link.Name(), link2.Name()); EXPECT_EQ(link.Inertial(), link2.Inertial()); + EXPECT_EQ(link.Density(), link2.Density()); EXPECT_EQ(link.RawPose(), link2.RawPose()); EXPECT_EQ(link.EnableWind(), link2.EnableWind()); EXPECT_EQ(link.CollisionCount(), link2.CollisionCount()); @@ -1015,6 +1021,23 @@ TEST(DOMLink, ToElement) EXPECT_EQ(link.ProjectorCount(), link2.ProjectorCount()); for (uint64_t i = 0; i < link2.ProjectorCount(); ++i) EXPECT_NE(nullptr, link2.ProjectorByIndex(i)); + + // Now set density in link + const double kDensity = 1234.5; + link.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = link.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + { + auto inertialElem = elemWithDensity->FindElement("inertial"); + ASSERT_TRUE(inertialElem->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, inertialElem->Get("density")); + } + + sdf::Link link3; + link3.Load(elemWithDensity); + ASSERT_TRUE(link3.Density().has_value()); + EXPECT_DOUBLE_EQ(kDensity, *link3.Density()); } /////////////////////////////////////////////////