diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index d6b8470c9..2ab4114ce 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,32 @@ 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. + /// \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, + sdf::ElementPtr _autoInertiaParams); + /// \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..3e5260e73 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,27 @@ 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 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 c2b2a15ec..ca944712d 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -49,13 +49,10 @@ 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 Density of the collision if it has been set. + public: std::optional density; - /// \brief True if density was set during load from sdf. - public: bool densitySetAtLoad = false; - - /// \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. @@ -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,23 +262,59 @@ 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, ElementPtr()); +} + +///////////////////////////////////////////////// +void Collision::CalculateInertial( + sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config, + const std::optional &_density, + sdf::ElementPtr _autoInertiaParams) +{ + // 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 ); } + // 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, - this->dataPtr->density, this->dataPtr->autoInertiaParams); + density, autoInertiaParams); if (!geomInertial) { @@ -309,6 +351,7 @@ sdf::ElementPtr Collision::Element() const return this->dataPtr->sdf; } +///////////////////////////////////////////////// sdf::ElementPtr Collision::ToElement() const { sdf::Errors errors; @@ -335,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 07f894a58..1ab3d1b18 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"); @@ -58,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), @@ -420,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); @@ -434,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()); } ///////////////////////////////////////////////// diff --git a/src/Link.cc b/src/Link.cc index 40a64012f..085feab93 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -69,6 +69,13 @@ 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 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}, @@ -180,6 +187,18 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); + if (inertialElem->HasElement("density")) + { + 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; @@ -309,6 +328,30 @@ 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; +} + +///////////////////////////////////////////////// +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 { @@ -620,7 +663,9 @@ 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, + this->dataPtr->autoInertiaParams); totalInertia = totalInertia + collisionInertia; } @@ -942,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 5e0968b75..2182ff2e3 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -125,6 +125,20 @@ TEST(DOMLink, Construction) EXPECT_EQ(inertial.BodyMatrix(), inertial.SpatialMatrix()); 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(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)); @@ -302,6 +316,355 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) EXPECT_NE(nullptr, root.Element()); } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity) +{ + 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(sdfString, 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, ResolveAutoInertialsWithDifferentAutoInertiaParams) +{ + 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 = []( + sdf::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(sdfString, 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) { @@ -621,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()); @@ -652,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()); } ///////////////////////////////////////////////// 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, 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