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.

Sign up for GitHub

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

Add test for auto-inertia with mass and default density #1517

Merged
merged 5 commits into from
Dec 16, 2024
Merged

Add test for auto-inertia with mass and default density #1517

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0998fd4
add test for auto-inertia with mass and default density
iche033 Dec 14, 2024
642ba54
Merge branch 'auto_inertia_mass' into auto_inertia_mass_no_density
iche033 Dec 14, 2024
53f9b6a
lint
iche033 Dec 14, 2024
cc27502
fix test
iche033 Dec 14, 2024
d1b7b34
fix comment
iche033 Dec 16, 2024
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
75 changes: 66 additions & 9 deletions python/test/pyLink_TEST.py
View file
Open in desktop
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
Open in desktop
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 40000.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
Loading