diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b00810d5..34408b75 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -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; nflex_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; nflex_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); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 8d070ae2..ccccdc0d 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -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"( + + + )"; + + 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 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 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