Skip to content

Add Ellipsoidal Shell Inertia Tests (Multiple Dimensions) #2987

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

Open
inland-taipen wants to merge 1 commit into
base: main
Choose a base branch
from
+70 −1
Open

Add Ellipsoidal Shell Inertia Tests (Multiple Dimensions) #2987

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
71 changes: 70 additions & 1 deletion test/user/user_objects_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <cstring>
#include <memory>
Expand Down Expand Up @@ -906,7 +907,6 @@ TEST_F(MjCGeomTest, ShellInertiaCylinder) {

TEST_F(MjCGeomTest, ShellInertiaEllipsoid) {
// test special case of ellipsoid with dimensions: a = b = c
// TODO(taylorhowell): add test for ellipsoid with dimensions: a != b != c
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
Expand Down Expand Up @@ -967,6 +967,75 @@ TEST_F(MjCGeomTest, ShellInertiaEllipsoid) {
mj_deleteModel(m);
}

TEST_F(MjCGeomTest, ShellInertiaEllipsoidDifferentDimensions) {
// test ellipsoid with dimensions: a != b != c
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="ellipsoid" size="0.1 0.2 0.3" shellinertia="true"/>
</body>
<body>
<geom type="ellipsoid" size="0.1 0.2 0.3" density="1e8"/>
</body>
<body>
<geom type="ellipsoid" size="0.10000001 0.20000001 0.30000001" density="1e8"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();

// dimensions: a = 0.1, b = 0.2, c = 0.3
mjtNum a = 0.1;
mjtNum b = 0.2;
mjtNum c = 0.3;
mjtNum a2 = a * a;
mjtNum b2 = b * b;
mjtNum c2 = c * c;

// body 1: shell inertia with default surface density (1000)
// Surface area using Thomsen approximation
double p = 1.6075;
double tmp = std::pow(a * b, p) + std::pow(b * c, p) + std::pow(c * a, p);
mjtNum surface_area = 4 * mjPI * std::pow(tmp / 3, 1 / p);
mjtNum mass1 = surface_area * 1000; // surface area * surface density
EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol);

// For shell inertia, the calculation uses the difference method (expanded - original)
// We verify that the inertias are different for different axes (a != b != c)
// The shell inertia should have Ix != Iy != Iz when a != b != c
mjtNum Ix = m->body_inertia[3];
mjtNum Iy = m->body_inertia[4];
mjtNum Iz = m->body_inertia[5];

// Verify that inertias are different (since a != b != c)
EXPECT_GT(std::abs(Ix - Iy), kInertiaTol);
EXPECT_GT(std::abs(Iy - Iz), kInertiaTol);
EXPECT_GT(std::abs(Ix - Iz), kInertiaTol);

// Verify volume inertia for body 2 (solid ellipsoid)
// For volume: Ix = mass * (b^2 + c^2) / 5, Iy = mass * (a^2 + c^2) / 5, Iz = mass * (a^2 + b^2) / 5
mjtNum* inertia2 = m->body_inertia + 6;
mjtNum mass2 = m->body_mass[2];
mjtNum expected_Ix_vol = mass2 * (b2 + c2) / 5;
mjtNum expected_Iy_vol = mass2 * (a2 + c2) / 5;
mjtNum expected_Iz_vol = mass2 * (a2 + b2) / 5;

EXPECT_NEAR(inertia2[0], expected_Ix_vol, kInertiaTol);
EXPECT_NEAR(inertia2[1], expected_Iy_vol, kInertiaTol);
EXPECT_NEAR(inertia2[2], expected_Iz_vol, kInertiaTol);

// Verify that volume inertias are also different
EXPECT_GT(std::abs(inertia2[0] - inertia2[1]), kInertiaTol);
EXPECT_GT(std::abs(inertia2[1] - inertia2[2]), kInertiaTol);
EXPECT_GT(std::abs(inertia2[0] - inertia2[2]), kInertiaTol);

mj_deleteModel(m);
}

TEST_F(MjCGeomTest, ShellInertiaBox) {
static constexpr char xml[] = R"(
<mujoco>
Expand Down