Skip to content

Commit

Permalink
-Backport auto inertia based on mass
Browse files Browse the repository at this point in the history 
Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
@iche033
iche033 committed Jan 14, 2025
1 parent 8978648 commit a82f8b2
Show file tree
Hide file tree
Showing 9 changed files with 851 additions and 23 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 @@ -479,12 +479,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 @@ -515,11 +517,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
74 changes: 71 additions & 3 deletions src/Link.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,9 @@ class sdf::Link::Implementation
/// \brief True if this link should be subject to gravity, false otherwise.
public: bool enableGravity = true;

/// \brief True if this link is kinematic only
public: bool kinematic = false;

/// \brief True if automatic caluclation for the link inertial is enabled
public: bool autoInertia = false;

Expand Down Expand Up @@ -208,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 @@ -319,6 +321,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
this->dataPtr->enableGravity = _sdf->Get<bool>("gravity",
this->dataPtr->enableGravity).first;

this->dataPtr->kinematic = _sdf->Get<bool>("kinematic",
this->dataPtr->kinematic).first;

return errors;
}

Expand Down Expand Up @@ -664,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 @@ -684,6 +714,29 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
{
this->dataPtr->autoInertiaSaved = true;
}
else if (_config.CalculateInertialConfiguration() ==
ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT)
{
this->dataPtr->autoInertiaSaved = true;
// Write calculated inertia values to //link/inertial element
inertialElem->GetElement("pose")->GetValue()->Set<gz::math::Pose3d>(
totalInertia.Pose());
inertialElem->GetElement("mass")->GetValue()->Set<double>(
totalInertia.MassMatrix().Mass());
auto momentOfInertiaElem = inertialElem->GetElement("inertia");
momentOfInertiaElem->GetElement("ixx")->GetValue()->Set<double>(
totalInertia.MassMatrix().Ixx());
momentOfInertiaElem->GetElement("ixy")->GetValue()->Set<double>(
totalInertia.MassMatrix().Ixy());
momentOfInertiaElem->GetElement("ixz")->GetValue()->Set<double>(
totalInertia.MassMatrix().Ixz());
momentOfInertiaElem->GetElement("iyy")->GetValue()->Set<double>(
totalInertia.MassMatrix().Iyy());
momentOfInertiaElem->GetElement("iyz")->GetValue()->Set<double>(
totalInertia.MassMatrix().Iyz());
momentOfInertiaElem->GetElement("izz")->GetValue()->Set<double>(
totalInertia.MassMatrix().Izz());
}
}
// If auto is false, this means inertial values were set
// from user given values in Link::Load(), therefore we can return
Expand Down Expand Up @@ -859,6 +912,18 @@ void Link::SetEnableGravity(bool _enableGravity)
this->dataPtr->enableGravity = _enableGravity;
}

/////////////////////////////////////////////////
bool Link::Kinematic() const
{
return this->dataPtr->kinematic;
}

/////////////////////////////////////////////////
void Link::SetKinematic(bool _kinematic)
{
this->dataPtr->kinematic = _kinematic;
}

/////////////////////////////////////////////////
bool Link::AutoInertia() const
{
Expand Down Expand Up @@ -1040,6 +1105,9 @@ sdf::ElementPtr Link::ToElement() const
// wind mode
elem->GetElement("enable_wind")->Set(this->EnableWind());

// kinematic
elem->GetElement("kinematic")->Set(this->Kinematic());

// Collisions
for (const sdf::Collision &collision : this->dataPtr->collisions)
{
Expand Down
Loading

0 comments on commit a82f8b2

Please sign in to comment.