From f5803b7e7fe2a1bbd1b939079d199c5c50e6770b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 7 Oct 2023 00:26:03 -0700 Subject: [PATCH] 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..e2dacc885 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