chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Merge pull request #3250 from gholmes829:fix/accumulate-inertia-com-frame-order

Browse Source 
PiperOrigin-RevId: 951758696
Change-Id: I99f3502b15b84e2fffc58f4b608844148479ca3e
This commit is contained in:
Copybara-Service
2026-07-21 16:21:08 -07:00
parent 2b30c6bc0f 0e1e0c7f96
commit 91bb075108
2 changed files with 34 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/user/user_objects_test.cc
+31
View File
@@ -2666,6 +2666,37 @@ TEST_F(UserObjectsTest, Inertial) {
EXPECT_THAT(error, HasSubstr("fullinertia and inertial orientation cannot"));
}
// Merged COM must be correct when a fused-static child has a non-identity
// inertial-frame orientation (e.g. from a degenerate diaginertia).
TEST_F(UserObjectsTest, FuseStaticWithRotatedInertial) {
static constexpr char xml[] = R"(
<mujoco>
<compiler fusestatic="true"/>
<worldbody>
<body name="parent">
<freejoint/>
<inertial pos="0 0 0" mass="1" diaginertia="1 1 1"/>
<body name="child" pos="1 0 0">
<inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5"
mass="1" diaginertia="0.01 0.01 0.02"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
// Child fuses into parent; only world + parent remain.
EXPECT_EQ(m->nbody, 2);
EXPECT_EQ(m->body_mass[1], 2);
// Mass-weighted COM in parent frame: (1*(0,0,0) + 1*(1,0,0)) / 2.
const mjtNum expected_com[3] = {0.5, 0, 0};
EXPECT_THAT(AsVector(m->body_ipos + 3, 3),
Pointwise(MjNear(1e-8, 1e-6), AsVector(expected_com, 3)));
}
TEST_F(UserObjectsTest, ZeroMass) {
static constexpr char xml[] = R"(
<mujoco>