From 955ae3f3c0d19b5225fa66b5d88e8f70e7fba89b Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Wed, 13 May 2026 07:14:36 -0700
Subject: [PATCH] Fix sign error in flex interp implicit integration.

The flex interp stiffness matrix is Negative Semi-Definite (NSD). When forming the RHS for implicit integration, the term involving the velocity and stiffness should be added, not subtracted. A new test is added to ensure energy stability for flex interp stretch stiffness with the implicitfast integrator.

PiperOrigin-RevId: 914845245
Change-Id: Iaaf0914909128e64e195f17cc5f2f344a8a43bc2
---
 src/engine/engine_derivative.c     |  4 ---
 src/engine/engine_forward.c        |  6 ++--
 test/engine/engine_forward_test.cc | 52 ++++++++++++++++++++++++++++++
 3 files changed, 54 insertions(+), 8 deletions(-)

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"(
+  <mujoco>
+    <option gravity="0 0 0" timestep="0.001" integrator="implicitfast">
+      <flag energy="enable"/>
+    </option>
+    <worldbody>
+      <flexcomp type="grid" count="4 4 4" cellcount="3 3 3"
+                spacing=".05 .05 .05" radius=".005" name="cube"
+                dim="3" mass="10" dof="trilinear">
+        <elasticity young="1e6" poisson="0.3" damping="0"/>
+        <contact selfcollide="none" internal="false"/>
+      </flexcomp>
+    </worldbody>
+  </mujoco>
+  )";
+
+  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