Skip to content

Commit

Permalink
Add test for auto-inertia with mass and default density (#1517)
Browse files Browse the repository at this point in the history 
Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
@iche033
iche033 authored Dec 16, 2024
1 parent 2509d55 commit c1dc394
Show file tree
Hide file tree
Showing 2 changed files with 175 additions and 9 deletions.
75 changes: 66 additions & 9 deletions python/test/pyLink_TEST.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -580,29 +580,86 @@ def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
" <model name='compound_model'>" + \
" <link name='compound_link'>" + \
" <inertial auto='True'>" + \
" <mass>4.0</mass>" + \
" <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>2.0</density>" + \
" <density>4000.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>" + \
" <collision name='box_collision'>" + \
" <pose>0.0 0.0 -1.0 0 0 0</pose>" + \
" <geometry>" + \
" <box>" + \
" <size>1 1 1</size>" + \
" <size>1 1 2</size>" + \
" </box>" + \
" </geometry>" + \
" </collision>" + \
Expand All @@ -621,10 +678,10 @@ def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
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),
self.assertEqual(12000.0, link.inertial().mass_matrix().mass())
self.assertEqual(Pose3d.ZERO,
link.inertial().pose())
self.assertEqual(Vector3d(1.55556, 1.55556, 0.666667),
self.assertEqual(Vector3d(9000.0, 9000.0, 2000.0),
link.inertial().mass_matrix().diagonal_moments())

def test_resolveauto_inertialsCalledWithAutoFalse(self):
Expand Down
109 changes: 109 additions & 0 deletions src/Link_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -929,6 +929,115 @@ TEST(DOMLink, ResolveAutoInertialsWithMassAndMultipleCollisions)
link->Inertial().MassMatrix().DiagonalMoments());
}

/////////////////////////////////////////////////
TEST(DOMLink, ResolveAutoInertialsWithMassAndDefaultDensity)
{
// 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. Density
// is specified for the top collision but not the bottom collision.
// The model should have 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>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</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>";

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(12000.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 4000.0
// | |
// |-------| z = 0
// | |
// | |
// | |
// | c | box on bottom, size 1x1x2, default density 1000.0
// | |
// | |
// | |
// --------- z = -2
//
// Top cube: volume = 1.0, mass = 4000.0, center of mass z = 0.5
// Bottom box: volume = 2.0, mass = 2000.0, center of mass z = -1.0
//
// Total mass from density: 6000.0
// Lumped center of mass z = (4000.0 * 0.5 + 2000.0 * (-1.0)) / 6000.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 = 4000.0 / 12 * (1*1 + 1*1) = 8000/12
// Bottom box: Ixx = Iyy = 2000.0 / 12 * (1*1 + 2*2) = 10000/12
// Izz = 2000.0 / 12 * (1*1 + 1*1) = 4000/12
//
// 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 = (8000/12 + 4000.0 * 0.5 * 0.5) + (10000/12 + 2000.0
// * (-1.0) * (-1.0))
// = (8000/12 + 1000) + (10000/12 + 2000.0)
// = 20000/12 + 34000/12
// = 54000/12 = 9000/2 = 4500
//
// Izz = 8000/12 + 4000/12 = 1000.0

// Then scale the inertias according to the specified mass of 12000.0
// mass_ratio = 12000.0 / 6000.0 = 2.0
// Ixx = Iyy = mass_ratio * Ixx_unscaled = 2.0 * 4500.0 = 9000.0
// Izz = mass_ratio * Izz_unscaled = 2.0 * 1000.0 = 2000.0
EXPECT_EQ(gz::math::Vector3d(9000.0, 9000.0, 2000.0),
link->Inertial().MassMatrix().DiagonalMoments());
}

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

0 comments on commit c1dc394

Please sign in to comment.