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

Accumulate inertial in mjs_bodyToFrame.

Browse Source 
PiperOrigin-RevId: 765129145
Change-Id: Ib7caca42ef4272ebe762655eee9380dae471be7f
This commit is contained in:
Alessio Quaglino
2025-05-30 04:09:37 -07:00
committed by Copybara-Service
parent f72a175a2c
commit 7932b4b202
4 changed files with 128 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+1 -71
View File
@@ -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
src/user/user_objects.cc
+93
View File
@@ -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()) {
src/user/user_objects.h
+4
View File
@@ -462,6 +462,10 @@ class mjCBody : public mjCBody_, private mjsBody {
// getters
std::vector<mjCBody*> 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
test/user/user_api_test.cc
+30
View File
@@ -1622,6 +1622,36 @@ TEST_F(MujocoTest, BodyToFrame) {
mj_deleteModel(expected);
}
TEST_F(MujocoTest, BodyToFrameWithInertial) {
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<body name="child">
<inertial mass="1" pos="0 0 0" quat="1 0 0 0" diaginertia="1 2 3"/>
</body>
</body>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<char, 1000> er;
mjtNum tol = 0;