diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index f1510b1b..f0582cbb 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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] = { diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 23f3dbb0..73789e24 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -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"( + + + + + + + + + + + + + )"; + + 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"(