diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 34625963..bf632202 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -980,10 +980,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, int shell_mode = order < 0; order = order < 0 ? -order : order; - // warn that bending derivatives are not yet implemented - if (shell_mode) { - mj_warning(d, mjWARN_INERTIA, f); // bending implicit derivatives missing - } int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 61171337..5534b01a 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1417,10 +1417,8 @@ static void flexInterp_cgsolve(const mjModel* m, mjData* d, // build RHS: rhs = qfrc mju_copy(rhs, qfrc, nv); - // flex_interp velocity correction: rhs -= h*K_interp*qvel - mju_zero(temp, nv); - mjd_flexInterp_mul(m, d, temp, d->qvel, h, 0); // temp = h*K_interp*v - mju_addToScl(rhs, temp, -1.0, nv); // rhs -= h*K_interp*v + // flex_interp velocity correction: rhs += h*K_interp*qvel (K_interp is NSD) + mjd_flexInterp_mul(m, d, rhs, d->qvel, h, 0); // rhs += h*K_interp*v // standard flex bending velocity correction: rhs -= h*K_bend*qvel mjd_flexBend_mul(m, d, rhs, d->qvel, -h, 0); // rhs -= h*K_bend*v diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 9566abaf..74f2642c 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -3960,5 +3960,57 @@ TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) { mj_deleteModel(m); } +// interp stretch stiffness with implicitfast must preserve energy stability +TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) { + static constexpr char xml[] = R"( + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + + mjData* d = mj_makeData(m); + + // perturb a central vertex with velocity + int center_body = m->nbody / 2; + int dofadr = m->body_dofadr[center_body]; + ASSERT_GT(m->body_dofnum[center_body], 0); + d->qvel[dofadr + 2] = 1.0; // z-velocity + + mj_forward(m, d); + mjtNum initial_energy = d->energy[0] + d->energy[1]; + ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero"; + + // step and track max energy + mjtNum max_energy = initial_energy; + int nsteps = 50; + for (int i = 0; i < nsteps; i++) { + mj_step(m, d); + mjtNum total_energy = d->energy[0] + d->energy[1]; + max_energy = mju_max(max_energy, total_energy); + } + + // energy must not blow up + EXPECT_LE(max_energy, initial_energy * 1.01) + << "energy exceeded initial by more than 1%: max=" << max_energy + << ", initial=" << initial_energy; + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco