Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Callback based API for Implementing Custom Moment of Inertia Calculators for Meshes #1304

Merged
merged 142 commits into from
Aug 30, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
142 commits
Select commit Hold shift + click to select a range
93e497e
Copy sdf 1.10 to 1.11
jasmeet0915 Jun 13, 2023
bf33df2
Bumped sdf 1.10 to sdf 1.11
jasmeet0915 Jun 19, 2023
9450574
Update SDF schema version in CMakeLists.txt
jasmeet0915 Jun 27, 2023
0841705
Added <material_density> element to <collision
jasmeet0915 Jun 19, 2023
1c27bfc
Added auto attribute to <inertia> tag
jasmeet0915 Jun 19, 2023
8a1f8a7
Added gz::math::Material member to Collision
jasmeet0915 Jun 26, 2023
e3fc2a3
Added MassMatrix() functions to Box, Capsule, Cylinder, Ellipsoid & S…
jasmeet0915 Jul 3, 2023
dbe7d3e
Added MassMatrix() functions to sdf::Geometry class
jasmeet0915 Jul 3, 2023
45a67bd
Replaced gz::math::Material member of sdf::Collision with double density
jasmeet0915 Jul 3, 2023
db76c5f
Added getter setter functions for density in sdf::Collision
jasmeet0915 Jul 3, 2023
7290483
Added MassMatrix() function to sdf::Collision class
jasmeet0915 Jul 3, 2023
c8120ae
Update Link::Load() to check for inertia auto attr & call MassMatrix …
jasmeet0915 Jul 3, 2023
42a4707
Cleaned up the Code
jasmeet0915 Jul 13, 2023
bbacbe9
Corrected default density value
jasmeet0915 Jul 17, 2023
b092904
Corrected function comments
jasmeet0915 Jul 17, 2023
6827d9a
Added ParserConfig param to Collision & Geometry MassMatrix()
jasmeet0915 Jul 18, 2023
85b3149
Added function to register & get CustomMoiCalculator in ParserConfig
jasmeet0915 Jul 18, 2023
49c6528
Added MassMatrix() function to sdf::Mesh
jasmeet0915 Jul 18, 2023
7b33556
Added <moi_calculator_params> to <collision
jasmeet0915 Jul 24, 2023
408b643
Added InterfaceMoiCalculator class
jasmeet0915 Jul 24, 2023
0bc1cf6
Update ParserConfig to use InterfaceMoiCalculator
jasmeet0915 Jul 24, 2023
a1db03b
Update Geometry::MassMatrix() to pass calculator params sdf element t…
jasmeet0915 Jul 24, 2023
d086d68
Update Mesh::MassMatrix to pass InterfaceMoiCalculator object to calc…
jasmeet0915 Jul 24, 2023
c3c52aa
Changed <material_density> tag to <density>
jasmeet0915 Jul 31, 2023
8ff8726
Updated Collision:MassMatrix() to use gz::math::Inertial object as pa…
jasmeet0915 Jul 13, 2023
0cd267b
Updated Link::Load() to add inertials from multiple collisions
jasmeet0915 Jul 13, 2023
6881c2d
Shifted auto attribute from <inertia> to <inertial>
jasmeet0915 Aug 4, 2023
dd0551a
Added CalculateInertial() function to sdf::Root
jasmeet0915 Aug 7, 2023
8d0c084
Added CalculateInertial() function to sdf::Model
jasmeet0915 Aug 7, 2023
942bf78
Added CalculateInertial() function to sdf::World
jasmeet0915 Aug 7, 2023
1eeed1e
Added CalculateInertial() function to sdf::Link to check for auto and…
jasmeet0915 Aug 7, 2023
b6a8a7b
Update Collision::MassMatrix() to set inertial pose using collision p…
jasmeet0915 Aug 7, 2023
33d9979
Called World::CalculateInertials() in Root::Load() function
jasmeet0915 Aug 8, 2023
76842fe
Added check for no collisions when auto is true & return an Element M…
jasmeet0915 Aug 8, 2023
f486a1b
Used enforcePolicyCondition() to print <density> element missing mess…
jasmeet0915 Aug 10, 2023
429eb2f
Restored original flow of Link::Load() to set inertial values & updat…
jasmeet0915 Aug 10, 2023
b349c0d
Renamed & changed MassMatrix() functions for all shapes to return gz:…
jasmeet0915 Aug 10, 2023
5f11863
Renamed & updated MassMatrix() function for sdf::Geometry to return o…
jasmeet0915 Aug 10, 2023
8214ae6
Renamed & Updated MassMatrix() function of sdf::Collision to use Iner…
jasmeet0915 Aug 10, 2023
0d1e1bc
Updated call to collision inertia calculation function with the new name
jasmeet0915 Aug 10, 2023
b1f7a51
Removed print statements
jasmeet0915 Aug 10, 2023
702dd9f
Completed codecheck
jasmeet0915 Aug 10, 2023
67126dd
Merge branch 'main' into jasmeet/auto_moment_of_inertia
jasmeet0915 Aug 10, 2023
e90455a
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 10, 2023
0a6a528
Removed const type qualifier from double density param for CalculateI…
jasmeet0915 Aug 11, 2023
138bdb4
Shifted check for density element from Collision::Load() to Collision…
jasmeet0915 Aug 11, 2023
fa1da0b
Shifted CalculateInertials() call to end of Root:Load()
jasmeet0915 Aug 11, 2023
9c6f8c5
Update CalculateInertials() function in Root, World, Model, Link & Co…
jasmeet0915 Aug 11, 2023
1a483a2
Completed codecheck
jasmeet0915 Aug 11, 2023
d1fdf8a
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 11, 2023
2f0c840
Removed const specifier for double density from Geometry & Collision
jasmeet0915 Aug 11, 2023
414ffd4
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 11, 2023
4d08091
Updated CustomMOICalculator function signature to return Inertial object
jasmeet0915 Aug 14, 2023
373fa3f
Updated Mesh::CalculateInertial() to return inertial objecT
jasmeet0915 Aug 14, 2023
8088f4b
Updated Geometry::CalculateInertial() to take calculator params Eleme…
jasmeet0915 Aug 14, 2023
7b2e37b
Shifted check for moi_calculator_params element to Collision::Calcula…
jasmeet0915 Aug 14, 2023
ac943c6
Corrected function comments
jasmeet0915 Aug 14, 2023
8fef740
Included <optional> header
jasmeet0915 Aug 14, 2023
0f98215
Renamed <moi_calculator_params> element to <auto_inertia_params>
jasmeet0915 Aug 14, 2023
1f16e1f
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 14, 2023
17d60e1
Renamed CustomMOICalculator std::function alias to CustomInertiaCalcu…
jasmeet0915 Aug 16, 2023
b9a6698
Added check for empty mesh filePath before calling custom calculator
jasmeet0915 Aug 16, 2023
00c1496
Renamed InterfaceMOICalculator class to CustomInertiaCalcProperties
jasmeet0915 Aug 16, 2023
b23abc6
Removed std::cout statements
jasmeet0915 Aug 16, 2023
5c44ff0
Removed const specifier for double density
jasmeet0915 Aug 16, 2023
5e3d708
Completed codecheck
jasmeet0915 Aug 16, 2023
a929478
Added default values for density & calculatorParams ElementPtr
jasmeet0915 Aug 16, 2023
746d86e
Updated function comments
jasmeet0915 Aug 16, 2023
80e321c
Removed warning for missing <inertial> element
jasmeet0915 Aug 17, 2023
e3041e4
Added warning for overwritting of user given inertial value sif auto …
jasmeet0915 Aug 17, 2023
cc9849c
Removed call to CalculateInertial() from Root::Load()
jasmeet0915 Aug 17, 2023
0e41116
Corrected codecheck
jasmeet0915 Aug 21, 2023
4731e21
Added unit tests for CalculateInertial() function in Box, Capsule, Cy…
jasmeet0915 Aug 22, 2023
3aefdc3
Added unit tests for CalculateInertial() in Geometry
jasmeet0915 Aug 22, 2023
2836be9
Updated construction unit test of Collision with checks for density v…
jasmeet0915 Aug 22, 2023
dbe543f
Updated CalculateInertial() unit test to test for std::nullopt return…
jasmeet0915 Aug 22, 2023
8d459c7
Added unit tests for CalculateInertial() in sdf::Collision
jasmeet0915 Aug 22, 2023
464f7de
Completed codecheck
jasmeet0915 Aug 22, 2023
5af78f7
Merge branch 'main' into jasmeet/auto_moment_of_inertia
jasmeet0915 Aug 22, 2023
485dd84
Added enum class for CalculateInertial() configuration
jasmeet0915 Aug 24, 2023
ca78026
Added check for CalculateInertialConfiguration in Root::Load()
jasmeet0915 Aug 24, 2023
002c643
Renamed CALCULATE_AND_SAVE value to SAVE_CALCULATION in COnfigureCalc…
jasmeet0915 Aug 24, 2023
1688522
Added boolean autoInertiaSaved member to link
jasmeet0915 Aug 24, 2023
230bde2
Added check for autoInertiaSaved & SAVE_CALCULATION configuration in …
jasmeet0915 Aug 24, 2023
0df7eae
Completed codecheck
jasmeet0915 Aug 24, 2023
5b58b11
Merge branch 'main' into jasmeet/auto_moment_of_inertia
jasmeet0915 Aug 24, 2023
dd06f08
Added //inertial/density to the spec
jasmeet0915 Aug 24, 2023
325854c
Updated Geometry::CalculateInertial() to accept density & parser conf…
jasmeet0915 Aug 24, 2023
fc01633
Updated Geometry::CalculateInertial() to give warning when a not supp…
jasmeet0915 Aug 24, 2023
d1e7484
Updated Geometry unit test for not supported geom type
jasmeet0915 Aug 24, 2023
88d5eeb
Updated Collision::ToElement() to set density
jasmeet0915 Aug 24, 2023
0ec1aac
Added unit tests for collision pose relative to some other frame test…
jasmeet0915 Aug 24, 2023
dc30cf9
Completed codecheck
jasmeet0915 Aug 24, 2023
40811cf
Added unit tests for ParserConfig
jasmeet0915 Aug 24, 2023
01c49c4
Added unit tests for Link
jasmeet0915 Aug 24, 2023
b5648ff
Replaced tabs with spaces in sdf string in Link_Test
jasmeet0915 Aug 24, 2023
6a65644
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 24, 2023
4a2357a
Updated ParserConfig Unit test to test SetCalculateInertialConfigurat…
jasmeet0915 Aug 24, 2023
60131dd
Added test for CalculateInertial() called with auto set to false
jasmeet0915 Aug 24, 2023
a3cb221
Added unit test for World::CalculateInertial()
jasmeet0915 Aug 24, 2023
7483b89
Added unit test for CalculateInertial() call with SAVE_CALCULATION co…
jasmeet0915 Aug 24, 2023
5aa5e42
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 24, 2023
bd72ac8
Updated CalculateInertial() calls in Geometry Unit tests
jasmeet0915 Aug 24, 2023
e772c24
Included missing header files
jasmeet0915 Aug 25, 2023
f108330
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 25, 2023
3c1f29e
Included missing header files in Mesh and CustomInertiCalcProperties
jasmeet0915 Aug 25, 2023
3c22811
Made model member in CustomInertiaCalcProperties as optional
jasmeet0915 Aug 25, 2023
55d5ef1
Added unit test for CustomInertiaCalcProperties
jasmeet0915 Aug 25, 2023
c503112
Added default nullptr value for autoInertiaParams elementptr
jasmeet0915 Aug 25, 2023
4e2371d
Added unit test for the use of <auto_inertia_params> element
jasmeet0915 Aug 25, 2023
db5a4e2
Added sdf::Errors object as parameter for Mesh::CalculateInertial()
jasmeet0915 Aug 25, 2023
70d9576
Codecheck corrections
jasmeet0915 Aug 25, 2023
7850bb2
Updated Geometry CalculateInertial() Unit test for mesh geom type
jasmeet0915 Aug 25, 2023
c65a0a5
Added unit tests for Mesh::CalculateInertial()
jasmeet0915 Aug 25, 2023
1112607
Added <auto_inertia_params> element to <inertial> element
jasmeet0915 Aug 25, 2023
f3cd3be
Removed redundant sdf/Model.hh include from Collision_TEST
jasmeet0915 Aug 26, 2023
c5e181e
Merge branch 'main' into jasmeet/mesh_moi_calculator_api
jasmeet0915 Aug 27, 2023
ef8ae18
Merge branch 'main' into jasmeet/auto_moment_of_inertia
jasmeet0915 Aug 27, 2023
16ce852
Updated CalculateInertial() documentation for Box, Capsule, Cylinder,…
jasmeet0915 Aug 28, 2023
65c3703
Updated Collision::Density() & Collision::SetDensity() docs
jasmeet0915 Aug 28, 2023
fee0d90
Updated Root::CalculateInertial() docs
jasmeet0915 Aug 28, 2023
ff88720
Updated description for <density> element in <inertial>
jasmeet0915 Aug 28, 2023
af8052e
Added doc for autoInertiaSaved variable in sdf::Link
jasmeet0915 Aug 28, 2023
943b886
Update function APIs
jasmeet0915 Aug 28, 2023
a658c25
Added flag to track if density was set at load
jasmeet0915 Aug 28, 2023
d652717
Added separate variable to track if auto inertia is enabled or not
jasmeet0915 Aug 28, 2023
ec62766
Completed codecheck
jasmeet0915 Aug 28, 2023
6818408
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 28, 2023
adc2813
Corrected function docs in Mesh & Geometry class
jasmeet0915 Aug 29, 2023
c8f9fc4
Updated Collision::CalculateInertial() API
jasmeet0915 Aug 29, 2023
b05e267
Updated check for geometry type before resolving inertial pose
jasmeet0915 Aug 29, 2023
3a21b69
Updated Root::CalculateInertial() docs
jasmeet0915 Aug 29, 2023
91c828a
Added const specifier to variables where needed
jasmeet0915 Aug 29, 2023
563da4b
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 29, 2023
1986cfd
Rename CalculateInertials() API in Root, World, Model & Link
jasmeet0915 Aug 30, 2023
8953e10
Merge branch 'jasmeet/auto_moment_of_inertia' into jasmeet/mesh_moi_c…
jasmeet0915 Aug 30, 2023
cb9f54e
Rename function call in Collision_TEST
jasmeet0915 Aug 30, 2023
bf9bfe9
Merge branch 'sdf14' into jasmeet/mesh_moi_calculator_api
jasmeet0915 Aug 30, 2023
a414168
Completed codecheck
jasmeet0915 Aug 30, 2023
e921982
Updated //inertial/auto_inertia_params element description
jasmeet0915 Aug 30, 2023
4de7035
Updated Mesh::CalculateInertial() API to take errors as first param
jasmeet0915 Aug 30, 2023
9a49998
Completed codecheck
jasmeet0915 Aug 30, 2023
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions include/sdf/Collision.hh
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,18 @@ namespace sdf
/// \param[in] _density Density of the collision.
public: void SetDensity(double _density);

/// \brief Get the ElementPtr to the <auto_inertia_params> 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 <auto_inertia_params> element.
public: sdf::ElementPtr AutoInertiaParams() const;

/// \brief Function to set the auto inertia params using a
/// sdf ElementPtr object
/// \param[in] _autoInertiaParams ElementPtr to <auto_inertia_params>
/// 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;
Expand Down
88 changes: 88 additions & 0 deletions include/sdf/CustomInertiaCalcProperties.hh
Original file line number Diff line number Diff line change
@@ -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 <optional>

#include <gz/utils/ImplPtr.hh>
#include <gz/math/Inertial.hh>

#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<sdf::Mesh> &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 <auto_inertia_params> 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 =
azeey marked this conversation as resolved.
Show resolved Hide resolved
std::function<std::optional<gz::math::Inertiald>(sdf::Errors &,
const sdf::CustomInertiaCalcProperties &)>;
}
}

#endif
8 changes: 4 additions & 4 deletions include/sdf/Geometry.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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.
quarkytale marked this conversation as resolved.
Show resolved Hide resolved
/// \param[in] _autoInertiaParams ElementPtr to <auto_inertia_params>
/// \return A std::optional with gz::math::Inertiald object or std::nullopt
public: std::optional<gz::math::Inertiald> CalculateInertial(
azeey marked this conversation as resolved.
Show resolved Hide resolved
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.
Expand Down
18 changes: 18 additions & 0 deletions include/sdf/Mesh.hh
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,12 @@
#define SDF_MESH_HH_

#include <string>
#include <optional>

#include <gz/math/Vector3.hh>
#include <gz/math/Inertial.hh>
#include <gz/utils/ImplPtr.hh>
#include <sdf/CustomInertiaCalcProperties.hh>
#include <sdf/Element.hh>
#include <sdf/Error.hh>
#include <sdf/sdf_config.h>
Expand Down Expand Up @@ -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
/// <auto_inertia_params> element
/// \param[in] _config Parser Configuration
/// \return A std::optional with gz::math::Inertiald object or std::nullopt
public: std::optional<gz::math::Inertiald>
azeey marked this conversation as resolved.
Show resolved Hide resolved
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.
Expand Down
11 changes: 11 additions & 0 deletions include/sdf/ParserConfig.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down Expand Up @@ -194,6 +195,16 @@ class SDFORMAT_VISIBLE ParserConfig
/// \return Vector of registered model parser callbacks.
public: const std::vector<CustomModelParser> &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.
Expand Down
4 changes: 4 additions & 0 deletions sdf/1.11/collision.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,10 @@
</description>
</element>

<element name="auto_inertia_params" required="0">
<description>Parent tag to hold user-defined custom params for mesh inertia calculator</description>
</element>

<include filename="pose.sdf" required="0"/>

<include filename="geometry.sdf" required="1"/>
Expand Down
8 changes: 8 additions & 0 deletions sdf/1.11/inertial.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,14 @@
</description>
</element>

<element name="auto_inertia_params" required="0">
<description>
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.
</description>
</element>

<element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
<description>
This pose (translation, rotation) describes the position and orientation
Expand Down
23 changes: 22 additions & 1 deletion src/Collision.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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 <moi_calculator_params> tag
public: sdf::ElementPtr autoInertiaParams{nullptr};

/// \brief The SDF element pointer used during load.
public: sdf::ElementPtr sdf;

Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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)
{
Expand Down
63 changes: 63 additions & 0 deletions src/Collision_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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
// <auto_inertial_params> element is used to provide properties
// for the custom calculator like voxel size, grid type, etc.
std::string sdf = "<?xml version=\"1.0\"?>"
" <sdf version=\"1.11\">"
" <model name='shapes'>"
" <link name='link'>"
" <inertial auto='true' />"
" <collision name='box_col'>"
" <auto_inertia_params>"
" <gz:voxel_size>0.01</gz:voxel_size>"
" <gz:voxel_grid_type>float</gz:voxel_grid_type>"
" </auto_inertia_params>"
" <density>1240.0</density>"
" <geometry>"
" <box>"
" <size>2 2 2</size>"
" </box>"
" </geometry>"
" </collision>"
" </link>"
" </model>"
" </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);
const sdf::Collision *collision = link->CollisionByIndex(0);

root.ResolveAutoInertials(errors, sdfParserConfig);
EXPECT_TRUE(errors.empty());

sdf::ElementPtr autoInertiaParamsElem = collision->AutoInertiaParams();

// <auto_inertial_params> 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&
azeey marked this conversation as resolved.
Show resolved Hide resolved
double voxelSize = autoInertiaParamsElem->Get<double>("gz:voxel_size");
std::string voxelGridType =
autoInertiaParamsElem->Get<std::string>("gz:voxel_grid_type");
EXPECT_EQ("float", voxelGridType);
EXPECT_DOUBLE_EQ(0.01, voxelSize);
}

/////////////////////////////////////////////////
TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink)
{
Expand Down
Loading
Loading