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

Fix flexcomp instability when parent body has non-identity quaternion

Browse Source 
Fast path in mj_flexPassiveInterp, mj_flexPassiveBendInterp, and
mj_flexPassiveStretch assumed body slide joints are world-aligned
(J = I). When parent body has a non-identity quaternion, joint axes
are rotated (J = R_body), causing wrong force mapping and instability
(NaN/Inf in QACC).

Fix: project world-frame forces onto body local frame via
mju_mulMatTVec3(R_body^T, force) before adding to qfrc_spring/damper.
Also fix the derivative paths:
- mjd_flexInterp_kernel fast path: R^T * K_rot * R * vec
- mjd_flexStiff_assemble (CSR): R_bi^T * K_rot_block * R_bj

The CSR-assembled stiffness matrix is the actual path used by the
implicit CG solver (mj_flexCG gate). The test uses solver="CG" to
activate this path; without it, flex stiffness is integrated
explicitly and no derivative fix can help.

Ported from GitHub PR https://github.com/google-deepmind/mujoco/pull/3379
Original author: Devansh (https://github.com/devansh0703)
Fixes https://github.com/google-deepmind/mujoco/issues/3364

PiperOrigin-RevId: 952789284
Change-Id: If924f7160dd16c0cc88170d80e6e605da0fe2e04
This commit is contained in:
Alessio Quaglino
2026-07-23 09:01:51 -07:00
committed by Copybara-Service
parent b04a18a00a
commit fe9dc58477
3 changed files with 146 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_passive.c
+23 -5
View File
@@ -194,8 +194,16 @@ static void mj_flexPassiveInterp(const mjModel* m, mjData* d, int f,
(m->flex_node[3*nidx+0] == 0 &&
m->flex_node[3*nidx+1] == 0 &&
m->flex_node[3*nidx+2] == 0))) {
if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i);
if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i);
if (enbl_spring) {
mjtNum qfrc_loc[3];
mju_mulMatTVec3(qfrc_loc, d->xmat+9*bid, frc_g+3*i);
mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], qfrc_loc);
}
if (enbl_damper) {
mjtNum qdmp_loc[3];
mju_mulMatTVec3(qdmp_loc, d->xmat+9*bid, dmp_g+3*i);
mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], qdmp_loc);
}
} else {
if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring);
if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper);
@@ -428,8 +436,16 @@ static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f,
(m->flex_node[3*nidx+0] == 0 &&
m->flex_node[3*nidx+1] == 0 &&
m->flex_node[3*nidx+2] == 0))) {
if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i);
if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i);
if (enbl_spring) {
mjtNum qfrc_loc[3];
mju_mulMatTVec3(qfrc_loc, d->xmat+9*bid, frc_g+3*i);
mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], qfrc_loc);
}
if (enbl_damper) {
mjtNum qdmp_loc[3];
mju_mulMatTVec3(qdmp_loc, d->xmat+9*bid, dmp_g+3*i);
mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], qdmp_loc);
}
} else {
if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring);
if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper);
@@ -618,8 +634,10 @@ static void mj_flexPassiveStretch(const mjModel* m, mjData* d, int f,
} else {
int body_dofnum = m->body_dofnum[bid];
int body_dofadr = m->body_dofadr[bid];
mjtNum qfrc_loc[3];
mju_mulMatTVec3(qfrc_loc, d->xmat+9*bid, qfrc+3*v);
for (int x = 0; x < body_dofnum; x++) {
d->qfrc_spring[body_dofadr+x] += qfrc[3*v+x];
d->qfrc_spring[body_dofadr+x] += qfrc_loc[x];
}
}
}