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

@@ -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];

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