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

Fix COM frame composition in mjCBody::AccumulateInertia

Browse Source
This commit is contained in:
gholmes829
2026-04-29 18:03:17 -05:00
parent 8ccd2b6db5
commit 0e1e0c7f96
2 changed files with 35 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+3 -3
View File
@@ -2543,9 +2543,9 @@ void mjCBody::AccumulateInertia(const mjsBody* other, mjsBody* result) {
// body_ipose = body_pose * body_ipose
double other_ipos[3];
double other_iquat[4];
mjuu_copyvec(other_ipos, other->ipos, 3);
mjuu_copyvec(other_iquat, other->iquat, 4);
mjuu_frameaccum(other_ipos, other_iquat, other->pos, other->quat);
mjuu_copyvec(other_ipos, other->pos, 3);
mjuu_copyvec(other_iquat, other->quat, 4);
mjuu_frameaccum(other_ipos, other_iquat, other->ipos, other->iquat);
// organize data
double mass[2] = {
test/user/user_objects_test.cc
+32
View File
@@ -2666,6 +2666,38 @@ 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];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, 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)));
mj_deleteModel(m);
}
TEST_F(UserObjectsTest, ZeroMass) {
static constexpr char xml[] = R"(
<mujoco>