chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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);
test/engine/engine_forward_test.cc
+61
View File
@@ -1756,5 +1756,66 @@ TEST_F(ForwardTest, FlexDampingRigidMotion) {
mj_deleteModel(model);
}
// verify that implicit integrator respects parent-flex coupling
TEST_F(ForwardTest, FlexParentCoupling) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit" timestep="0.01"/>
<worldbody>
<body name="parent" pos="0 0 0">
<freejoint/>
<geom size=".1" mass="0.1"/>
<flexcomp name="flex" type="grid" count="3 3 3" spacing="1 1 1"
radius=".01" dim="3" mass="100" dof="trilinear" pos="1 1 1">
<contact selfcollide="none"/>
<elasticity young="1e4" poisson="0.3" damping="50"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// set state: parent moving, flex deformed
// this ensures both H_fp (coupling) and qacc_parent are non-trivial
// Run with Euler (timestep 1e-6)
model->opt.timestep = 1e-6;
model->opt.integrator = mjINT_EULER;
mj_resetData(model, data);
data->qvel[0] = 1.0;
data->qpos[7] += 0.01;
data->qfrc_applied[0] = 10000.0; // Apply large force to parent
mj_step(model, data); // Step integrates
std::vector<mjtNum> qvel_euler(model->nv);
mju_copy(qvel_euler.data(), data->qvel, model->nv);
// Run with Implicit (timestep 1e-6)
model->opt.integrator = mjINT_IMPLICIT;
mj_resetData(model, data);
data->qvel[0] = 1.0;
data->qpos[7] += 0.01;
data->qfrc_applied[0] = 10000.0;
mj_step(model, data); // Step integrates
std::vector<mjtNum> qvel_implicit(model->nv);
mju_copy(qvel_implicit.data(), data->qvel, model->nv);
// Check agreement
double max_diff = 0;
for (int i = 0; i < model->nv; ++i) {
double diff = mju_abs(qvel_euler[i] - qvel_implicit[i]);
if (diff > max_diff) max_diff = diff;
}
EXPECT_LT(max_diff, 2e-5)
<< "Implicit integrator should match Euler at small timestep";
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco