diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 5c2275b20..d6b8470c9 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -86,6 +86,18 @@ namespace sdf /// \param[in] _density Density of the collision. public: void SetDensity(double _density); + /// \brief Get the ElementPtr to the element + /// This element can be used as a parent element to hold user-defined + /// params for the custom moment of inertia calculator. + /// \return ElementPtr object for the element. + public: sdf::ElementPtr AutoInertiaParams() const; + + /// \brief Function to set the auto inertia params using a + /// sdf ElementPtr object + /// \param[in] _autoInertiaParams ElementPtr to + /// element + public: void SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams); + /// \brief Get a pointer to the collisions's geometry. /// \return The collision's geometry. public: const Geometry *Geom() const; diff --git a/include/sdf/CustomInertiaCalcProperties.hh b/include/sdf/CustomInertiaCalcProperties.hh new file mode 100644 index 000000000..9b164849f --- /dev/null +++ b/include/sdf/CustomInertiaCalcProperties.hh @@ -0,0 +1,88 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef SDF_CUSTOM_INERTIA_CALC_PROPERTIES_HH_ +#define SDF_CUSTOM_INERTIA_CALC_PROPERTIES_HH_ + +#include + +#include +#include + +#include "sdf/Element.hh" +#include "sdf/Mesh.hh" +#include "sdf/config.hh" +#include "sdf/Types.hh" +#include "sdf/Error.hh" + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE +{ + +// Forward Declarations +class Mesh; + +class SDFORMAT_VISIBLE CustomInertiaCalcProperties +{ + /// \brief Default Constructor + public: CustomInertiaCalcProperties(); + + /// \brief Constructor with mesh properties + /// \param[in] _density Double density value + /// \param[in] _mesh sdf::Mesh object + /// \param[in] _calculatorParams sdf::ElementPtr for calculator params element + public: CustomInertiaCalcProperties(const double _density, + const sdf::Mesh _mesh, + const sdf::ElementPtr _calculatorParams); + + /// \brief Get the density of the mesh. + /// \return Double density of the mesh. + public: double Density() const; + + /// \brief Function to set the density of the interface object + /// \param[in] _density Double density value + public: void SetDensity(double _density); + + /// \brief Get the reference to the mesh oject being used. + /// \return Reference to the sdf::Mesh object. + public: const std::optional &Mesh() const; + + /// \brief Function to set the mesh object + /// \param[in] _mesh sdf::Mesh object + public: void SetMesh(sdf::Mesh &_mesh); + + /// \brief Get the reference to the sdf element. + /// User defined calculator params can be accessed through this element + /// \return sdf::ElementPtr for the tag + public: const sdf::ElementPtr AutoInertiaParams() const; + + /// \brief Function to set the calculator params sdf element object + /// \param[in] _autoInertiaParamsElem sdf::ElementPtr for calculator params + public: void SetAutoInertiaParams(sdf::ElementPtr _autoInertiaParamsElem); + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) +}; + +using CustomInertiaCalculator = + std::function(sdf::Errors &, + const sdf::CustomInertiaCalcProperties &)>; +} +} + +#endif diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 7c6779c2e..35fd1e1cd 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -213,16 +213,16 @@ 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 + /// \brief Calculate and return the Inertial 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. + /// \param[in] _autoInertiaParams ElementPtr to /// \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); + sdf::Errors &_errors, const ParserConfig &_config, + double _density, sdf::ElementPtr _autoInertiaParams); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index a3baca8ba..f19d98818 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -18,8 +18,12 @@ #define SDF_MESH_HH_ #include +#include + #include +#include #include +#include #include #include #include @@ -104,6 +108,20 @@ namespace sdf /// \param[in] _center True to center the submesh. public: void SetCenterSubmesh(const bool _center); + /// \brief Calculate and return the Inertial values for the Mesh + /// \param[out] _errors A vector of Errors object. Each object + /// would contain an error code and an error message. + /// \param[in] _density Density of the mesh in kg/m^3 + /// \param[in] _autoInertiaParams ElementPtr to + /// element + /// \param[in] _config Parser Configuration + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional + CalculateInertial(sdf::Errors &_errors, + double _density, + const sdf::ElementPtr _autoInertiaParams, + 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 /// not been called. diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 24fd4c6b9..635a44294 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -27,6 +27,7 @@ #include "sdf/Error.hh" #include "sdf/InterfaceElements.hh" +#include "sdf/CustomInertiaCalcProperties.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" @@ -194,6 +195,16 @@ class SDFORMAT_VISIBLE ParserConfig /// \return Vector of registered model parser callbacks. public: const std::vector &CustomModelParsers() const; + /// \brief Registers a custom Moment of Inertia Calculator for Meshes + /// \param[in] _inertiaCalculator Callback with signature as described in + /// sdf/CustomInertiaCalcProperties.hh. + public: void RegisterCustomInertiaCalc( + CustomInertiaCalculator _inertiaCalculator); + + /// \brief Get the registered custom mesh MOI Calculator + /// \return registered mesh MOI Calculator. + public: const CustomInertiaCalculator &CustomInertiaCalc() const; + /// \brief Set the preserveFixedJoint flag. /// \param[in] _preserveFixedJoint True to preserve fixed joints, false to /// reduce the fixed joints and merge the child link into the parent. diff --git a/sdf/1.11/collision.sdf b/sdf/1.11/collision.sdf index 496d69a97..425300cff 100644 --- a/sdf/1.11/collision.sdf +++ b/sdf/1.11/collision.sdf @@ -22,6 +22,10 @@ + + Parent tag to hold user-defined custom params for mesh inertia calculator + + diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf index ce2c30f14..99a3538eb 100644 --- a/sdf/1.11/inertial.sdf +++ b/sdf/1.11/inertial.sdf @@ -25,6 +25,14 @@ + + + Parent tag to hold user-defined custom params for mesh inertia calculator + The elements used under this would be overwritten by the elements in auto_inertia_params + in collision. + + + This pose (translation, rotation) describes the position and orientation diff --git a/src/Collision.cc b/src/Collision.cc index 89d15ffce..777e30b79 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -55,6 +55,9 @@ class sdf::Collision::Implementation /// \brief True if density was set during load from sdf. public: bool densitySetAtLoad = false; + /// \brief SDF element pointer to tag + public: sdf::ElementPtr autoInertiaParams{nullptr}; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -157,6 +160,18 @@ void Collision::SetDensity(double _density) this->dataPtr->density = _density; } +///////////////////////////////////////////////// +sdf::ElementPtr Collision::AutoInertiaParams() const +{ + return this->dataPtr->autoInertiaParams; +} + +///////////////////////////////////////////////// +void Collision::SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams) +{ + this->dataPtr->autoInertiaParams = _autoInertiaParams; +} + ///////////////////////////////////////////////// const Geometry *Collision::Geom() const { @@ -248,9 +263,15 @@ void Collision::CalculateInertial( ); } + if (this->dataPtr->sdf->HasElement("auto_inertia_params")) + { + this->dataPtr->autoInertiaParams = + this->dataPtr->sdf->GetElement("auto_inertia_params"); + } + auto geomInertial = this->dataPtr->geom.CalculateInertial(_errors, _config, - this->dataPtr->density); + this->dataPtr->density, this->dataPtr->autoInertiaParams); if (!geomInertial) { diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index b6aed7ec0..07f894a58 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -53,9 +53,18 @@ TEST(DOMcollision, Construction) EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } + EXPECT_EQ(collision.Density(), 1000.0); collision.SetDensity(1240.0); EXPECT_DOUBLE_EQ(collision.Density(), 1240.0); + EXPECT_EQ(collision.AutoInertiaParams(), nullptr); + sdf::ElementPtr autoInertialParamsElem(new sdf::Element()); + autoInertialParamsElem->SetName("auto_inertial_params"); + collision.SetAutoInertiaParams(autoInertialParamsElem); + EXPECT_EQ(collision.AutoInertiaParams(), autoInertialParamsElem); + EXPECT_EQ(collision.AutoInertiaParams()->GetName(), + autoInertialParamsElem->GetName()); + collision.SetRawPose({-10, -20, -30, GZ_PI, GZ_PI, GZ_PI}); EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, GZ_PI, GZ_PI, GZ_PI), collision.RawPose()); @@ -270,6 +279,60 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose()); } +///////////////////////////////////////////////// +TEST(DOMCollision, CalculateInertialWithAutoInertiaParamsElement) +{ + // This example considers a custom voxel-based inertia calculator + // element is used to provide properties + // for the custom calculator like voxel size, grid type, etc. + std::string sdf = "" + " " + " " + " " + " " + " " + " " + " 0.01" + " float" + " " + " 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); + + root.ResolveAutoInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + + sdf::ElementPtr autoInertiaParamsElem = collision->AutoInertiaParams(); + + // element is used as parent element for custom + // intertia calculator params. Custom elements have to be defined with a + // namespace prefix(gz in this case). More about this can be found in the + // following proposal: + // http://sdformat.org/tutorials?tut=custom_elements_attributes_proposal&cat=pose_semantics_docs& + double voxelSize = autoInertiaParamsElem->Get("gz:voxel_size"); + std::string voxelGridType = + autoInertiaParamsElem->Get("gz:voxel_grid_type"); + EXPECT_EQ("float", voxelGridType); + EXPECT_DOUBLE_EQ(0.01, voxelSize); +} + ///////////////////////////////////////////////// TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) { diff --git a/src/CustomInertiaCalcProperties.cc b/src/CustomInertiaCalcProperties.cc new file mode 100644 index 000000000..f5d3985e3 --- /dev/null +++ b/src/CustomInertiaCalcProperties.cc @@ -0,0 +1,94 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include "sdf/CustomInertiaCalcProperties.hh" +#include "sdf/Mesh.hh" +#include "sdf/Element.hh" + +#include + +using namespace sdf; + +class CustomInertiaCalcProperties::Implementation +{ + /// \brief Density of the mesh. 1000 kg/m^3 by default + public: double density{1000.0}; + + /// \brief Optional SDF mesh object. Default is std::nullopt + public: std::optional mesh{std::nullopt}; + + /// \brief SDF element pointer to tag. + /// This can be used to access custom params for the + /// Inertia Caluclator + public: sdf::ElementPtr inertiaCalculatorParams{nullptr}; +}; + +///////////////////////////////////////////////// +CustomInertiaCalcProperties::CustomInertiaCalcProperties() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +CustomInertiaCalcProperties::CustomInertiaCalcProperties(const double _density, + const sdf::Mesh _mesh, + const sdf::ElementPtr _calculatorParams) + : dataPtr(gz::utils::MakeImpl()) +{ + this->dataPtr->density = _density; + this->dataPtr->mesh = _mesh; + this->dataPtr->inertiaCalculatorParams = _calculatorParams; +} + +///////////////////////////////////////////////// +double CustomInertiaCalcProperties::Density() const +{ + return this->dataPtr->density; +} + +///////////////////////////////////////////////// +void CustomInertiaCalcProperties::SetDensity(double _density) +{ + this->dataPtr->density = _density; +} + +///////////////////////////////////////////////// +const std::optional &CustomInertiaCalcProperties::Mesh() const +{ + return this->dataPtr->mesh; +} + +///////////////////////////////////////////////// +void CustomInertiaCalcProperties::SetMesh(sdf::Mesh &_mesh) +{ + this->dataPtr->mesh = _mesh; +} + +///////////////////////////////////////////////// +const sdf::ElementPtr CustomInertiaCalcProperties::AutoInertiaParams() const +{ + return this->dataPtr->inertiaCalculatorParams; +} + +///////////////////////////////////////////////// +void CustomInertiaCalcProperties::SetAutoInertiaParams( + sdf::ElementPtr _autoInertiaParamsElem) +{ + this->dataPtr->inertiaCalculatorParams = _autoInertiaParamsElem; +} diff --git a/src/CustomInertiaCalcProperties_TEST.cc b/src/CustomInertiaCalcProperties_TEST.cc new file mode 100644 index 000000000..06a1b64c6 --- /dev/null +++ b/src/CustomInertiaCalcProperties_TEST.cc @@ -0,0 +1,143 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include +#include + +#include + +///////////////////////////////////////////////// +TEST(DOMCustomInertiaCalcProperties, Construction) +{ + sdf::CustomInertiaCalcProperties customInertiaCaclProps; + + EXPECT_DOUBLE_EQ(customInertiaCaclProps.Density(), 1000.0); + customInertiaCaclProps.SetDensity(1240.0); + EXPECT_DOUBLE_EQ(customInertiaCaclProps.Density(), 1240.0); + + EXPECT_EQ(customInertiaCaclProps.AutoInertiaParams(), nullptr); + sdf::ElementPtr autoInertiaParamsElem = sdf::ElementPtr(new sdf::Element); + autoInertiaParamsElem->SetName("auto_inertia_params"); + customInertiaCaclProps.SetAutoInertiaParams(autoInertiaParamsElem); + EXPECT_EQ(customInertiaCaclProps.AutoInertiaParams(), + autoInertiaParamsElem); + EXPECT_EQ(customInertiaCaclProps.AutoInertiaParams()->GetName(), + autoInertiaParamsElem->GetName()); + + EXPECT_EQ(customInertiaCaclProps.Mesh(), std::nullopt); + sdf::Mesh mesh; + mesh.SetScale(gz::math::Vector3d(2, 2, 2)); + customInertiaCaclProps.SetMesh(mesh); + EXPECT_EQ(customInertiaCaclProps.Mesh()->Scale(), mesh.Scale()); +} + +///////////////////////////////////////////////// +TEST(DOMCustomInertiaCalcProperties, ConstructionWithMeshConstructor) +{ + double density = 1240.0; + + sdf::Mesh mesh; + mesh.SetScale(gz::math::Vector3d(2, 2, 2)); + + sdf::ElementPtr autoInertiaParamsElem = sdf::ElementPtr(new sdf::Element); + autoInertiaParamsElem->SetName("auto_inertia_params"); + + sdf::CustomInertiaCalcProperties customInertiaCalcProps( + density, + mesh, + autoInertiaParamsElem + ); + + EXPECT_DOUBLE_EQ(customInertiaCalcProps.Density(), density); + EXPECT_EQ(customInertiaCalcProps.Mesh()->Scale(), mesh.Scale()); + EXPECT_EQ(customInertiaCalcProps.AutoInertiaParams(), + autoInertiaParamsElem); +} + +///////////////////////////////////////////////// +TEST(DOMCustomInertiaCalcProperties, CopyConstructor) +{ + sdf::CustomInertiaCalcProperties customInertiaCalcProps; + customInertiaCalcProps.SetDensity(1240.0); + + sdf::CustomInertiaCalcProperties customInertiaCalcProps2( + customInertiaCalcProps); + EXPECT_DOUBLE_EQ(customInertiaCalcProps2.Density(), + customInertiaCalcProps.Density()); +} + +///////////////////////////////////////////////// +TEST(DOMCustomInertiaCalcProperties, CopyAssignmentOperator) +{ + sdf::CustomInertiaCalcProperties customInertiaCalcProps; + customInertiaCalcProps.SetDensity(1240.0); + + sdf::CustomInertiaCalcProperties customInertiaCalcProps2; + customInertiaCalcProps2 = customInertiaCalcProps; + + EXPECT_DOUBLE_EQ(customInertiaCalcProps2.Density(), + customInertiaCalcProps.Density()); +} + +/////////////////////////////////////////////// +TEST(DOMCustomInertiaCalcProperties, MoveConstructor) +{ + sdf::CustomInertiaCalcProperties customInertiaCalcProps; + customInertiaCalcProps.SetDensity(1240.0); + + sdf::CustomInertiaCalcProperties customInertiaCalcProps2( + std::move(customInertiaCalcProps)); + + EXPECT_DOUBLE_EQ(customInertiaCalcProps2.Density(), 1240.0); +} + +///////////////////////////////////////////////// +TEST(DOMCustomInertiaCalcProperties, MoveAssignmentOperator) +{ + sdf::CustomInertiaCalcProperties customInertiaCalcProps; + customInertiaCalcProps.SetDensity(1240.0); + + sdf::CustomInertiaCalcProperties customInertiaCalcProps2; + customInertiaCalcProps2 = std::move(customInertiaCalcProps); + + EXPECT_DOUBLE_EQ(customInertiaCalcProps2.Density(), 1240.0); +} + +///////////////////////////////////////////////// +TEST(DOMCustomInertiaCalcProperties, CopyAssignmentAfterMove) +{ + sdf::CustomInertiaCalcProperties customInertiaCalcProps1; + customInertiaCalcProps1.SetDensity(1240.0); + + sdf::CustomInertiaCalcProperties customInertiaCalcProps2; + customInertiaCalcProps2.SetDensity(2710.0); + + EXPECT_DOUBLE_EQ(2710.0, customInertiaCalcProps2.Density()); + EXPECT_DOUBLE_EQ(1240.0, customInertiaCalcProps1.Density()); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::CustomInertiaCalcProperties tmp = std::move(customInertiaCalcProps1); + customInertiaCalcProps1 = customInertiaCalcProps2; + customInertiaCalcProps2 = tmp; + + EXPECT_DOUBLE_EQ(2710.0, customInertiaCalcProps1.Density()); + EXPECT_DOUBLE_EQ(1240.0, customInertiaCalcProps2.Density()); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index fb3a56d52..ab169905d 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -313,7 +313,8 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) } std::optional Geometry::CalculateInertial( - sdf::Errors &_errors, const sdf::ParserConfig &_config, double _density) + sdf::Errors &_errors, const ParserConfig &_config, + double _density, sdf::ElementPtr _autoInertiaParams) { std::optional geomInertial; @@ -334,6 +335,12 @@ std::optional Geometry::CalculateInertial( case GeometryType::SPHERE: geomInertial = this->dataPtr->sphere->CalculateInertial(_density); break; + case GeometryType::MESH: + geomInertial = + this->dataPtr->mesh->CalculateInertial(_errors, _density, + _autoInertiaParams, + _config); + break; default: Error invalidGeomTypeErr( ErrorCode::WARNING, diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 6c3f724fe..7b12f878a 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -29,6 +29,7 @@ #include "sdf/ParserConfig.hh" #include "sdf/Types.hh" #include "sdf/Element.hh" +#include "sdf/CustomInertiaCalcProperties.hh" #include "test_utils.hh" #include #include @@ -315,14 +316,15 @@ TEST(DOMGeometry, CalculateInertial) double expectedMass; gz::math::MassMatrix3d expectedMassMat; gz::math::Inertiald expectedInertial; - const sdf::ParserConfig sdfParserConfig; + sdf::ParserConfig sdfParserConfig; + sdf::ElementPtr autoInertiaParams; sdf::Errors errors; // Not supported geom type { geom.SetType(sdf::GeometryType::EMPTY); auto notSupportedInertial = geom.CalculateInertial(errors, - sdfParserConfig, density); + sdfParserConfig, density, autoInertiaParams); ASSERT_EQ(notSupportedInertial, std::nullopt); } @@ -349,7 +351,7 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::BOX); geom.SetBoxShape(box); auto boxInertial = geom.CalculateInertial(errors, - sdfParserConfig, density); + sdfParserConfig, density, autoInertiaParams); ASSERT_NE(std::nullopt, boxInertial); EXPECT_EQ(expectedInertial, *boxInertial); @@ -385,7 +387,7 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::CAPSULE); geom.SetCapsuleShape(capsule); auto capsuleInertial = geom.CalculateInertial(errors, - sdfParserConfig, density); + sdfParserConfig, density, autoInertiaParams); ASSERT_NE(std::nullopt, capsuleInertial); EXPECT_EQ(expectedInertial, *capsuleInertial); @@ -416,7 +418,7 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::CYLINDER); geom.SetCylinderShape(cylinder); auto cylinderInertial = geom.CalculateInertial(errors, - sdfParserConfig, density); + sdfParserConfig, density, autoInertiaParams); ASSERT_NE(std::nullopt, cylinderInertial); EXPECT_EQ(expectedInertial, *cylinderInertial); @@ -449,7 +451,7 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::ELLIPSOID); geom.SetEllipsoidShape(ellipsoid); auto ellipsoidInertial = geom.CalculateInertial(errors, - sdfParserConfig, density); + sdfParserConfig, density, autoInertiaParams); ASSERT_NE(std::nullopt, ellipsoidInertial); EXPECT_EQ(expectedInertial, *ellipsoidInertial); @@ -478,13 +480,65 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::SPHERE); geom.SetSphereShape(sphere); auto sphereInertial = geom.CalculateInertial(errors, - sdfParserConfig, density); + sdfParserConfig, density, autoInertiaParams); ASSERT_NE(std::nullopt, sphereInertial); EXPECT_EQ(expectedInertial, *sphereInertial); EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); EXPECT_EQ(expectedInertial.Pose(), sphereInertial->Pose()); } + + // Mesh + { + sdf::Mesh mesh; + sdf::CustomInertiaCalcProperties inertiaCalcProps; + + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + + // Test Lambda function for registering as custom inertia calculator + auto customMeshInertiaCalculator = []( + sdf::Errors &_errors, + const sdf::CustomInertiaCalcProperties &_inertiaProps + ) -> std::optional + { + if (_inertiaProps.Density() <= 0) + { + _errors.push_back( + {sdf::ErrorCode::LINK_INERTIA_INVALID, + "Inertia is invalid"}); + return std::nullopt; + } + + gz::math::Inertiald meshInerial; + + meshInerial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + + return meshInerial; + }; + + // Register the lambda function as Custom Inertia Calculator + sdfParserConfig.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + + // File Path needs to be considered as a valid mesh + mesh.SetFilePath("/some_path"); + + geom.SetType(sdf::GeometryType::MESH); + geom.SetMeshShape(mesh); + + auto meshInertial = geom.CalculateInertial(errors, + sdfParserConfig, density, autoInertiaParams); + + EXPECT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(meshInertial->MassMatrix().Mass(), 1.0); + EXPECT_EQ(meshInertial->MassMatrix().DiagonalMoments(), + gz::math::Vector3d::One); + } } ///////////////////////////////////////////////// diff --git a/src/Link.cc b/src/Link.cc index 7d8d5a182..d56792a06 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -597,7 +597,8 @@ Errors Link::ResolveInertial( } ///////////////////////////////////////////////// -void Link::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) +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 diff --git a/src/Mesh.cc b/src/Mesh.cc index 25b1d1ac0..5e598345b 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -14,8 +14,15 @@ * limitations under the License. * */ + +#include + +#include +#include "sdf/CustomInertiaCalcProperties.hh" #include "sdf/parser.hh" #include "sdf/Mesh.hh" +#include "sdf/Element.hh" +#include "sdf/ParserConfig.hh" #include "Utils.hh" using namespace sdf; @@ -187,6 +194,27 @@ void Mesh::SetCenterSubmesh(const bool _center) this->dataPtr->centerSubmesh = _center; } +////////////////////////////////////////////////// +std::optional Mesh::CalculateInertial(sdf::Errors &_errors, + double _density, const sdf::ElementPtr _autoInertiaParams, + const ParserConfig &_config) +{ + if (this->dataPtr->filePath.empty()) + { + _errors.push_back({ + sdf::ErrorCode::WARNING, + "File Path for the mesh was empty. Could not calculate inertia"}); + return std::nullopt; + } + + const auto &customCalculator = _config.CustomInertiaCalc(); + + sdf::CustomInertiaCalcProperties calcInterface = CustomInertiaCalcProperties( + _density, *this, _autoInertiaParams); + + return customCalculator(_errors, calcInterface); +} + ///////////////////////////////////////////////// sdf::ElementPtr Mesh::ToElement() const { diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 35d374436..7a8c4c4ca 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -14,10 +14,19 @@ * limitations under the License. * */ +#include #include #include "sdf/Mesh.hh" #include "test_utils.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/CustomInertiaCalcProperties.hh" +#include "sdf/Types.hh" +#include "sdf/Error.hh" + +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMMesh, Construction) @@ -181,6 +190,110 @@ TEST(DOMMesh, Load) EXPECT_NE(nullptr, mesh.Element()); } +///////////////////////////////////////////////// +TEST(DOMMesh, CalcualteInertial) +{ + sdf::Mesh mesh; + sdf::ParserConfig config; + sdf::Errors errors; + sdf::CustomInertiaCalcProperties inertiaCalcProps; + + double density = 0; + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + + // File Path needs to be considered as a valid mesh + mesh.SetFilePath("/some_path"); + + // Test Lambda function for registering as custom inertia calculator + auto customMeshInertiaCalculator = []( + sdf::Errors &_errors, + const sdf::CustomInertiaCalcProperties &_inertiaProps + ) -> std::optional + { + if (_inertiaProps.Density() <= 0) + { + _errors.push_back( + {sdf::ErrorCode::LINK_INERTIA_INVALID, + "Inertia is invalid"}); + return std::nullopt; + } + + gz::math::Inertiald meshInerial; + + meshInerial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + + return meshInerial; + }; + + config.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + + auto meshInertial = mesh.CalculateInertial(errors, + density, autoInertiaParamsElem, config); + ASSERT_FALSE(errors.empty()); + ASSERT_EQ(meshInertial, std::nullopt); + + density = 1240.0; + sdf::Errors errors2; + auto meshInertial2 = mesh.CalculateInertial(errors2, + density, autoInertiaParamsElem, config); + ASSERT_TRUE(errors2.empty()); + EXPECT_DOUBLE_EQ(meshInertial2->MassMatrix().Mass(), 1.0); + EXPECT_EQ(meshInertial2->MassMatrix().DiagonalMoments(), + gz::math::Vector3d::One); +} + +///////////////////////////////////////////////// +TEST(DOMMesh, CalculateInertiaWithEmptyFilePath) +{ + sdf::Mesh mesh; + sdf::ParserConfig config; + sdf::Errors errors; + sdf::CustomInertiaCalcProperties inertiaCalcProps; + + double density = 0; + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + + // Test Lambda function for registering as custom inertia calculator + auto customMeshInertiaCalculator = []( + sdf::Errors &_errors, + const sdf::CustomInertiaCalcProperties &_inertiaProps + ) -> std::optional + { + if (_inertiaProps.Density() <= 0) + { + _errors.push_back( + {sdf::ErrorCode::LINK_INERTIA_INVALID, + "Inertia is invalid"}); + return std::nullopt; + } + + gz::math::Inertiald meshInerial; + + meshInerial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + + return meshInerial; + }; + + config.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + + auto meshInertial = mesh.CalculateInertial(errors, + density, autoInertiaParamsElem, config); + ASSERT_FALSE(errors.empty()); + ASSERT_EQ(meshInertial, std::nullopt); +} + ///////////////////////////////////////////////// TEST(DOMMesh, ToElement) { diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index 96059afc6..209de15c5 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -20,6 +20,7 @@ #include "sdf/ParserConfig.hh" #include "sdf/Filesystem.hh" #include "sdf/Types.hh" +#include "sdf/CustomInertiaCalcProperties.hh" using namespace sdf; @@ -52,6 +53,9 @@ class sdf::ParserConfig::Implementation /// \brief Collection of custom model parsers. public: std::vector customParsers; + /// \brief Collection of custom model parsers. + public: CustomInertiaCalculator customInertiaCalculator; + /// \brief Flag to explicitly preserve fixed joints when /// reading the SDF/URDF file. public: bool preserveFixedJoint = false; @@ -164,7 +168,8 @@ EnforcementPolicy ParserConfig::DeprecatedElementsPolicy() const } ///////////////////////////////////////////////// -ConfigureResolveAutoInertials ParserConfig::CalculateInertialConfiguration() const +ConfigureResolveAutoInertials + ParserConfig::CalculateInertialConfiguration() const { return this->dataPtr->resolveAutoInertialsConfig; } @@ -188,6 +193,19 @@ const std::vector &ParserConfig::CustomModelParsers() const return this->dataPtr->customParsers; } +///////////////////////////////////////////////// +void ParserConfig::RegisterCustomInertiaCalc( + CustomInertiaCalculator _inertiaCalculator) +{ + this->dataPtr->customInertiaCalculator = _inertiaCalculator; +} + +///////////////////////////////////////////////// +const CustomInertiaCalculator &ParserConfig::CustomInertiaCalc() const +{ + return this->dataPtr->customInertiaCalculator; +} + ///////////////////////////////////////////////// void ParserConfig::URDFSetPreserveFixedJoint(bool _preserveFixedJoint) { diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 00add6e3b..65719e4ef 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -63,7 +63,6 @@ TEST(ParserConfig, Construction) 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 7a37f1466..3230184de 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -598,7 +598,8 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, } ///////////////////////////////////////////////// -void Root::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) +void Root::ResolveAutoInertials(sdf::Errors &_errors, + const ParserConfig &_config) { // Calculate and set Inertials for all the worlds for (sdf::World &world : this->dataPtr->worlds)