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 tests for auto-inertial with explicit mass #1514

Merged
merged 2 commits into from
Dec 9, 2024
Merged

Add tests for auto-inertial with explicit mass #1514

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
caf22f7
link_dom test: Fix typo
scpeters Dec 9, 2024
cd505f5
Test case for auto-inertials with explicit mass
scpeters Dec 9, 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
13 changes: 13 additions & 0 deletions src/gz_TEST.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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>
Loading