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

Fix flex-parent coupling in implicit integrator.

Browse Source 
The implicit integrator was not correctly accounting for the off-diagonal coupling terms between flex and non-flex (parent) degrees of freedom in the mass matrix. This change extracts these coupling terms during the factorization step and applies a correction to the flex forces before solving for the flex accelerations, ensuring that the parent accelerations influence the flex dynamics. A new test verifies that the implicit integrator now matches Euler for small timesteps in a model with flex-parent coupling.

PiperOrigin-RevId: 868098126
Change-Id: Ia8cccd7dd428cd0c0898e1663a7e41017a2311b2
This commit is contained in:
Alessio Quaglino
2026-02-10 05:33:26 -08:00
committed by Copybara-Service
parent e93a69ea01
commit e16ee4fcb4
2 changed files with 103 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+42 -6
View File
@@ -1184,6 +1184,10 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mjtNum* H_flex = NULL;
int* flex_dof_indices = NULL;
int nflexdofs = 0;
int ncoupling = 0;
mjtNum* coupling_val = NULL;
int* coupling_row = NULL;
int* coupling_col = NULL;
// factorization
if (!skipfactor) {
@@ -1220,7 +1224,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
if (m->flex_interp[f]) {
int nodenum = m->flex_nodenum[f];
int nodeadr = m->flex_nodeadr[f];
for (int n=0; n<nodenum; n++) {
for (int n=0; n < nodenum; n++) {
int b = m->flex_nodebodyid[nodeadr + n];
nflexdofs += m->body_dofnum[b];
}
@@ -1238,7 +1242,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
if (m->flex_interp[f]) {
int nodenum = m->flex_nodenum[f];
int nodeadr = m->flex_nodeadr[f];
for (int n=0; n<nodenum; n++) {
for (int n=0; n < nodenum; n++) {
int b = m->flex_nodebodyid[nodeadr + n];
int dofnum = m->body_dofnum[b];
int dofadr = m->body_dofadr[b];
@@ -1251,15 +1255,35 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
}
}
// build H_flex (dense) from qLU (implicit) or qH (implicitfast)
H_flex = mjSTACKALLOC(d, nflexdofs*nflexdofs, mjtNum);
mju_zero(H_flex, nflexdofs*nflexdofs);
const int* rownnz = (m->opt.integrator == mjINT_IMPLICIT) ? m->D_rownnz : m->M_rownnz;
const int* rowadr = (m->opt.integrator == mjINT_IMPLICIT) ? m->D_rowadr : m->M_rowadr;
const int* colind = (m->opt.integrator == mjINT_IMPLICIT) ? m->D_colind : m->M_colind;
const mjtNum* source = (m->opt.integrator == mjINT_IMPLICIT) ? d->qLU : d->qH;
// count coupling terms (off-diagonal: flex row, non-flex col)
for (int i=0; i < nflexdofs; i++) {
int row = flex_dof_indices[i];
int start = rowadr[row];
int end = start + rownnz[row];
for (int k=start; k < end; k++) {
if (global2local[colind[k]] < 0) {
ncoupling++;
}
}
}
// allocate coupling storage
if (ncoupling > 0) {
coupling_val = mjSTACKALLOC(d, ncoupling, mjtNum);
coupling_row = mjSTACKALLOC(d, ncoupling, int);
coupling_col = mjSTACKALLOC(d, ncoupling, int);
}
// build H_flex (dense) from qLU (implicit) or qH (implicitfast)
H_flex = mjSTACKALLOC(d, nflexdofs*nflexdofs, mjtNum);
mju_zero(H_flex, nflexdofs*nflexdofs);
int coup_cnt = 0;
for (int i=0; i < nflexdofs; i++) {
int row = flex_dof_indices[i];
int start = rowadr[row];
@@ -1269,6 +1293,11 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
int local_j = global2local[col];
if (local_j >= 0) {
H_flex[i*nflexdofs + local_j] = source[k];
} else if (coup_cnt < ncoupling) {
coupling_val[coup_cnt] = source[k];
coupling_row[coup_cnt] = i; // local flex index
coupling_col[coup_cnt] = col; // global parent index
coup_cnt++;
}
}
}
@@ -1326,6 +1355,13 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
qfrc_flex[i] = qfrc[global_dof] + res[global_dof] * factor;
}
// apply coupling correction: qfrc_flex -= H_coupling * qacc_parent
if (ncoupling > 0) {
for (int k=0; k < ncoupling; k++) {
qfrc_flex[coupling_row[k]] -= coupling_val[k] * qacc[coupling_col[k]];
}
}
// solve H_flex * qacc_flex = qfrc_flex
// reuse qfrc_flex as result buffer (qacc_flex)
mju_cholSolve(qfrc_flex, H_flex, qfrc_flex, nflexdofs);