Skip to content

Commit

Permalink
rescale inertia to match specified mass
Browse files Browse the repository at this point in the history 
Signed-off-by: Ian Chen <[email protected]>

update python test

Signed-off-by: Ian Chen <[email protected]>

udate api doc for resolving auto inertia

Signed-off-by: Ian Chen <[email protected]>

Add tests for auto-inertial with explicit mass (#1514)

* link_dom test: Fix typo

Signed-off-by: Steve Peters <[email protected]>

* Test case for auto-inertials with explicit mass

Signed-off-by: Steve Peters <[email protected]>

---------

Signed-off-by: Steve Peters <[email protected]>

update doc add comments

Signed-off-by: Ian Chen <[email protected]>

add one more test with multiple collisions

Signed-off-by: Ian Chen <[email protected]>

Update mass expectation in auto-inertial tests

The mass is no longer explicitly set in these
Link tests, so replace the not-equals expectation
with an expectation of what the mass should be.

Signed-off-by: Steve Peters <[email protected]>

fix build

Signed-off-by: Ian Chen <[email protected]>

remove empty line

Signed-off-by: Ian Chen <[email protected]>

remove density warning, fix typo

Signed-off-by: Ian Chen <[email protected]>

Update multiple-collision test with justification (#1515)

Modifies the auto-inertial test with multiple collisions
with different densities so that the lumped center of
gravity is at the link origin and derives the expected
moment of inertia values.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@iche033
iche033 committed Dec 13, 2024
1 parent ec757a6 commit 2d466af
Show file tree
Hide file tree
Showing 8 changed files with 485 additions and 23 deletions.
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
107 changes: 105 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,112 @@ 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>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'>" + \
" <pose>0.0 0.0 0.5 0 0 0</pose>" + \
" <density>2.0</density>" + \
" <geometry>" + \
" <box>" + \
" <size>1 1 1</size>" + \
" </box>" + \
" </geometry>" + \
" </collision>" + \
" <collision name='box_collision2'>" + \
" <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>" + \
" </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(0, 0, -0.166667, 0, 0, 0),
link.inertial().pose())
self.assertEqual(Vector3d(1.55556, 1.55556, 0.666667),
link.inertial().mass_matrix().diagonal_moments())

def test_resolveauto_inertialsCalledWithAutoFalse(self):
sdf = "<?xml version=\"1.0\"?>" + \
" <sdf version=\"1.11\">" + \
Expand Down
12 changes: 1 addition & 11 deletions src/Collision.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -291,18 +291,8 @@ 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
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 this Collision's auto inertia params have not been set, then use the
Expand Down
24 changes: 21 additions & 3 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 @@ -681,7 +680,27 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
totalInertia = totalInertia + collisionInertia;
}

this->dataPtr->inertial = totalInertia;
// If mass is specified, scale inertia to match desired mass
auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
if (inertialElem->HasElement("mass"))
{
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 +714,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
164 changes: 162 additions & 2 deletions src/Link_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -727,12 +727,15 @@ TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions)
/////////////////////////////////////////////////
TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue)
{
// A model with link inertial auto set to true.
// 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.
std::string 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 @@ -763,12 +766,169 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue)
root.ResolveAutoInertials(errors, sdfParserConfig);
EXPECT_TRUE(errors.empty());

EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass());
EXPECT_EQ(2.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, ResolveAutoInertialsWithMass)
{
// A model with link inertial auto set to true.
// The inertia matrix is specified but should be ignored.
// <mass> is specified - the auto computed inertial values should
// be scaled based on the desired mass.
std::string 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>";

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_DOUBLE_EQ(4.0, link->Inertial().MassMatrix().Mass());
EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose());
EXPECT_EQ(gz::math::Vector3d(0.66666, 0.66666, 0.66666),
link->Inertial().MassMatrix().DiagonalMoments());
}

/////////////////////////////////////////////////
TEST(DOMLink, ResolveAutoInertialsWithMassAndMultipleCollisions)
{
// A model with link inertial auto set to true.
// The inertia matrix is specified but should be ignored.
// <mass> is specified - the auto computed inertial values should
// be scaled based on the desired mass.
// The model contains two collisions with different sizes and densities
// and a lumped center of mass at the link origin.
std::string 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>";

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 mass value from //inertial/mass
EXPECT_DOUBLE_EQ(12.0, link->Inertial().MassMatrix().Mass());

// Inertial values based on density before scaling to match specified mass
//
// Collision geometries:
//
// --------- z = 1
// | |
// | c | cube on top, side length 1.0, density 4.0
// | |
// |-------| z = 0
// | |
// | |
// | |
// | c | box on bottom, size 1x1x2, density 1.0
// | |
// | |
// | |
// --------- z = -2
//
// Top cube: volume = 1.0, mass = 4.0, center of mass z = 0.5
// Bottom box: volume = 2.0, mass = 2.0, center of mass z = -1.0
//
// Total mass from density: 6.0
// Lumped center of mass z = (4.0 * 0.5 + 2.0 * (-1.0)) / 6.0 = 0.0
EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose());

// Moment of inertia at center of each shape
// Top cube: Ixx = Iyy = Izz = 4.0 / 12 * (1*1 + 1*1) = 8/12 = 2/3
// Bottom box: Ixx = Iyy = 2.0 / 12 * (1*1 + 2*2) = 10/12 = 5/6
// Izz = 2.0 / 12 * (1*1 + 1*1) = 4/12 = 1/3
//
// Lumped moment of inertia at lumped center of mass
// Ixx = sum(Ixx_i + mass_i * center_of_mass_z_i^2) for i in [top, bottom]
// Iyy = Ixx
// Izz = Izz_top + Izz_bottom
//
// Ixx = Iyy = (2/3 + 4.0 * 0.5 * 0.5) + (5/6 + 2.0 * (-1.0) * (-1.0))
// = (2/3 + 1) + (5/6 + 2.0)
// = 5/3 + 17/6
// = 27/6 = 9/2 = 4.5
//
// Izz = 2/3 + 1/3 = 1.0

// Then scale the inertias according to the specified mass of 12.0
// mass_ratio = 12.0 / 6.0 = 2.0
// Ixx = Iyy = mass_ratio * Ixx_unscaled = 2.0 * 4.5 = 9.0
// Izz = mass_ratio * Izz_unscaled = 2.0 * 1.0 = 2.0
EXPECT_EQ(gz::math::Vector3d(9.0, 9.0, 2.0),
link->Inertial().MassMatrix().DiagonalMoments());
}

/////////////////////////////////////////////////
TEST(DOMLink, ResolveAutoInertialsCalledWithAutoFalse)
{
Expand Down
Loading

0 comments on commit 2d466af

Please sign in to comment.