Skip to content

Commit

Permalink
Add tests for auto-inertial with explicit mass (#1514)
Browse files Browse the repository at this point in the history 
* link_dom test: Fix typo

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

* Test case for auto-inertials with explicit mass

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

---------

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@scpeters
scpeters authored Dec 9, 2024
1 parent d0fdc9c commit 686dc24
Show file tree
Hide file tree
Showing 3 changed files with 189 additions and 2 deletions.
13 changes: 13 additions & 0 deletions src/gz_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2021,6 +2021,19 @@ TEST(inertial_stats, SDF)
EXPECT_EQ(expectedOutput, output);
}

// Check a good SDF file with auto-inertials and explicit mass
// from the same folder by passing a relative path
{
std::string path = "inertial_stats_auto_mass.sdf";
const auto pathBase = sdf::testing::TestFile("sdf");

std::string output =
custom_exec_str("cd " + pathBase + " && " +
GzCommand() + " sdf --inertial-stats " +
path + SdfVersion());
EXPECT_EQ(expectedOutput, output);
}

expectedOutput =
"Error Code " +
std::to_string(static_cast<int>(
Expand Down
38 changes: 36 additions & 2 deletions test/integration/link_dom.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -948,7 +948,7 @@ TEST(DOMLink, InertialAuto)
const sdf::Link *link = model->LinkByName("link_1");
ASSERT_NE(link, nullptr);

// Verify inertial values for link_1 match thos in inertial_stats.sdf
// Verify inertial values for link_1 match those in inertial_stats.sdf
gz::math::Inertiald inertial = link->Inertial();
gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
Expand All @@ -969,6 +969,40 @@ TEST(DOMLink, InertialAuto)
EXPECT_FALSE(inertialElem->HasElement("pose"));
}

/////////////////////////////////////////////////
TEST(DOMLink, InertialAutoExplicitMass)
{
const std::string testFile = sdf::testing::TestFile("sdf",
"inertial_stats_auto_mass.sdf");

// Load the SDF file
sdf::Root root;
auto errors = root.Load(testFile);
EXPECT_TRUE(errors.empty()) << errors;

const sdf::Model *model = root.Model();
ASSERT_NE(model, nullptr);

std::vector<std::string> linkNames{"link_1", "link_2", "link_3", "link_4"};
for (const std::string &linkName : linkNames)
{
const sdf::Link *link = model->LinkByName(linkName);
ASSERT_NE(link, nullptr);

// Verify inertial values for link_i match those in inertial_stats.sdf
gz::math::Inertiald inertial = link->Inertial();
gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
EXPECT_DOUBLE_EQ(6.0, massMatrix.Mass());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Ixx());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Iyy());
EXPECT_DOUBLE_EQ(1.0, massMatrix.Izz());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixy());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixz());
EXPECT_DOUBLE_EQ(0.0, massMatrix.Iyz());
}
}

/////////////////////////////////////////////////
TEST(DOMLink, InertialAutoSaveInElement)
{
Expand All @@ -990,7 +1024,7 @@ TEST(DOMLink, InertialAutoSaveInElement)
const sdf::Link *link = model->LinkByName("link_1");
ASSERT_NE(link, nullptr);

// Verify inertial values for link_1 match thos in inertial_stats.sdf
// Verify inertial values for link_1 match those in inertial_stats.sdf
gz::math::Inertiald inertial = link->Inertial();
gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
Expand Down
140 changes: 140 additions & 0 deletions test/sdf/inertial_stats_auto_mass.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,140 @@
<?xml version="1.0" ?>
<sdf version="1.11">
<!---
Model consists of 4 cubes places symmetrically in the XY plane.
+y
│
┌─┼─┐
L3 │ │ │(0,5,0)
└─┼─┘
│
L2┌───┐ │ ┌───┐L1
────┼┼┼┼┼───┼─────┼┼┼┼┼─── +x
└───┘ │ └───┘
(-5,0,0) │ (5,0,0)
┌─┼─┐
│ │ │(0,-5,0)
└─┼─┘
L4│
-->
<![CDATA[
This model is used to verify the "gz sdf --inertial-stats" tool.
]]>
<model name="test_model">
<pose>0 0 0 0 0 0</pose>

<!-- set //inertial/density -->
<link name="link_1">
<pose>5 0 0 0 0 0</pose>
<inertial auto="true">
<density>6</density>
</inertial>
<collision name="collision_1">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
<visual name="visual_1">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>

<!-- set //inertial/mass and default collision density -->
<link name="link_2">
<pose>-5 0 0 0 0 0</pose>
<inertial auto="true">
<mass>6</mass>
</inertial>
<collision name="collision_2">
<density>1000</density>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
<visual name="visual_2">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>

<!-- use two half-cube collisions instead of single cube -->
<!-- set //inertial/mass and default collision density -->
<link name="link_3">
<pose>0 5 0 0 0 0</pose>
<inertial auto="true">
<mass>6</mass>
</inertial>
<collision name="collision_3_up">
<pose>0 0 0.25 0 0 0</pose>
<density>1000</density>
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
</collision>
<collision name="collision_3_down">
<pose>0 0 -0.25 0 0 0</pose>
<density>1000</density>
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
</collision>
<visual name="visual_3">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>

<!-- use two half-cube collisions instead of single cube -->
<!-- set //inertial/mass and identical non-default collision densities -->
<link name="link_4">
<pose>0 -5 0 0 0 0</pose>
<inertial auto="true">
<mass>6</mass>
</inertial>
<collision name="collision_4_up">
<pose>0 0 0.25 0 0 0</pose>
<density>123.456</density>
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
</collision>
<collision name="collision_4_down">
<pose>0 0 -0.25 0 0 0</pose>
<density>123.456</density>
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
</collision>
<visual name="visual_4">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>
</model>

</sdf>

0 comments on commit 686dc24

Please sign in to comment.