chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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