Skip to content

Commit

Permalink
Resolve auto inertia based on input mass (#1513)
Browse files Browse the repository at this point in the history 
Support auto-inertia computation using mass and density. Implemented based on the suggestions in #1482. Inertia is first auto resolved from all collisions as usual. If mass is specified, we normalize the inertia to get unit inertia, then scaling is applied to match the desired mass.

---------

Signed-off-by: Steve Peters <[email protected]>
Signed-off-by: Ian Chen <[email protected]>
Co-authored-by: Steve Peters <[email protected]>
  • Loading branch information
@iche033 @scpeters
iche033 and scpeters authored Dec 21, 2024
1 parent 30d4a5f commit aaadeea
Show file tree
Hide file tree
Showing 9 changed files with 713 additions and 26 deletions.
5 changes: 4 additions & 1 deletion include/sdf/Collision.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,12 +170,15 @@ namespace sdf
/// \param[in] _autoInertiaParams An ElementPtr to the auto_inertia_params
/// element to be used if the auto_inertia_params have not been set in this
/// collision.
/// \param[in] _warnIfDensityIsUnset True to generate a warning that
/// the default density value will be used if it is not explicitly set.
public: void CalculateInertial(
sdf::Errors &_errors,
gz::math::Inertiald &_inertial,
const ParserConfig &_config,
const std::optional<double> &_density,
sdf::ElementPtr _autoInertiaParams);
sdf::ElementPtr _autoInertiaParams,
bool _warnIfDensityIsUnset = true);

/// \brief Get a pointer to the SDF element that was used during
/// load.
Expand Down
10 changes: 7 additions & 3 deletions include/sdf/Link.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,9 +351,13 @@ namespace sdf
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.
/// & inertial pose) for the link. This function will calculate
/// the inertial values if the auto attribute is set to true.
/// If mass is not provided by the user, the inertial values will be
/// determined based on either link density or collision density if
/// explicitly set. Otherwise, if mass is specified, the inertia matrix
/// will be scaled to match the desired mass, while respecting
/// the ratio of density values between collisions.
/// \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
Expand Down
164 changes: 162 additions & 2 deletions python/test/pyLink_TEST.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -483,12 +483,14 @@ def test_resolveauto_inertialsWithMultipleCollisions(self):
link.inertial().mass_matrix().diagonal_moments())

def test_inertial_values_given_with_auto_set_to_true(self):
# The inertia matrix is specified but should be ignored.
# <mass> is not speicifed so the inertial values should be computed
# based on the collision density value.
sdf = "<?xml version=\"1.0\"?>" + \
"<sdf version=\"1.11\">" + \
" <model name='compound_model'>" + \
" <link name='compound_link'>" + \
" <inertial auto='True'>" + \
" <mass>4.0</mass>" + \
" <pose>1 1 1 2 2 2</pose>" + \
" <inertia>" + \
" <ixx>1</ixx>" + \
Expand Down Expand Up @@ -519,11 +521,169 @@ def test_inertial_values_given_with_auto_set_to_true(self):
root.resolve_auto_inertials(errors, sdfParserConfig)
self.assertEqual(len(errors), 0)

self.assertNotEqual(4.0, link.inertial().mass_matrix().mass())
self.assertEqual(2.0, link.inertial().mass_matrix().mass())
self.assertEqual(Pose3d.ZERO, link.inertial().pose())
self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333),
link.inertial().mass_matrix().diagonal_moments())

def test_resolveauto_inertialsWithMass(self):
# The inertia matrix is specified but should be ignored.
# <mass> is speicifed - the auto computed inertial values should
# be scaled based on the desired mass.
sdf = "<?xml version=\"1.0\"?>" + \
"<sdf version=\"1.11\">" + \
" <model name='compound_model'>" + \
" <link name='compound_link'>" + \
" <inertial auto='True'>" + \
" <mass>4.0</mass>" + \
" <pose>1 1 1 2 2 2</pose>" + \
" <inertia>" + \
" <ixx>1</ixx>" + \
" <iyy>1</iyy>" + \
" <izz>1</izz>" + \
" </inertia>" + \
" </inertial>" + \
" <collision name='box_collision'>" + \
" <density>2.0</density>" + \
" <geometry>" + \
" <box>" + \
" <size>1 1 1</size>" + \
" </box>" + \
" </geometry>" + \
" </collision>" + \
" </link>" + \
" </model>" + \
" </sdf>"

root = Root()
sdfParserConfig = ParserConfig()
errors = root.load_sdf_string(sdf, sdfParserConfig)
self.assertEqual(errors, None)

model = root.model()
link = model.link_by_index(0)
errors = []
root.resolve_auto_inertials(errors, sdfParserConfig)
self.assertEqual(len(errors), 0)

self.assertEqual(4.0, link.inertial().mass_matrix().mass())
self.assertEqual(Pose3d.ZERO, link.inertial().pose())
self.assertEqual(Vector3d(0.66666, 0.66666, 0.66666),
link.inertial().mass_matrix().diagonal_moments())

def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
# The inertia matrix is specified but should be ignored.
# <mass> is speicifed - the auto computed inertial values should
# be scaled based on the desired mass.
sdf = "<?xml version=\"1.0\"?>" + \
"<sdf version=\"1.11\">" + \
" <model name='compound_model'>" + \
" <link name='compound_link'>" + \
" <inertial auto='True'>" + \
" <mass>12.0</mass>" + \
" <pose>1 1 1 2 2 2</pose>" + \
" <inertia>" + \
" <ixx>1</ixx>" + \
" <iyy>1</iyy>" + \
" <izz>1</izz>" + \
" </inertia>" + \
" </inertial>" + \
" <collision name='cube_collision'>" + \
" <pose>0.0 0.0 0.5 0 0 0</pose>" + \
" <density>4.0</density>" + \
" <geometry>" + \
" <box>" + \
" <size>1 1 1</size>" + \
" </box>" + \
" </geometry>" + \
" </collision>" + \
" <collision name='box_collision'>" + \
" <pose>0.0 0.0 -1.0 0 0 0</pose>" + \
" <density>1.0</density>" + \
" <geometry>" + \
" <box>" + \
" <size>1 1 2</size>" + \
" </box>" + \
" </geometry>" + \
" </collision>" + \
" </link>" + \
" </model>" + \
"</sdf>"

root = Root()
sdfParserConfig = ParserConfig()
errors = root.load_sdf_string(sdf, sdfParserConfig)
self.assertEqual(errors, None)

model = root.model()
link = model.link_by_index(0)
errors = []
root.resolve_auto_inertials(errors, sdfParserConfig)
self.assertEqual(len(errors), 0)

self.assertEqual(12.0, link.inertial().mass_matrix().mass())
self.assertEqual(Pose3d.ZERO,
link.inertial().pose())
self.assertEqual(Vector3d(9.0, 9.0, 2.0),
link.inertial().mass_matrix().diagonal_moments())

def test_resolveauto_inertialsWithMassAndDefaultDensity(self):
# The inertia matrix is specified but should be ignored.
# <mass> is speicifed - the auto computed inertial values should
# be scaled based on the desired mass.
# Density is not specified for the bottom collision - it should
# use the default value
sdf = "<?xml version=\"1.0\"?>" + \
"<sdf version=\"1.11\">" + \
" <model name='compound_model'>" + \
" <link name='compound_link'>" + \
" <inertial auto='True'>" + \
" <mass>12000.0</mass>" + \
" <pose>1 1 1 2 2 2</pose>" + \
" <inertia>" + \
" <ixx>1</ixx>" + \
" <iyy>1</iyy>" + \
" <izz>1</izz>" + \
" </inertia>" + \
" </inertial>" + \
" <collision name='cube_collision'>" + \
" <pose>0.0 0.0 0.5 0 0 0</pose>" + \
" <density>4000.0</density>" + \
" <geometry>" + \
" <box>" + \
" <size>1 1 1</size>" + \
" </box>" + \
" </geometry>" + \
" </collision>" + \
" <collision name='box_collision'>" + \
" <pose>0.0 0.0 -1.0 0 0 0</pose>" + \
" <geometry>" + \
" <box>" + \
" <size>1 1 2</size>" + \
" </box>" + \
" </geometry>" + \
" </collision>" + \
" </link>" + \
" </model>" + \
"</sdf>"

root = Root()
sdfParserConfig = ParserConfig()
errors = root.load_sdf_string(sdf, sdfParserConfig)
self.assertEqual(errors, None)

model = root.model()
link = model.link_by_index(0)
errors = []
root.resolve_auto_inertials(errors, sdfParserConfig)
self.assertEqual(len(errors), 0)

self.assertEqual(12000.0, link.inertial().mass_matrix().mass())
self.assertEqual(Pose3d.ZERO,
link.inertial().pose())
self.assertEqual(Vector3d(9000.0, 9000.0, 2000.0),
link.inertial().mass_matrix().diagonal_moments())

def test_resolveauto_inertialsCalledWithAutoFalse(self):
sdf = "<?xml version=\"1.0\"?>" + \
" <sdf version=\"1.11\">" + \
Expand Down
28 changes: 16 additions & 12 deletions src/Collision.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,7 +272,8 @@ void Collision::CalculateInertial(
gz::math::Inertiald &_inertial,
const ParserConfig &_config,
const std::optional<double> &_density,
sdf::ElementPtr _autoInertiaParams)
sdf::ElementPtr _autoInertiaParams,
bool _warnIfDensityIsUnset)
{
// Order of precedence for density:
double density;
Expand All @@ -291,18 +292,21 @@ void Collision::CalculateInertial(
// 3. DensityDefault value.
else
{
// If density was not explicitly set, send a warning
// about the default value being used
// If density was not explicitly set, default value is used
// Send a warning about the default value being used if needed
density = DensityDefault();
Error densityMissingErr(
ErrorCode::ELEMENT_MISSING,
"Collision is missing a <density> child element. "
"Using a default density value of " +
std::to_string(DensityDefault()) + " kg/m^3. "
);
enforceConfigurablePolicyCondition(
_config.WarningsPolicy(), densityMissingErr, _errors
);
if (_warnIfDensityIsUnset)
{
Error densityMissingErr(
ErrorCode::ELEMENT_MISSING,
"Collision is missing a <density> child element. "
"Using a default density value of " +
std::to_string(DensityDefault()) + " kg/m^3. "
);
enforceConfigurablePolicyCondition(
_config.WarningsPolicy(), densityMissingErr, _errors
);
}
}

// If this Collision's auto inertia params have not been set, then use the
Expand Down
31 changes: 27 additions & 4 deletions src/Link.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,6 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
// If auto is to true but user has still provided
// inertial values
if (inertialElem->HasElement("pose") ||
inertialElem->HasElement("mass") ||
inertialElem->HasElement("inertia"))
{
Error err(
Expand Down Expand Up @@ -670,18 +669,43 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
return;
}

auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
bool massSpecified = inertialElem->HasElement("mass");
// Warn about using default collision density value if mass is not specified
bool warnUseDefaultDensity = !massSpecified;

gz::math::Inertiald totalInertia;

for (sdf::Collision &collision : this->dataPtr->collisions)
{
gz::math::Inertiald collisionInertia;
collision.CalculateInertial(_errors, collisionInertia, _config,
this->dataPtr->density,
this->dataPtr->autoInertiaParams);
this->dataPtr->autoInertiaParams,
warnUseDefaultDensity);
totalInertia = totalInertia + collisionInertia;
}

this->dataPtr->inertial = totalInertia;
// If mass is specified, scale inertia to match desired mass
if (massSpecified)
{
double mass = inertialElem->Get<double>("mass");
const gz::math::MassMatrix3d &totalMassMatrix = totalInertia.MassMatrix();
// normalize to get the unit mass matrix
gz::math::MassMatrix3d unitMassMatrix(1.0,
totalMassMatrix.DiagonalMoments() / totalMassMatrix.Mass(),
totalMassMatrix.OffDiagonalMoments() / totalMassMatrix.Mass());
// scale the final inertia to match specified mass
this->dataPtr->inertial = gz::math::Inertiald(
gz::math::MassMatrix3d(mass,
mass * unitMassMatrix.DiagonalMoments(),
mass * unitMassMatrix.OffDiagonalMoments()),
totalInertia.Pose());
}
else
{
this->dataPtr->inertial = totalInertia;
}

// If CalculateInertial() was called with SAVE_CALCULATION
// configuration then set autoInertiaSaved to true
Expand All @@ -695,7 +719,6 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
{
this->dataPtr->autoInertiaSaved = true;
// Write calculated inertia values to //link/inertial element
auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
inertialElem->GetElement("pose")->GetValue()->Set<gz::math::Pose3d>(
totalInertia.Pose());
inertialElem->GetElement("mass")->GetValue()->Set<double>(
Expand Down
Loading

0 comments on commit aaadeea

Please sign in to comment.