From 35cdc779e640ce9e7bda81c4c30c2cfad197d541 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 13 May 2026 03:57:41 -0700 Subject: [PATCH] Add implicit bending stiffness for standard flex. Standard flex (flex_interp=0) with thin-plate bending treated bending forces purely explicitly. This caused contact-induced vertex vibrations and non-physical energy injection for flat resting sheets, because the solver treated each vertex as an independent mass during contact and contact normals are orthogonal to stretch constraints. Fix: extend the existing preconditioned CG solver to include the constant bending stiffness K_bend in the implicit operator via matrix-free mat-vec. PiperOrigin-RevId: 914774020 Change-Id: I45e0d6749abb6f873566203bccae956514b2576b --- model/flex/poncho.xml | 4 +- model/flex/poncho_edgeequality.xml | 4 +- src/engine/engine_derivative.c | 66 ++++++++++++-- src/engine/engine_derivative.h | 15 ++- src/engine/engine_forward.c | 40 +++++--- test/engine/engine_derivative_test.cc | 4 +- test/engine/engine_forward_test.cc | 126 +++++++++++++++++++++++++- 7 files changed, 221 insertions(+), 38 deletions(-) diff --git a/model/flex/poncho.xml b/model/flex/poncho.xml index ffa8aa28..72acc750 100644 --- a/model/flex/poncho.xml +++ b/model/flex/poncho.xml @@ -15,7 +15,7 @@ - @@ -1414,7 +1414,7 @@ 398 399 418 398 376 378"> - + diff --git a/model/flex/poncho_edgeequality.xml b/model/flex/poncho_edgeequality.xml index d14378df..20ffecc2 100644 --- a/model/flex/poncho_edgeequality.xml +++ b/model/flex/poncho_edgeequality.xml @@ -15,7 +15,7 @@ - @@ -1414,7 +1414,7 @@ 398 399 418 398 376 378"> - + diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 85f91e33..34625963 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1127,25 +1127,71 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, -// compute res += (h^2 + h*damping) * J'*K*J * vec, for all interpolated flexes -void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) { - // s1=h*h, s2=h => scale = h*h + h*damping - mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h * h, h, NULL, 0, 0); +// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes +void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2) { + mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, s1, s2, NULL, 0, 0); } -// compute res += h * J'*K*J * vec, for all interpolated flexes (stiffness only, no damping) -void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) { - // s1=h, s2=0 => scale = h (no damping contribution) - mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h, 0, NULL, 0, 0); + +// compute res += scale * K_bend * vec for standard (non-interp) flex bending +// scale = s1 + s2 * flex_damping[f] per flex +// for stiffness+damping: s1=h^2, s2=h => scale = h^2 + h*damping +// for stiffness only: s1=h, s2=0 => scale = h +void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2) { + for (int f = 0; f < m->nflex; f++) { + // skip interp, rigid, or non-2D + if (m->flex_interp[f] || m->flex_rigid[f] || m->flex_dim[f] != 2) { + continue; + } + + int bendingadr = m->flex_bendingadr[f]; + if (bendingadr < 0) { + continue; + } + + mjtNum scale = s1 + s2 * m->flex_damping[f]; + if (!scale) { + continue; + } + + const mjtNum* b = m->flex_bending + bendingadr; + const int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f]; + int edgenum = m->flex_edgenum[f]; + int edgeadr = m->flex_edgeadr[f]; + + for (int e = 0; e < edgenum; e++) { + const int* edge = m->flex_edge + 2*(e + edgeadr); + const int* flap = m->flex_edgeflap + 2*(e + edgeadr); + int v[4] = {edge[0], edge[1], flap[0], flap[1]}; + + // skip boundary edges (no second flap vertex) + if (v[3] == -1) { + continue; + } + + // apply 4x4 bending stencil, coordinate-wise + for (int i = 0; i < 4; i++) { + int dof_i = m->body_dofadr[bodyid[v[i]]]; + for (int x = 0; x < 3; x++) { + mjtNum val = 0; + for (int j = 0; j < 4; j++) { + int dof_j = m->body_dofadr[bodyid[v[j]]]; + val += b[17*e + 4*i + j] * vec[dof_j + x]; + } + res[dof_i + x] += scale * val; + } + } + } + } } - - // add (d qfrc_actuator / d qvel) to qDeriv void mjd_actuator_vel(const mjModel* m, mjData* d) { int nu = m->nu; diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h index 1ddb3af9..09a5e659 100644 --- a/src/engine/engine_derivative.h +++ b/src/engine/engine_derivative.h @@ -43,15 +43,14 @@ MJAPI void mjd_passive_vel(const mjModel* m, mjData* d); // subtract (d qfrc_bias / d qvel) from qDeriv (dense version) MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d); -// derivative of flex_interp generalized force w.r.t position: res = (d qfrc_flexinterp / d qpos) * vec -// res and vec are vectors of size m->nv -MJAPI void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h); - -// derivative of flex_interp generalized force w.r.t position (stiffness only, no damping) -MJAPI void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h); - - +// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes +MJAPI void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2); +// compute res += scale * K_bend * vec for standard (non-interp) flex bending +// scale = s1 + s2 * flex_damping[f] per flex +MJAPI void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2); #ifdef __cplusplus diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 5e285f39..61171337 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1371,14 +1371,25 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } -// return 1 if any flex needs implicit interp treatment -static int flexInterp_has_active(const mjModel* m) { +// return 1 if any flex needs implicit stiffness treatment (interp or bending) +static int flex_has_implicit_stiffness(const mjModel* m) { for (int f=0; f < m->nflex; f++) { - if (m->flex_interp[f] && !m->flex_rigid[f] && + if (m->flex_rigid[f]) { + continue; + } + + // interpolated flex with stiffness + if (m->flex_interp[f] && m->flex_edgeequality[f] != 3 && m->flex_stiffness[m->flex_stiffnessadr[f]] != 0) { return 1; } + + // standard flex with bending + if (!m->flex_interp[f] && m->flex_dim[f] == 2 && + m->flex_bendingadr[f] >= 0) { + return 1; + } } return 0; } @@ -1403,24 +1414,27 @@ static void flexInterp_cgsolve(const mjModel* m, mjData* d, mjtNum* Ap = mjSTACKALLOC(d, nv, mjtNum); mjtNum* temp = mjSTACKALLOC(d, nv, mjtNum); - // build RHS: rhs = qfrc - h*K*qvel (velocity correction from flex stiffness) + // build RHS: rhs = qfrc mju_copy(rhs, qfrc, nv); + + // flex_interp velocity correction: rhs -= h*K_interp*qvel mju_zero(temp, nv); - mjd_flexInterp_mulK(m, d, temp, d->qvel, h); // temp = h*K*v (stiffness only) - mju_addToScl(rhs, temp, -1.0, nv); // rhs -= h*K*v + 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 + + // 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 // --- helper lambda-style inline: compute Ap = A*x --- - // A*x = (M - h*qDeriv)*x - (h^2+h*d)*K*x + // A*x = (M - h*qDeriv)*x - (h^2+h*d)*K_interp*x + (h^2+h*d)*K_bend*x #define FLEX_CG_MATVEC(Ap_out, x_in) \ mju_mulMatVecSparse(Ap_out, d->qDeriv, x_in, nv, m->D_rownnz, m->D_rowadr, \ m->D_colind, NULL); \ - mju_zero(temp, nv); \ mju_mulSymVecSparse(temp, d->M, x_in, nv, m->M_rownnz, m->M_rowadr, \ m->M_colind); \ mju_addScl(Ap_out, temp, Ap_out, -h, nv); \ - mju_zero(temp, nv); \ - mjd_flexInterp_mulKD(m, d, temp, x_in, h); \ - mju_addToScl(Ap_out, temp, -1.0, nv) + mjd_flexInterp_mul(m, d, Ap_out, x_in, -(h*h), -h); \ + mjd_flexBend_mul(m, d, Ap_out, x_in, h*h, h) // --- helper: preconditioner solve z = (M - h*qDeriv)^{-1} * r --- #define FLEX_CG_PRECOND(z_out, r_in) \ @@ -1857,7 +1871,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } // check for flex_interp that needs implicit treatment - int has_flex_interp = !sleep_filter && flexInterp_has_active(m); + int has_flex_stiffness = !sleep_filter && flex_has_implicit_stiffness(m); // factorization if (!skipfactor) { @@ -1911,7 +1925,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } // flex: CG correction for implicit flex stiffness - if (has_flex_interp) { + if (has_flex_stiffness) { flexInterp_cgsolve(m, d, qacc, qfrc, m->nv); } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index fb9d777f..0b3be449 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1474,7 +1474,7 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name, } // Helper: assemble flex stiffness into dense matrix via matrix-vector products. -// Builds K column-by-column using mjd_flexInterp_mulKD. +// Builds K column-by-column using mjd_flexInterp_mul. // Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH // convention where stiffness is subtracted from the system matrix). static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, @@ -1485,7 +1485,7 @@ static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, mju_zero(e_i.data(), nv); mju_zero(col.data(), nv); e_i[i] = 1.0; - mjd_flexInterp_mulKD(m, d, col.data(), e_i.data(), h); + mjd_flexInterp_mul(m, d, col.data(), e_i.data(), h * h, h); // col = +(h^2 + h*damp)*K*e_i, negate to match addH convention (H -= K) for (int j = 0; j < nv; j++) { H_dense[j * nv + i] = -col[j]; diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index dddf612b..9566abaf 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -3147,7 +3147,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { // using mulKD for legacy check consistency, but we know it applies h^2+h*d // scaling; actually, let's stick to the high-level property checks from // FlexStiffnessSign which used mulKD - mjd_flexInterp_mulKD(model, data, flex_Kv.data(), v.data(), h); + mjd_flexInterp_mul(model, data, flex_Kv.data(), v.data(), h * h, h); // compute v^T*M*v and v^T*scale*K*v mjtNum vMv = mju_dot(v.data(), Mv.data(), nv); @@ -3836,5 +3836,129 @@ TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) { mj_deleteModel(m); } +// flex sheet dropping on a plane should not gain energy from implicit bending +TEST_F(ImplicitIntegratorTest, FlexContactEnergy) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + ASSERT_EQ(m->nflex, 1); + + mjData* d = mj_makeData(m); + + // compute initial energy + mj_forward(m, d); + mjtNum initial_energy = d->energy[0] + d->energy[1]; + ASSERT_GT(initial_energy, 0); + + // simulate + mjtNum max_energy = initial_energy; + int max_energy_step = 0; + int nsteps = 500; + for (int i = 0; i < nsteps; i++) { + mj_step(m, d); + mjtNum total_energy = d->energy[0] + d->energy[1]; + if (total_energy > max_energy) { + max_energy = total_energy; + max_energy_step = i + 1; + } + } + + mjtNum energy_ratio = max_energy / initial_energy; + + EXPECT_LE(energy_ratio, 1.01) + << "contact solver injected energy: max_energy/initial_energy = " + << energy_ratio << " (max at step " << max_energy_step << ")" + << "\n initial_energy = " << initial_energy + << "\n max_energy = " << max_energy; + + mj_deleteData(d); + mj_deleteModel(m); +} + +// bending damping on a flat flex must dissipate energy with implicit integrator +TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + ASSERT_EQ(m->nflex, 1); + ASSERT_GT(m->flex_damping[0], 0) << "flex_damping not set"; + + mjData* d = mj_makeData(m); + + // perturb a central vertex with upward velocity + // vertex layout is 6x6 grid; pick a central vertex (row=3, col=3 -> id=21) + int center_vert = 21; + int bid = m->flex_vertbodyid[m->flex_vertadr[0] + center_vert]; + int dofadr = m->body_dofadr[bid]; + d->qvel[dofadr + 2] = 1.0; // z-velocity + + // initial forward to compute energy + mj_forward(m, d); + mjtNum initial_energy = d->energy[0] + d->energy[1]; + ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero"; + + // step forward and check energy decay + mjtNum max_energy = initial_energy; + int nsteps = 100; + 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 never exceed initial (system must not go unstable) + EXPECT_LE(max_energy, initial_energy * 1.01) + << "energy exceeded initial by more than 1%: max=" << max_energy + << ", initial=" << initial_energy; + + // after 100 steps (0.1 seconds), energy should have decayed significantly + mjtNum final_energy = d->energy[0] + d->energy[1]; + EXPECT_LT(final_energy, 0.5 * initial_energy) + << "energy did not decay by at least 50% after " << nsteps << " steps" + << " (initial=" << initial_energy << ", final=" << final_energy << ")"; + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco