From 7de58db5aa5e2d85a4ffe713ba9ce30be4ef3ccc Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 30 Aug 2023 20:43:34 +0530 Subject: [PATCH] Automatic Moment of Inertia Calculations for Basic Shapes (#1299) This PR adds Automatic Moment of Inertia Calculations for Basic Shapes (Box, Capsule, Cylinder, Ellipsoid and Sphere). As mentioned in the proposal the PR adds the following to the SDF Spec: `//inertial/@auto` attribute to enable automatic calculations `//link/inertial/density` tag for mentioning the density for the collision `//link/collision/density` tag for overriding the value density set at the `//link/inertial/density`. --------- Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 11 +++ include/sdf/Capsule.hh | 11 +++ include/sdf/Collision.hh | 21 ++++ include/sdf/Cylinder.hh | 11 +++ include/sdf/Ellipsoid.hh | 11 +++ include/sdf/Geometry.hh | 15 +++ include/sdf/Link.hh | 38 +++++++ include/sdf/Model.hh | 8 ++ include/sdf/ParserConfig.hh | 25 +++++ include/sdf/Root.hh | 11 +++ include/sdf/Sphere.hh | 11 +++ include/sdf/World.hh | 8 ++ sdf/1.11/collision.sdf | 8 ++ sdf/1.11/inertial.sdf | 16 +++ src/Box.cc | 25 +++++ src/Box_TEST.cc | 50 ++++++++++ src/Capsule.cc | 24 +++++ src/Capsule_TEST.cc | 50 ++++++++++ src/Collision.cc | 85 ++++++++++++++++ src/Collision_TEST.cc | 163 ++++++++++++++++++++++++++++++ src/Cylinder.cc | 22 +++++ src/Cylinder_TEST.cc | 51 ++++++++++ src/Ellipsoid.cc | 24 +++++ src/Ellipsoid_TEST.cc | 51 ++++++++++ src/Geometry.cc | 42 ++++++++ src/Geometry_TEST.cc | 191 +++++++++++++++++++++++++++++++++++- src/Link.cc | 127 +++++++++++++++++++++--- src/Link_TEST.cc | 166 +++++++++++++++++++++++++++++++ src/Model.cc | 17 ++++ src/ParserConfig.cc | 19 ++++ src/ParserConfig_TEST.cc | 8 ++ src/Root.cc | 24 +++++ src/Root_TEST.cc | 40 ++++++++ src/Sphere.cc | 24 +++++ src/Sphere_TEST.cc | 46 ++++++++- src/World.cc | 11 +++ src/World_TEST.cc | 63 ++++++++++++ 37 files changed, 1513 insertions(+), 15 deletions(-) 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) {