diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 3a4174026..ae3515f39 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -17,8 +17,11 @@ #ifndef SDF_BOX_HH_ #define SDF_BOX_HH_ +#include + #include #include +#include #include #include #include @@ -65,6 +68,14 @@ namespace sdf /// \return A reference to a gz::math::Boxd object. public: gz::math::Boxd &Shape(); + /// \brief Calculate and return the Inertial values for the Box. In order + /// to calculate the inertial properties, the function mutates the object + /// by updating its material properties. + /// \param[in] _density Density of the box in kg/m^3 + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional + CalculateInertial(double _density); + /// \brief Create and return an SDF element filled with data from this /// box. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 44b8eeec8..97f6dce0b 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -17,7 +17,10 @@ #ifndef SDF_CAPSULE_HH_ #define SDF_CAPSULE_HH_ +#include + #include +#include #include #include #include @@ -72,6 +75,14 @@ namespace sdf /// \return A reference to a gz::math::Capsuled object. public: gz::math::Capsuled &Shape(); + /// \brief Calculate and return the Inertial values for the Capsule. + /// In order to calculate the inertial properties, the function + /// mutates the object by updating its material properties. + /// \param[in] _density Density of the capsule in kg/m^3 + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional CalculateInertial( + double _density); + /// \brief Create and return an SDF element filled with data from this /// capsule. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index fb7ec6bc4..5c2275b20 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -20,12 +20,15 @@ #include #include #include +#include +#include #include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" +#include "sdf/ParserConfig.hh" namespace sdf { @@ -75,6 +78,14 @@ namespace sdf /// \param[in] _name Name of the collision. public: void SetName(const std::string &_name); + /// \brief Get the density of the collision. + /// \return Density of the collision. + public: double Density() const; + + /// \brief Set the density of the collision. + /// \param[in] _density Density of the collision. + public: void SetDensity(double _density); + /// \brief Get a pointer to the collisions's geometry. /// \return The collision's geometry. public: const Geometry *Geom() const; @@ -119,6 +130,16 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; + /// \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 + public: void CalculateInertial(sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config); + /// \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/Cylinder.hh b/include/sdf/Cylinder.hh index eab0b155c..bba37ba24 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -17,7 +17,10 @@ #ifndef SDF_CYLINDER_HH_ #define SDF_CYLINDER_HH_ +#include + #include +#include #include #include #include @@ -72,6 +75,14 @@ namespace sdf /// \return A reference to a gz::math::Cylinderd object. public: gz::math::Cylinderd &Shape(); + /// \brief Calculate and return the Inertial values for the Cylinder. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. + /// \param[in] _density Density of the cylinder in kg/m^3 + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional + CalculateInertial(double _density); + /// \brief Create and return an SDF element filled with data from this /// cylinder. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index 10ce5e7f9..33a046c2c 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -17,6 +17,9 @@ #ifndef SDF_ELLIPSOID_HH_ #define SDF_ELLIPSOID_HH_ +#include + +#include #include #include #include @@ -64,6 +67,14 @@ namespace sdf /// \return A reference to a gz::math::Ellipsoidd object. public: gz::math::Ellipsoidd &Shape(); + /// \brief Calculate and return the Inertial values for the Ellipsoid. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. + /// \param[in] _density Density of the ellipsoid in kg/m^3 + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional + CalculateInertial(double _density); + /// \brief Create and return an SDF element filled with data from this /// ellipsoid. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index b095bf1af..7c6779c2e 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -18,11 +18,15 @@ #define SDF_GEOMETRY_HH_ #include +#include #include +#include #include #include #include +#include +#include namespace sdf { @@ -209,6 +213,17 @@ namespace sdf /// \param[in] _heightmap The heightmap shape. public: void SetHeightmapShape(const Heightmap &_heightmap); + /// \brief Calculate and return the Mass Matrix values for the Geometry + /// \param[out] _errors A vector of Errors object. Each object + /// would contain an error code and an error message. + /// \param[in] _config Parser Config + /// \param[in] _density The density of the geometry element. + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional CalculateInertial( + sdf::Errors &_errors, + const sdf::ParserConfig &_config, + double _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 a2a5c93d0..c92d8d18d 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -27,6 +27,8 @@ #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Error.hh" namespace sdf { @@ -326,6 +328,16 @@ namespace sdf public: Errors ResolveInertial(gz::math::Inertiald &_inertial, const std::string &_resolveTo = "") const; + /// \brief Calculate & set inertial values(mass, mass matrix + /// & inertial pose) for the link. Inertial values can be provided + /// by the user through the SDF or can be calculated automatically + /// by setting the auto attribute to true. + /// \param[out] _errors A vector of Errors object. Each object + /// would contain an error code and an error message. + /// \param[in] _config Custom parser configuration + public: void ResolveAutoInertials(sdf::Errors &_errors, + const ParserConfig &_config); + /// \brief Get the pose of the link. This is the pose of the link /// as specified in SDF ( ... ). /// \return The pose of the link. @@ -380,6 +392,32 @@ namespace sdf /// \sa Model::SetEnableWind(bool) public: void SetEnableWind(bool _enableWind); + /// \brief Check if the automatic calculation for the link inertial + /// is enabled or not. + /// \return True if automatic calculation is enabled. This can be done + /// setting the auto attribute of the element of the link to + /// true or by setting the autoInertia member to true + /// using SetAutoInertia(). + public: bool AutoInertia() const; + + /// \brief Enable automatic inertial calculations by setting autoInertia + /// to true. + /// \param[in] _autoInertia True or False + public: void SetAutoInertia(bool _autoInertia); + + /// \brief Check if the inertial values for this link were saved. + /// If true, the inertial values for this link wont be calculated + /// when CalculateInertial() is called. This value is set to true + /// when CalculateInertial() is called with SAVE_CALCULATION + /// configuration. + /// \return True if CalculateInertial() was called with SAVE_CALCULATION + /// configuration, false otherwise. + public: bool AutoInertiaSaved() const; + + /// \brief Set the autoInertiaSaved() values + /// \param[in] _autoInertiaSaved True or False + public: void SetAutoInertiaSaved(bool _autoInertiaSaved); + /// \brief Add a collision to the link. /// \param[in] _collision Collision to add. /// \return True if successful, false if a collision with the name already diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index dbece5917..86cc3d422 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -508,6 +508,14 @@ namespace sdf /// \param[in] _plugin Plugin to add. public: void AddPlugin(const Plugin &_plugin); + /// \brief Calculate and set the inertials for all the links belonging + /// to the model object + /// \param[out] _errrors 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 + public: void ResolveAutoInertials(sdf::Errors &_errors, + const ParserConfig &_config); + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Root::Load or /// World::SetPoseRelativeToGraph if this is a standalone model and diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index ad8195fe5..24fd4c6b9 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -48,6 +48,20 @@ enum class EnforcementPolicy LOG, }; +/// \enum ConfigureResolveAutoInertials +/// \brief Configuration options of how CalculateInertial() function +/// would be used +enum class ConfigureResolveAutoInertials +{ + /// \brief If this value is used, CalculateInertial() won't be + /// called from inside the Root::Load() function + SKIP_CALCULATION_IN_LOAD, + + /// \brief If this values is used, CalculateInertial() would be + /// called and the computed inertial values would be saved + SAVE_CALCULATION +}; + // Forward declare private data class. class ParserConfigPrivate; @@ -160,6 +174,17 @@ class SDFORMAT_VISIBLE ParserConfig /// \return The deperacted elements policy enum value public: EnforcementPolicy DeprecatedElementsPolicy() const; + /// \brief Get the current configuration for the CalculateInertial() + /// function + /// \return Current set value of the ConfigureResolveAutoInertials enum + public: ConfigureResolveAutoInertials CalculateInertialConfiguration() const; + + /// \brief Set the configuration for the CalculateInertial() function + /// \param[in] _configuration The configuration to set for the + /// CalculateInertial() function + public: void SetCalculateInertialConfiguration( + ConfigureResolveAutoInertials _configuration); + /// \brief Registers a custom model parser. /// \param[in] _modelParser Callback as described in /// sdf/InterfaceElements.hh. diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 1d88defcc..9ad5f1632 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -226,6 +226,17 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors UpdateGraphs(); + /// \brief Calculate & set the inertial properties (mass, mass matrix + /// and inertial pose) for all the worlds & models that are children + /// of this Root object. This function must be called after calling + /// Root::Load() or UpdateGraphs() since it uses frame graphs to + /// resolve inertial pose for links and they would be automatically built + /// \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 + public: void ResolveAutoInertials(sdf::Errors &_errors, + const ParserConfig &_config); + /// \brief Create and return an SDF element filled with data from this /// root. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 3ed63ea02..05eb072e3 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -17,6 +17,9 @@ #ifndef SDF_SPHERE_HH_ #define SDF_SPHERE_HH_ +#include + +#include #include #include @@ -65,6 +68,14 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Calculate and return the Inertial values for the Sphere. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. + /// \param[in] _density Density of the sphere in kg/m^3 + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional + CalculateInertial(double _density); + /// \brief Create and return an SDF element filled with data from this /// sphere. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 0afc85472..7e449abab 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -516,6 +516,14 @@ namespace sdf /// \param[in] _plugin Plugin to add. public: void AddPlugin(const Plugin &_plugin); + /// \brief Calculate and set the inertials for all the models in the world + /// object + /// \param[out] _errrors 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 + public: void ResolveAutoInertials(sdf::Errors &_errors, + const ParserConfig &_config); + /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child /// entities for resolving poses. This is private and is intended to be /// called by Root::Load. diff --git a/sdf/1.11/collision.sdf b/sdf/1.11/collision.sdf index e83406df1..496d69a97 100644 --- a/sdf/1.11/collision.sdf +++ b/sdf/1.11/collision.sdf @@ -14,6 +14,14 @@ Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics. + + + Mass Density of the collision geometry. + This is used to determine mass and inertia values during automatic calculation. + Default is the density of water 1000 kg/m^3. + + + diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf index f4d31ef59..ce2c30f14 100644 --- a/sdf/1.11/inertial.sdf +++ b/sdf/1.11/inertial.sdf @@ -5,10 +5,26 @@ properties, and optionally its fluid added mass. + + + Set to true if you want automatic computation for the moments of inertia (ixx, iyy, izz) + and products of inertia(ixy, iyz, ixz). Default value is false. + + + The mass of the link. + + + Mass Density of the collision geometry. + This is used to determine mass and inertia values during automatic calculation. + This density value would be overwritten by the density value in collision. + Default is the density of water 1000 kg/m^3. + + + This pose (translation, rotation) describes the position and orientation diff --git a/src/Box.cc b/src/Box.cc index a2eae360f..a2338c1fe 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -14,7 +14,12 @@ * limitations under the License. * */ +#include + #include +#include +#include +#include #include "sdf/Box.hh" #include "sdf/parser.hh" #include "Utils.hh" @@ -115,6 +120,26 @@ gz::math::Boxd &Box::Shape() return this->dataPtr->box; } +///////////////////////////////////////////////// +std::optional Box::CalculateInertial(double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->box.SetMaterial(material); + + auto boxMassMatrix = this->dataPtr->box.MassMatrix(); + + if (!boxMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald boxInertial; + boxInertial.SetMassMatrix(boxMassMatrix.value()); + return std::make_optional(boxInertial); + } +} + ///////////////////////////////////////////////// sdf::ElementPtr Box::ToElement() const { diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index c386710b5..9fb631242 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -14,11 +14,16 @@ * limitations under the License. * */ +#include #include #include "sdf/Box.hh" #include "sdf/Element.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMBox, Construction) @@ -145,6 +150,51 @@ TEST(DOMBox, Shape) EXPECT_EQ(gz::math::Vector3d(1, 2, 3), box.Size()); } +///////////////////////////////////////////////// +TEST(DOMBox, CalculateInertial) +{ + sdf::Box box; + + // density of aluminium + double density = 2710; + + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() + box.SetSize(gz::math::Vector3d(-1, 1, 0)); + auto invalidBoxInertial = box.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidBoxInertial); + + const double l = 2; + const double w = 2; + const double h = 2; + box.SetSize(gz::math::Vector3d(l, w, h)); + + double expectedMass = box.Shape().Volume() * density; + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto boxInertial = box.CalculateInertial(density); + EXPECT_EQ(box.Shape().Material().Density(), density); + ASSERT_NE(std::nullopt, boxInertial); + EXPECT_EQ(expectedInertial, *boxInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + boxInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + boxInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), boxInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMBox, ToElement) { diff --git a/src/Capsule.cc b/src/Capsule.cc index 0730b455a..1f8e2bd00 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -14,7 +14,11 @@ * limitations under the License. * */ +#include #include + +#include +#include #include "sdf/Capsule.hh" #include "sdf/parser.hh" #include "Utils.hh" @@ -137,6 +141,26 @@ gz::math::Capsuled &Capsule::Shape() return this->dataPtr->capsule; } +///////////////////////////////////////////////// +std::optional Capsule::CalculateInertial(double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->capsule.SetMat(material); + + auto capsuleMassMatrix = this->dataPtr->capsule.MassMatrix(); + + if(!capsuleMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald capsuleInertial; + capsuleInertial.SetMassMatrix(capsuleMassMatrix.value()); + return std::make_optional(capsuleInertial); + } +} + ///////////////////////////////////////////////// sdf::ElementPtr Capsule::ToElement() const { diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index b24495966..4cb39a851 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -15,9 +15,14 @@ * */ +#include #include #include "sdf/Capsule.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMCapsule, Construction) @@ -181,6 +186,51 @@ TEST(DOMCapsule, Shape) EXPECT_DOUBLE_EQ(0.456, capsule.Length()); } +///////////////////////////////////////////////// +TEST(DOMCapsule, CalculateInertial) +{ + sdf::Capsule capsule; + + // density of aluminium + const double density = 2710; + const double l = 2.0; + const double r = 0.1; + + capsule.SetLength(l); + capsule.SetRadius(r); + + double expectedMass = capsule.Shape().Volume() * density; + const double cylinderVolume = GZ_PI * r*r * l; + const double sphereVolume = GZ_PI * 4. / 3. * r*r*r; + const double volume = cylinderVolume + sphereVolume; + const double cylinderMass = expectedMass * cylinderVolume / volume; + const double sphereMass = expectedMass * sphereVolume / volume; + + double ixxIyy = (1/12.0) * cylinderMass * (3*r*r + l*l) + + sphereMass * (0.4*r*r + 0.375*r*l + 0.25*l*l); + double izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyy, ixxIyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto capsuleInertial = capsule.CalculateInertial(density); + EXPECT_EQ(capsule.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, capsuleInertial); + EXPECT_EQ(expectedInertial, *capsuleInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + capsuleInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + capsuleInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), capsuleInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMCapsule, ToElement) { diff --git a/src/Collision.cc b/src/Collision.cc index e2aeac08f..89d15ffce 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -17,12 +17,15 @@ #include #include #include +#include #include "sdf/Collision.hh" #include "sdf/Error.hh" #include "sdf/Geometry.hh" #include "sdf/parser.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Element.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" #include "Utils.hh" @@ -46,6 +49,12 @@ 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 The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -114,6 +123,13 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->surface.Load(_sdf->GetElement("surface", errors)); } + // Load the density value if given + if (_sdf->HasElement("density")) + { + this->dataPtr->density = _sdf->Get("density"); + this->dataPtr->densitySetAtLoad = true; + } + return errors; } @@ -129,6 +145,18 @@ void Collision::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +double Collision::Density() const +{ + return this->dataPtr->density; +} + +///////////////////////////////////////////////// +void Collision::SetDensity(double _density) +{ + this->dataPtr->density = _density; +} + ///////////////////////////////////////////////// const Geometry *Collision::Geom() const { @@ -200,6 +228,59 @@ sdf::SemanticPose Collision::SemanticPose() const this->dataPtr->poseRelativeToGraph); } +///////////////////////////////////////////////// +void Collision::CalculateInertial( + sdf::Errors &_errors, + 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) + { + Error densityMissingErr( + ErrorCode::ELEMENT_MISSING, + "Collision is missing a child element. " + "Using a default density value of 1000.0 kg/m^3. " + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), densityMissingErr, _errors + ); + } + + auto geomInertial = + this->dataPtr->geom.CalculateInertial(_errors, _config, + this->dataPtr->density); + + if (!geomInertial) + { + _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, + "Inertia Calculated for collision: " + + this->dataPtr->name + " is invalid."}); + } + else + { + _inertial = geomInertial.value(); + + // If collision pose is in Link Frame then set that as inertial pose + // Else resolve collision pose in Link Frame and then set as inertial pose + if (this->dataPtr->poseRelativeTo.empty()) + { + _inertial.SetPose(this->dataPtr->pose); + } + else + { + gz::math::Pose3d collisionPoseLinkFrame; + Errors poseConvErrors = + this->SemanticPose().Resolve(collisionPoseLinkFrame); + _errors.insert(_errors.end(), + poseConvErrors.begin(), + poseConvErrors.end()); + _inertial.SetPose(collisionPoseLinkFrame); + } + } +} + ///////////////////////////////////////////////// sdf::ElementPtr Collision::Element() const { @@ -231,6 +312,10 @@ 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()); + // Set the geometry elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true); diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 0129b25db..b6aed7ec0 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -18,8 +18,18 @@ #include #include "sdf/Collision.hh" #include "sdf/Geometry.hh" +#include "sdf/Box.hh" +#include "sdf/Model.hh" +#include "sdf/Link.hh" #include "sdf/Surface.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" +#include "sdf/Root.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMcollision, Construction) @@ -27,6 +37,7 @@ TEST(DOMcollision, Construction) sdf::Collision collision; EXPECT_EQ(nullptr, collision.Element()); EXPECT_TRUE(collision.Name().empty()); + EXPECT_EQ(collision.Density(), 1000.0); collision.SetName("test_collison"); EXPECT_EQ(collision.Name(), "test_collison"); @@ -42,6 +53,9 @@ TEST(DOMcollision, Construction) EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } + collision.SetDensity(1240.0); + EXPECT_DOUBLE_EQ(collision.Density(), 1240.0); + 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), collision.RawPose()); @@ -170,6 +184,155 @@ TEST(DOMcollision, SetSurface) EXPECT_EQ(collision.Surface()->Contact()->CollideBitmask(), 0x2); } +///////////////////////////////////////////////// +TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial) +{ + sdf::Collision collision; + EXPECT_DOUBLE_EQ(1000.0, collision.Density()); + + sdf::ElementPtr sdf(new sdf::Element()); + collision.Load(sdf); + + gz::math::Inertiald collisionInertial; + const sdf::ParserConfig sdfParserConfig; + sdf::Geometry geom; + sdf::Box box; + + // Invalid Inertial test + box.SetSize(gz::math::Vector3d(-1, 1, 0)); + geom.SetType(sdf::GeometryType::BOX); + geom.SetBoxShape(box); + collision.SetGeom(geom); + + sdf::Errors errors; + + collision.CalculateInertial(errors, collisionInertial, + sdfParserConfig); + ASSERT_FALSE(errors.empty()); +} + +///////////////////////////////////////////////// +TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + const sdf::Collision *collision = link->CollisionByIndex(0); + + sdf::Errors inertialErr; + root.ResolveAutoInertials(inertialErr, sdfParserConfig); + + const double l = 2; + const double w = 2; + const double h = 2; + + double expectedMass = l*w*h * collision->Density(); + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + ASSERT_TRUE(inertialErr.empty()); + EXPECT_DOUBLE_EQ(1240.0, collision->Density()); + EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.MassMatrix(), link->Inertial().MassMatrix()); + EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose()); +} + +///////////////////////////////////////////////// +TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) +{ + std::string sdf = "" + " " + " " + " " + " 0 0 1 0 0 0" + " " + " " + " " + " " + " 0 0 -1 0 0 0" + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + const sdf::Collision *collision = link->CollisionByIndex(0); + + sdf::Errors inertialErr; + root.ResolveAutoInertials(inertialErr, sdfParserConfig); + + const double l = 2; + const double w = 2; + const double h = 2; + + double expectedMass = l*w*h * collision->Density(); + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + ASSERT_TRUE(inertialErr.empty()); + EXPECT_DOUBLE_EQ(1240.0, collision->Density()); + EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.MassMatrix(), link->Inertial().MassMatrix()); + EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose()); +} + ///////////////////////////////////////////////// TEST(DOMCollision, ToElement) { diff --git a/src/Cylinder.cc b/src/Cylinder.cc index cc13b0e5d..8c7779d00 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -15,6 +15,9 @@ * */ #include +#include + +#include #include "sdf/Cylinder.hh" #include "sdf/parser.hh" #include "Utils.hh" @@ -137,6 +140,25 @@ gz::math::Cylinderd &Cylinder::Shape() return this->dataPtr->cylinder; } +std::optional Cylinder::CalculateInertial(double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->cylinder.SetMat(material); + + auto cylinderMassMatrix = this->dataPtr->cylinder.MassMatrix(); + + if (!cylinderMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald cylinderInertial; + cylinderInertial.SetMassMatrix(cylinderMassMatrix.value()); + return std::make_optional(cylinderInertial); + } +} + ///////////////////////////////////////////////// sdf::ElementPtr Cylinder::ToElement() const { diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index efc7f302b..5c7d04ead 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -15,9 +15,14 @@ * */ +#include #include #include "sdf/Cylinder.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMCylinder, Construction) @@ -177,6 +182,52 @@ TEST(DOMCylinder, Shape) EXPECT_DOUBLE_EQ(0.456, cylinder.Length()); } +///////////////////////////////////////////////// +TEST(DOMCylinder, CalculateInertial) +{ + sdf::Cylinder cylinder; + + // density of aluminium + const double density = 2170; + + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() + cylinder.SetLength(-1); + cylinder.SetRadius(0); + auto invalidCylinderInertial = cylinder.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidCylinderInertial); + + const double l = 2.0; + const double r = 0.1; + + cylinder.SetLength(l); + cylinder.SetRadius(r); + + double expectedMass = cylinder.Shape().Volume() * density; + double ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l); + double izz = 0.5 * expectedMass * r * r; + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyy, ixxIyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto cylinderInertial = cylinder.CalculateInertial(density); + EXPECT_EQ(cylinder.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, cylinderInertial); + EXPECT_EQ(expectedInertial, *cylinderInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + cylinderInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + cylinderInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), cylinderInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMCylinder, ToElement) { diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 140414779..294a0b87b 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -15,6 +15,10 @@ * */ #include +#include + +#include +#include #include "sdf/Ellipsoid.hh" #include "sdf/parser.hh" #include "Utils.hh" @@ -116,6 +120,26 @@ gz::math::Ellipsoidd &Ellipsoid::Shape() return this->dataPtr->ellipsoid; } +///////////////////////////////////////////////// +std::optional Ellipsoid::CalculateInertial(double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->ellipsoid.SetMat(material); + + auto ellipsoidMassMatrix = this->dataPtr->ellipsoid.MassMatrix(); + + if(!ellipsoidMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald ellipsoidInertial; + ellipsoidInertial.SetMassMatrix(ellipsoidMassMatrix.value()); + return std::make_optional(ellipsoidInertial); + } +} + ///////////////////////////////////////////////// sdf::ElementPtr Ellipsoid::ToElement() const { diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index 9f3e33a8e..cdba939b9 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -15,9 +15,14 @@ * */ +#include #include #include "sdf/Ellipsoid.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMEllipsoid, Construction) @@ -141,6 +146,52 @@ TEST(DOMEllipsoid, Shape) EXPECT_EQ(expectedRadii, ellipsoid.Radii()); } +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CalculateInertial) +{ + sdf::Ellipsoid ellipsoid; + + // density of aluminium + const double density = 2170; + + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() + ellipsoid.SetRadii(gz::math::Vector3d(-1, 2, 0)); + auto invalidEllipsoidInertial = ellipsoid.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidEllipsoidInertial); + + const double a = 1.0; + const double b = 10.0; + const double c = 100.0; + + ellipsoid.SetRadii(gz::math::Vector3d(a, b, c)); + + double expectedMass = ellipsoid.Shape().Volume() * density; + double ixx = (expectedMass / 5.0) * (b*b + c*c); + double iyy = (expectedMass / 5.0) * (a*a + c*c); + double izz = (expectedMass / 5.0) * (a*a + b*b); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto ellipsoidInertial = ellipsoid.CalculateInertial(density); + EXPECT_EQ(ellipsoid.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, ellipsoidInertial); + EXPECT_EQ(expectedInertial, *ellipsoidInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + ellipsoidInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + ellipsoidInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), ellipsoidInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMEllipsoid, ToElement) { diff --git a/src/Geometry.cc b/src/Geometry.cc index 33df5174c..fb3a56d52 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -17,6 +17,7 @@ #include +#include #include "sdf/Geometry.hh" #include "sdf/Box.hh" #include "sdf/Capsule.hh" @@ -28,6 +29,9 @@ #include "sdf/Plane.hh" #include "sdf/Polyline.hh" #include "sdf/Sphere.hh" +#include "sdf/Element.hh" +#include "sdf/Types.hh" +#include "sdf/Error.hh" #include "Utils.hh" @@ -308,6 +312,44 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) this->dataPtr->polylines = _polylines; } +std::optional Geometry::CalculateInertial( + sdf::Errors &_errors, const sdf::ParserConfig &_config, double _density) +{ + std::optional geomInertial; + + switch (this->dataPtr->type) + { + case GeometryType::BOX: + geomInertial = this->dataPtr->box->CalculateInertial(_density); + break; + case GeometryType::CAPSULE: + geomInertial = this->dataPtr->capsule->CalculateInertial(_density); + break; + case GeometryType::CYLINDER: + geomInertial = this->dataPtr->cylinder->CalculateInertial(_density); + break; + case GeometryType::ELLIPSOID: + geomInertial = this->dataPtr->ellipsoid->CalculateInertial(_density); + break; + case GeometryType::SPHERE: + geomInertial = this->dataPtr->sphere->CalculateInertial(_density); + break; + default: + Error invalidGeomTypeErr( + ErrorCode::WARNING, + "Automatic inertia calculations are not supported for the given" + " Geometry type. " + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), invalidGeomTypeErr, _errors + ); + geomInertial = std::nullopt; + break; + } + + return geomInertial; +} + ///////////////////////////////////////////////// sdf::ElementPtr Geometry::Element() const { diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 3155c9870..6c3f724fe 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ - +#include #include #include "sdf/Box.hh" #include "sdf/Capsule.hh" @@ -26,7 +26,14 @@ #include "sdf/Plane.hh" #include "sdf/Polyline.hh" #include "sdf/Sphere.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" +#include "sdf/Element.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMGeometry, Construction) @@ -298,6 +305,188 @@ TEST(DOMGeometry, Polyline) EXPECT_EQ(gz::math::Vector2d(8.7, 0.9), polylineShape[1].Points()[1]); } +///////////////////////////////////////////////// +TEST(DOMGeometry, CalculateInertial) +{ + sdf::Geometry geom; + + // Density of Aluminimum + const double density = 2170.0; + double expectedMass; + gz::math::MassMatrix3d expectedMassMat; + gz::math::Inertiald expectedInertial; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors; + + // Not supported geom type + { + geom.SetType(sdf::GeometryType::EMPTY); + auto notSupportedInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); + ASSERT_EQ(notSupportedInertial, std::nullopt); + } + + // Box + { + sdf::Box box; + const double l = 2; + const double w = 2; + const double h = 2; + box.SetSize(gz::math::Vector3d(l, w, h)); + + expectedMass = box.Shape().Volume() * density; + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixx, iyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::BOX); + geom.SetBoxShape(box); + auto boxInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); + + ASSERT_NE(std::nullopt, boxInertial); + EXPECT_EQ(expectedInertial, *boxInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), boxInertial->Pose()); + } + + // Capsule + { + sdf::Capsule capsule; + const double l = 2.0; + const double r = 0.1; + capsule.SetLength(l); + capsule.SetRadius(r); + + expectedMass = capsule.Shape().Volume() * density; + const double cylinderVolume = GZ_PI * r*r * l; + const double sphereVolume = GZ_PI * 4. / 3. * r*r*r; + const double volume = cylinderVolume + sphereVolume; + const double cylinderMass = expectedMass * cylinderVolume / volume; + const double sphereMass = expectedMass * sphereVolume / volume; + double ixxIyy = (1/12.0) * cylinderMass * (3*r*r + l*l) + + sphereMass * (0.4*r*r + 0.375*r*l + 0.25*l*l); + double izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass); + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::CAPSULE); + geom.SetCapsuleShape(capsule); + auto capsuleInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); + + ASSERT_NE(std::nullopt, capsuleInertial); + EXPECT_EQ(expectedInertial, *capsuleInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), capsuleInertial->Pose()); + } + + // Cylinder + { + sdf::Cylinder cylinder; + const double l = 2.0; + const double r = 0.1; + + cylinder.SetLength(l); + cylinder.SetRadius(r); + + expectedMass = cylinder.Shape().Volume() * density; + double ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l); + double izz = 0.5 * expectedMass * r * r; + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::CYLINDER); + geom.SetCylinderShape(cylinder); + auto cylinderInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); + + ASSERT_NE(std::nullopt, cylinderInertial); + EXPECT_EQ(expectedInertial, *cylinderInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), cylinderInertial->Pose()); + } + + // Ellipsoid + { + sdf::Ellipsoid ellipsoid; + + const double a = 1.0; + const double b = 10.0; + const double c = 100.0; + + ellipsoid.SetRadii(gz::math::Vector3d(a, b, c)); + + expectedMass = ellipsoid.Shape().Volume() * density; + double ixx = (expectedMass / 5.0) * (b*b + c*c); + double iyy = (expectedMass / 5.0) * (a*a + c*c); + double izz = (expectedMass / 5.0) * (a*a + b*b); + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixx, iyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::ELLIPSOID); + geom.SetEllipsoidShape(ellipsoid); + auto ellipsoidInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); + + ASSERT_NE(std::nullopt, ellipsoidInertial); + EXPECT_EQ(expectedInertial, *ellipsoidInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), ellipsoidInertial->Pose()); + } + + // Sphere + { + sdf::Sphere sphere; + const double r = 0.1; + + sphere.SetRadius(r); + + expectedMass = sphere.Shape().Volume() * density; + double ixxIyyIzz = 0.4 * expectedMass * r * r; + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments( + gz::math::Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::SPHERE); + geom.SetSphereShape(sphere); + auto sphereInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); + + ASSERT_NE(std::nullopt, sphereInertial); + EXPECT_EQ(expectedInertial, *sphereInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), sphereInertial->Pose()); + } +} + ///////////////////////////////////////////////// TEST(DOMGeometry, ToElement) { diff --git a/src/Link.cc b/src/Link.cc index c74783a11..7d8d5a182 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -31,6 +31,8 @@ #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Element.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" @@ -78,6 +80,13 @@ class sdf::Link::Implementation /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; + /// \brief True if automatic caluclation for the link inertial is enabled + public: bool autoInertia = false; + + /// \brief This variable is used to track whether the inertia values for + /// link was saved in CalculateInertial() + public: bool autoInertiaSaved = false; + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; }; @@ -171,23 +180,46 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); - if (inertialElem->HasElement("pose")) - loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); + if (inertialElem->Get("auto")) + { + this->dataPtr->autoInertia = true; + // If auto is to true but user has still provided + // inertial values + if (inertialElem->HasElement("pose") || + inertialElem->HasElement("mass") || + inertialElem->HasElement("inertia")) + { + 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." + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); + } + } + // If auto is set to false or not specified + else + { + if (inertialElem->HasElement("pose")) + loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); - // Get the mass. - mass = inertialElem->Get("mass", 1.0).first; + // Get the mass. + mass = inertialElem->Get("mass", 1.0).first; - if (inertialElem->HasElement("inertia")) - { - sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + if (inertialElem->HasElement("inertia")) + { + sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); - xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); - xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); - xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); + xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); + xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); + xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); - xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); - xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); - xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); + xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); + xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + } } if (inertialElem->HasElement("fluid_added_mass")) @@ -246,6 +278,7 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) } } } + if (!this->dataPtr->inertial.SetMassMatrix( gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) { @@ -563,6 +596,50 @@ Errors Link::ResolveInertial( return errors; } +///////////////////////////////////////////////// +void Link::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) +{ + // If inertial calculations is set to automatic & the inertial values for the + // link was not saved previously + if (this->dataPtr->autoInertia && !this->dataPtr->autoInertiaSaved) + { + // Return an error if auto is set to true but there are no + // collision elements in the link + if (this->dataPtr->collisions.empty()) + { + _errors.push_back({ErrorCode::ELEMENT_MISSING, + "Inertial is set to auto but there are no " + " elements for the link."}); + return; + } + + gz::math::Inertiald totalInertia; + + for (sdf::Collision &collision : this->dataPtr->collisions) + { + gz::math::Inertiald collisionInertia; + collision.CalculateInertial(_errors, collisionInertia, _config); + totalInertia = totalInertia + collisionInertia; + } + + this->dataPtr->inertial = totalInertia; + + // If CalculateInertial() was called with SAVE_CALCULATION + // configuration then set autoInertiaSaved to true + if (_config.CalculateInertialConfiguration() == + ConfigureResolveAutoInertials::SAVE_CALCULATION) + { + this->dataPtr->autoInertiaSaved = true; + } + } + // If auto is false, this means inertial values were set + // from user given values in Link::Load(), therefore we can return + else + { + return; + } +} + ///////////////////////////////////////////////// const gz::math::Pose3d &Link::RawPose() const { @@ -717,6 +794,30 @@ void Link::SetEnableWind(const bool _enableWind) this->dataPtr->enableWind = _enableWind; } +///////////////////////////////////////////////// +bool Link::AutoInertia() const +{ + return this->dataPtr->autoInertia; +} + +///////////////////////////////////////////////// +void Link::SetAutoInertia(bool _autoInertia) +{ + this->dataPtr->autoInertia = _autoInertia; +} + +///////////////////////////////////////////////// +bool Link::AutoInertiaSaved() const +{ + return this->dataPtr->autoInertiaSaved; +} + +///////////////////////////////////////////////// +void Link::SetAutoInertiaSaved(bool _autoInertiaSaved) +{ + this->dataPtr->autoInertiaSaved = _autoInertiaSaved; +} + ////////////////////////////////////////////////// bool Link::AddCollision(const Collision &_collision) { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index bf53c118a..524d003f5 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -22,10 +22,14 @@ #include "sdf/Collision.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" +#include "sdf/Root.hh" +#include "sdf/Model.hh" #include "sdf/ParticleEmitter.hh" #include "sdf/Projector.hh" #include "sdf/Sensor.hh" #include "sdf/Visual.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" ///////////////////////////////////////////////// TEST(DOMLink, Construction) @@ -68,6 +72,14 @@ TEST(DOMLink, Construction) link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); + EXPECT_FALSE(link.AutoInertiaSaved()); + link.SetAutoInertiaSaved(true); + EXPECT_TRUE(link.AutoInertiaSaved()); + + EXPECT_FALSE(link.AutoInertia()); + link.SetAutoInertia(true); + EXPECT_TRUE(link.AutoInertia()); + EXPECT_EQ(0u, link.SensorCount()); EXPECT_EQ(nullptr, link.SensorByIndex(0)); EXPECT_EQ(nullptr, link.SensorByIndex(1)); @@ -266,6 +278,160 @@ TEST(DOMLink, InvalidInertia) EXPECT_FALSE(link.Inertial().MassMatrix().IsValid()); } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + 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()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) +{ + std::string sdf = "" + "" + " " + " 0 0 1.0 0 0 0" + " " + " " + " " + " 0 0 -0.5 0 0 0" + " 2.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " 0 0 0.5 0 0 0" + " 4" + " " + " " + " 0.5" + " 1.0" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + + // Mass of cube(volume * density) + mass of cylinder(volume * density) + double expectedMass = 1.0 * 2.0 + GZ_PI * 0.5 * 0.5 * 1 * 4.0; + + EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(2.013513, 2.013513, 0.72603), + link->Inertial().MassMatrix().DiagonalMoments()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) +{ + std::string sdf = "" + "" + " " + " " + " " + " 4.0" + " 1 1 1 2 2 2" + " " + " 1" + " 1" + " 1" + " " + " " + " " + " 2.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + + EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose()); + EXPECT_EQ(gz::math::Vector3d(0.33333, 0.33333, 0.33333), + link->Inertial().MassMatrix().DiagonalMoments()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsCalledWithAutoFalse) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_TRUE(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()); +} + ///////////////////////////////////////////////// TEST(DOMLink, AddCollision) { diff --git a/src/Model.cc b/src/Model.cc index 838f05c94..944e50b87 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -905,6 +905,23 @@ void Model::SetPoseRelativeTo(const std::string &_frame) this->dataPtr->poseRelativeTo = _frame; } +///////////////////////////////////////////////// +void Model::ResolveAutoInertials(sdf::Errors &_errors, + const ParserConfig &_config) +{ + // Loop through all the nested models, if there are any + for (sdf::Model &model : this->dataPtr->models) + { + model.ResolveAutoInertials(_errors, _config); + } + + // Calculate and set inertials for all the links in the model + for (sdf::Link &link : this->dataPtr->links) + { + link.ResolveAutoInertials(_errors, _config); + } +} + ///////////////////////////////////////////////// void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index d6282383a..96059afc6 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -43,6 +43,12 @@ class sdf::ParserConfig::Implementation /// to behave behave differently than the `warningsPolicy`. 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() + public: ConfigureResolveAutoInertials resolveAutoInertialsConfig = + ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD; + /// \brief Collection of custom model parsers. public: std::vector customParsers; @@ -157,6 +163,19 @@ EnforcementPolicy ParserConfig::DeprecatedElementsPolicy() const this->dataPtr->warningsPolicy); } +///////////////////////////////////////////////// +ConfigureResolveAutoInertials ParserConfig::CalculateInertialConfiguration() const +{ + return this->dataPtr->resolveAutoInertialsConfig; +} + +///////////////////////////////////////////////// +void ParserConfig::SetCalculateInertialConfiguration( + ConfigureResolveAutoInertials _configuration) +{ + this->dataPtr->resolveAutoInertialsConfig = _configuration; +} + ///////////////////////////////////////////////// void ParserConfig::RegisterCustomModelParser(CustomModelParser _modelParser) { diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 6d139817c..00add6e3b 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -56,6 +56,14 @@ TEST(ParserConfig, Construction) EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.WarningsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy()); + + EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD, + config.CalculateInertialConfiguration()); + config.SetCalculateInertialConfiguration( + sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); + EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION, + config.CalculateInertialConfiguration()); + EXPECT_FALSE(config.URDFPreserveFixedJoint()); EXPECT_FALSE(config.StoreResolvedURIs()); } diff --git a/src/Root.cc b/src/Root.cc index 35b39d97e..7a37f1466 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -392,6 +392,13 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // Check that //axis*/mimic/@joint values specify valid joints. checkJointAxisMimicValues(this, errors); + // Check if CalculateInertialConfiguration() is not set to skip in load + if (_config.CalculateInertialConfiguration() != + ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD) + { + this->ResolveAutoInertials(errors, _config); + } + return errors; } @@ -590,6 +597,23 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, _model.SetPoseRelativeToGraph(this->modelPoseRelativeToGraph); } +///////////////////////////////////////////////// +void Root::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) +{ + // Calculate and set Inertials for all the worlds + for (sdf::World &world : this->dataPtr->worlds) + { + world.ResolveAutoInertials(_errors, _config); + } + + // Calculate and set Inertials for the model, if it is present + if (std::holds_alternative(this->dataPtr->modelLightOrActor)) + { + sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); + model.ResolveAutoInertials(_errors, _config); + } +} + ///////////////////////////////////////////////// sdf::ElementPtr Root::ToElement(const OutputConfig &_config) const { diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index f7a643743..d48382276 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -26,6 +26,8 @@ #include "sdf/World.hh" #include "sdf/Frame.hh" #include "sdf/Root.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" #include "test_config.hh" ///////////////////////////////////////////////// @@ -322,6 +324,44 @@ TEST(DOMRoot, FrameSemanticsOnMove) } } +///////////////////////////////////////////////// +TEST(DOMRoot, ResolveAutoInertialsWithSaveCalculationConfiguration) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + + sdfParserConfig.SetCalculateInertialConfiguration( + sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); + + sdf::Errors inertialErr; + root.ResolveAutoInertials(inertialErr, sdfParserConfig); + EXPECT_TRUE(inertialErr.empty()); + ASSERT_TRUE(link->AutoInertiaSaved()); +} + ///////////////////////////////////////////////// TEST(DOMRoot, AddWorld) { diff --git a/src/Sphere.cc b/src/Sphere.cc index bc181fa26..9d274530c 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -14,6 +14,10 @@ * limitations under the License. * */ +#include +#include +#include + #include "sdf/parser.hh" #include "sdf/Sphere.hh" #include "Utils.hh" @@ -114,6 +118,26 @@ sdf::ElementPtr Sphere::Element() const return this->dataPtr->sdf; } +///////////////////////////////////////////////// +std::optional Sphere::CalculateInertial(double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->sphere.SetMaterial(material); + + auto sphereMassMatrix = this->dataPtr->sphere.MassMatrix(); + + if (!sphereMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald sphereInertial; + sphereInertial.SetMassMatrix(sphereMassMatrix.value()); + return std::make_optional(sphereInertial); + } +} + ///////////////////////////////////////////////// sdf::ElementPtr Sphere::ToElement() const { diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index d4796ee4d..e7da830dc 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -14,10 +14,14 @@ * limitations under the License. * */ - +#include #include #include "sdf/Sphere.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMSphere, Construction) @@ -135,6 +139,46 @@ TEST(DOMSphere, Shape) EXPECT_DOUBLE_EQ(0.123, sphere.Radius()); } +///////////////////////////////////////////////// +TEST(DOMSphere, CalculateInertial) +{ + sdf::Sphere sphere; + + // density of aluminium + const double density = 2170; + + sphere.SetRadius(-2); + auto invalidSphereInertial = sphere.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidSphereInertial); + + const double r = 0.1; + + sphere.SetRadius(r); + + double expectedMass = sphere.Shape().Volume() * density; + double ixxIyyIzz = 0.4 * expectedMass * r * r; + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto sphereInertial = sphere.CalculateInertial(density); + EXPECT_EQ(sphere.Shape().Material().Density(), density); + ASSERT_NE(std::nullopt, sphereInertial); + EXPECT_EQ(expectedInertial, *sphereInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + sphereInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + sphereInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), sphereInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMSphere, ToElement) { diff --git a/src/World.cc b/src/World.cc index e91a4b6c5..d94ff685a 100644 --- a/src/World.cc +++ b/src/World.cc @@ -859,6 +859,17 @@ const NestedInclude *World::InterfaceModelNestedIncludeByIndex( return nullptr; } +///////////////////////////////////////////////// +void World::ResolveAutoInertials(sdf::Errors &_errors, + const ParserConfig &_config) +{ + // Call ResolveAutoInertials() function for all the models + for (sdf::Model &model : this->dataPtr->models) + { + model.ResolveAutoInertials(_errors, _config); + } +} + ///////////////////////////////////////////////// void World::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { diff --git a/src/World_TEST.cc b/src/World_TEST.cc index b1f93b92e..3100ce166 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -19,13 +19,19 @@ #include #include #include +#include +#include +#include #include "sdf/Frame.hh" #include "sdf/Light.hh" #include "sdf/Actor.hh" #include "sdf/Model.hh" +#include "sdf/Link.hh" +#include "sdf/Types.hh" #include "sdf/Physics.hh" #include "sdf/Root.hh" #include "sdf/World.hh" +#include "sdf/Collision.hh" ///////////////////////////////////////////////// TEST(DOMWorld, Construction) @@ -509,6 +515,63 @@ TEST(DOMWorld, AddLight) EXPECT_EQ(lightFromWorld->Name(), light.Name()); } +///////////////////////////////////////////////// +TEST(DOMWorld, ResolveAutoInertials) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " " + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::World *world = root.WorldByIndex(0); + const sdf::Model *model = world->ModelByIndex(0); + const sdf::Link *link = model->LinkByIndex(0); + + root.ResolveAutoInertials(errors, sdfParserConfig); + + const double l = 2; + const double w = 2; + const double h = 2; + + double expectedMass = l*w*h * 1240.0; + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + ASSERT_TRUE(errors.empty()); + EXPECT_EQ(expectedInertial, link->Inertial()); +} + ///////////////////////////////////////////////// TEST(DOMWorld, ToElement) {