diff --git a/src/user/user_model.cc b/src/user/user_model.cc index e3d4544a..12a69f17 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3852,78 +3852,8 @@ void mjCModel::FuseStatic(void) { } //------------- add mass and inertia (if parent not world) - if (body->parent && body->parent->name != "world" && body->mass >= mjMINVAL) { - // body_ipose = body_pose * body_ipose - changeframe(body->ipos, body->iquat, body->pos, body->quat); - - // organize data - double mass[2] = { - par->mass, - body->mass - }; - double inertia[2][3] = { - {par->inertia[0], par->inertia[1], par->inertia[2]}, - {body->inertia[0], body->inertia[1], body->inertia[2]} - }; - double ipos[2][3] = { - {par->ipos[0], par->ipos[1], par->ipos[2]}, - {body->ipos[0], body->ipos[1], body->ipos[2]} - }; - double iquat[2][4] = { - {par->iquat[0], par->iquat[1], par->iquat[2], par->iquat[3]}, - {body->iquat[0], body->iquat[1], body->iquat[2], body->iquat[3]} - }; - - // compute total mass - par->mass = 0; - mjuu_setvec(par->ipos, 0, 0, 0); - for (int j=0; j < 2; j++) { - par->mass += mass[j]; - par->ipos[0] += mass[j]*ipos[j][0]; - par->ipos[1] += mass[j]*ipos[j][1]; - par->ipos[2] += mass[j]*ipos[j][2]; - } - - // small mass: allow for now, check for errors later - if (par->mass < mjMINVAL) { - par->mass = 0; - mjuu_setvec(par->inertia, 0, 0, 0); - mjuu_setvec(par->ipos, 0, 0, 0); - mjuu_setvec(par->iquat, 1, 0, 0, 0); - } - - // proceed with regular computation - else { - // locipos = center-of-mass - par->ipos[0] /= par->mass; - par->ipos[1] /= par->mass; - par->ipos[2] /= par->mass; - - // add inertias - double toti[6] = {0, 0, 0, 0, 0, 0}; - for (int j=0; j < 2; j++) { - double inertA[6], inertB[6]; - double dpos[3] = { - ipos[j][0] - par->ipos[0], - ipos[j][1] - par->ipos[1], - ipos[j][2] - par->ipos[2] - }; - - mjuu_globalinertia(inertA, inertia[j], iquat[j]); - mjuu_offcenter(inertB, mass[j], dpos); - for (int k=0; k < 6; k++) { - toti[k] += inertA[k] + inertB[k]; - } - } - - // compute principal axes of inertia - mjuu_copyvec(par->fullinertia, toti, 6); - const char* err1 = mjuu_fullInertia(par->iquat, par->inertia, par->fullinertia); - if (err1) { - throw mjCError(nullptr, "error '%s' in fusing static body inertias", err1); - } - } + par->AccumulateInertia(body); } //------------- replace body with its children in parent body list diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 80d57a8d..6a1b2451 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1422,6 +1422,15 @@ mjCFrame* mjCBody::ToFrame() { mjCFrame* newframe = parent->AddFrame(frame); mjuu_copyvec(newframe->spec.pos, spec.pos, 3); mjuu_copyvec(newframe->spec.quat, spec.quat, 4); + if (parent->name != "world" && mass >= mjMINVAL) { + if (!parent->explicitinertial) { + parent->MakeInertialExplicit(); + mjuu_zerovec(parent->spec.ipos, 3); + mjuu_zerovec(parent->spec.iquat, 4); + mjuu_zerovec(parent->spec.inertia, 3); + } + parent->AccumulateInertia(&this->spec, &parent->spec); + } MapFrame(parent->bodies, bodies, newframe, parent); MapFrame(parent->geoms, geoms, newframe, parent); MapFrame(parent->joints, joints, newframe, parent); @@ -1721,6 +1730,90 @@ void mjCBody::MakeInertialExplicit() { +// accumulate inertia of another body into this body +void mjCBody::AccumulateInertia(const mjsBody* other, mjsBody* result) { + if (!result) { + result = this; // use the private mjsBody + } + + // 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); + + // organize data + double mass[2] = { + result->mass, + other->mass + }; + double inertia[2][3] = { + {result->inertia[0], result->inertia[1], result->inertia[2]}, + {other->inertia[0], other->inertia[1], other->inertia[2]} + }; + double ipos[2][3] = { + {result->ipos[0], result->ipos[1], result->ipos[2]}, + {other_ipos[0], other_ipos[1], other_ipos[2]} + }; + double iquat[2][4] = { + {result->iquat[0], result->iquat[1], result->iquat[2], result->iquat[3]}, + {other->iquat[0], other->iquat[1], other->iquat[2], other->iquat[3]} + }; + + // compute total mass + result->mass = 0; + mjuu_setvec(result->ipos, 0, 0, 0); + for (int j=0; j < 2; j++) { + result->mass += mass[j]; + result->ipos[0] += mass[j]*ipos[j][0]; + result->ipos[1] += mass[j]*ipos[j][1]; + result->ipos[2] += mass[j]*ipos[j][2]; + } + + // small mass: allow for now, check for errors later + if (result->mass < mjMINVAL) { + result->mass = 0; + mjuu_setvec(result->inertia, 0, 0, 0); + mjuu_setvec(result->ipos, 0, 0, 0); + mjuu_setvec(result->iquat, 1, 0, 0, 0); + } + + // proceed with regular computation + else { + // locipos = center-of-mass + result->ipos[0] /= result->mass; + result->ipos[1] /= result->mass; + result->ipos[2] /= result->mass; + + // add inertias + double toti[6] = {0, 0, 0, 0, 0, 0}; + for (int j=0; j < 2; j++) { + double inertA[6], inertB[6]; + double dpos[3] = { + ipos[j][0] - result->ipos[0], + ipos[j][1] - result->ipos[1], + ipos[j][2] - result->ipos[2] + }; + + mjuu_globalinertia(inertA, inertia[j], iquat[j]); + mjuu_offcenter(inertB, mass[j], dpos); + for (int k=0; k < 6; k++) { + toti[k] += inertA[k] + inertB[k]; + } + } + + // compute principal axes of inertia + mjuu_copyvec(result->fullinertia, toti, 6); + const char* err1 = mjuu_fullInertia(result->iquat, result->inertia, result->fullinertia); + if (err1) { + throw mjCError(nullptr, "error '%s' in fusing static body inertias", err1); + } + } +} + + + // compute bounding volume hierarchy void mjCBody::ComputeBVH() { if (geoms.empty()) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 9c150646..18158c5c 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -462,6 +462,10 @@ class mjCBody : public mjCBody_, private mjsBody { // getters std::vector Bodies() const { return bodies; } + // accumulate inertia of another body into this body, if `result` is not nullptr, the accumulated + // inertia will be stored in `result`, otherwise the body's private spec will be used. + void AccumulateInertia(const mjsBody* other, mjsBody* result = nullptr); + private: mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor mjCBody& operator=(const mjCBody& other); // copy assignment diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 1669ea20..a415d740 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1622,6 +1622,36 @@ TEST_F(MujocoTest, BodyToFrame) { mj_deleteModel(expected); } +TEST_F(MujocoTest, BodyToFrameWithInertial) { + static constexpr char xml_child[] = R"( + + + + + + + + + )"; + + std::array er; + mjSpec* spec = mj_parseXMLString(xml_child, 0, er.data(), er.size()); + EXPECT_THAT(spec, NotNull()) << er.data(); + mjModel* model = mj_compile(spec, 0); + EXPECT_THAT(model, NotNull()); + mjsBody* parent = mjs_findBody(spec, "parent"); + EXPECT_THAT(parent, NotNull()); + mjsBody* child = mjs_findBody(spec, "child"); + EXPECT_THAT(child, NotNull()); + mjs_bodyToFrame(&child); + EXPECT_THAT(parent->mass, 1); + EXPECT_THAT(parent->fullinertia[0], 1); + EXPECT_THAT(parent->fullinertia[1], 2); + EXPECT_THAT(parent->fullinertia[2], 3); + mj_deleteSpec(spec); + mj_deleteModel(model); +} + TEST_F(MujocoTest, AttachSpecToSite) { std::array er; mjtNum tol = 0;