Merge pull request #3451 from smallquail:flex-geometric-stiffness

PiperOrigin-RevId: 958995742
Change-Id: Ibb60c3112b6dbc2373a5cd98df0477d639a5a001

doc/changelog.rst

@@ -65,6 +65,11 @@ Bug fixes
   large penetration depth between nearly touching thin boxes with positive margin, causing resting stacks to explode.
 - Fixed a bug in the box-box collider where penetrations deeper than a box's smallest half-size could produce no
   contacts, letting boxes tunnel through thin boxes. Fixes :issue:`1800`.
+- Fixed the flex stretch stiffness operator, which was the Gauss-Newton Hessian of the stretch force rather than its
+  Jacobian: the geometric (stress-proportional) term was missing. Only the tensile part of that term is added, since
+  it is positive semi-definite exactly when the edge is in tension, and its consumers require an SPD operator; the
+  stretch force itself is unchanged. This affects the implicit integrators and the implicit effective metric, so
+  flexes using ``elastic2d="stretch"`` integrate slightly differently. Bending-only flexes are unaffected.
 
 Version 3.11.0 (July 27, 2026)
 ------------------------------

src/engine/engine_derivative.c

@@ -1432,10 +1432,13 @@ static const int stretch_edges[2][6][2] = {
   {{0, 1}, {1, 2}, {2, 0}, {2, 3}, {0, 3}, {1, 3}}};
 
 // compute res += (s1 + s2*flex_damping) * K_stretch * vec for standard (non-interp) flex
-// stretch, where K_stretch is the Gauss-Newton Hessian of the passive stretch force in
+// stretch, where K_stretch is the Hessian of the passive stretch force in mj_flexPassiveStretch:
-// mj_flexPassiveStretch: with elongation e_a = L_a^2 - L0_a^2 and force
+// with elongation e_a = L_a^2 - L0_a^2 and force f = -sum_ab M_ab e_a grad(e_b)/2,
-// f = -sum_ab M_ab e_a grad(e_b)/2, the GN Hessian is K = 2 sum_ab M_ab (s_a d_a)(s_b d_b)^T,
+//   K = 2 sum_ab M_ab (s_a d_a)(s_b d_b)^T  +  sum_a Me_a (Laplacian_a (x) I3),
-// d_a the current edge vector. Pinned vertices (zero-dof bodies) contribute nothing.
+// d_a the current edge vector and Me_a = sum_b M_ab e_b the edge tension. The first (Gauss-Newton)
+// term alone is not the Jacobian of the force: without the second (geometric) term the operator is
+// only first-order correct, which shows up directly as finite-difference error against
+// -d(qfrc_passive)/dq. Pinned vertices (zero-dof bodies) contribute nothing.
 void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
                          mjtNum s1, mjtNum s2) {
   for (int f = 0; f < m->nflex; f++) {
@@ -1461,13 +1464,16 @@ void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum*
     const mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
     const int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
     const mjtNum* k = m->flex_stiffness + stiffnessadr;
+    const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[f];
+    const mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f];
+    const mjtNum* reference = m->flexedge_length0 + m->flex_edgeadr[f];
     int elemnum = m->flex_elemnum[f];
 
     for (int t = 0; t < elemnum; t++) {
       const int* vert = elem + (dim+1)*t;
 
       // current edge vectors and g_a = d_a . (vec_{a0} - vec_{a1}), zero on pinned vertices
-      mjtNum dvec[6][3];
+      mjtNum dvec[6][3], dw[6][3];
       mjtNum g[6];
       for (int e = 0; e < nedge; e++) {
         int v0 = vert[edge[e][0]], v1 = vert[edge[e][1]];
@@ -1487,7 +1493,8 @@ void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum*
         }
         for (int x = 0; x < 3; x++) {
           dvec[e][x] = xpos[3*v0+x] - xpos[3*v1+x];
-          g[e] += dvec[e][x]*(w0[x] - w1[x]);
+          dw[e][x] = w0[x] - w1[x];
+          g[e] += dvec[e][x]*dw[e][x];
         }
       }
 
@@ -1502,7 +1509,24 @@ void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum*
         }
       }
 
-      // scatter: res_{b0/b1} +/-= 2*scale*(sum_a M_ba g_a) * d_b
+      // Edge tension for the geometric term, keeping only its TENSILE part. The geometric block is
+      // Me_a*[[I,-I],[-I,I]] over the edge's two vertices, which is PSD iff Me_a >= 0; a compressed
+      // edge would make K indefinite, and its consumers (the CG constraint solver and the PCG in
+      // mjd_effSolve) both require SPD. The clamp is structural, so no eigendecomposition is
+      // needed. mj_flexPassiveStretch keeps the full Me_a: the force is unchanged, only the
+      // operator is projected.
+      mjtNum Me[6];
+      for (int e = 0; e < nedge; e++) {
+        Me[e] = 0;
+        for (int a = 0; a < nedge; a++) {
+          int idx = edgeelem[t*nedge + a];
+          Me[e] += metric[nedge*e + a]*(deformed[idx]*deformed[idx] -
+                                        reference[idx]*reference[idx]);
+        }
+        Me[e] = mju_max(Me[e], 0);
+      }
+
+      // scatter: res_{b0/b1} +/-= 2*scale*(sum_a M_ba g_a) * d_b + scale*Me_b * (vec_b0 - vec_b1)
       for (int e = 0; e < nedge; e++) {
         mjtNum coef = 0;
         for (int a = 0; a < nedge; a++) {
@@ -1512,7 +1536,7 @@ void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum*
         int b0 = bodyid[vert[edge[e][0]]], b1 = bodyid[vert[edge[e][1]]];
         mjtNum rw[3], rl[3];
         for (int x = 0; x < 3; x++) {
-          rw[x] = coef*dvec[e][x];
+          rw[x] = coef*dvec[e][x] + scale*Me[e]*dw[e][x];
         }
         if (m->body_dofnum[b0]) {   // world -> dof frame
           mji_mulMatTVec3(rl, d->xmat + 9*b0, rw);
@@ -1940,6 +1964,9 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr
       const int* elem = m->flex_elem + m->flex_elemdataadr[f];
       const mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
       const mjtNum* kk = m->flex_stiffness + m->flex_stiffnessadr[f];
+      const int* eelem = m->flex_elemedge + m->flex_elemedgeadr[f];
+      const mjtNum* elen = d->flexedge_length + m->flex_edgeadr[f];
+      const mjtNum* elen0 = m->flexedge_length0 + m->flex_edgeadr[f];
       for (int t = 0; t < m->flex_elemnum[f]; t++) {
         const int* vert = elem + (dim+1)*t;
 
@@ -1962,7 +1989,19 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr
           }
         }
 
+        // tensile edge tension for the geometric term (see the clamp note in mjd_flexStretch_mul)
+        mjtNum Me[6];
+        for (int e1 = 0; e1 < nedge; e1++) {
+          Me[e1] = 0;
+          for (int e2 = 0; e2 < nedge; e2++) {
+            int idx = eelem[t*nedge + e2];
+            Me[e1] += metric[nedge*e1 + e2]*(elen[idx]*elen[idx] - elen0[idx]*elen0[idx]);
+          }
+          Me[e1] = mju_max(Me[e1], 0);
+        }
+
         // per vertex pair: block += 2*scale * sum_ab M_ab s_a,vi s_b,vj d_a d_b^T
+        //                         + scale * (sum_a Me_a s_a,vi s_a,vj) * I3
         for (int i = 0; i < nvrt; i++) {
           int si = vslot[m->flex_vertadr[f] + vert[i]];
           if (si < 0) continue;
@@ -1984,6 +2023,19 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr
                 }
               }
             }
+            // geometric term: a multiple of I3, so the frame sandwich below leaves it unchanged
+            mjtNum geo = 0;
+            for (int a = 0; a < nedge; a++) {
+              mjtNum sa = (i == edget[a][0]) ? 1 : ((i == edget[a][1]) ? -1 : 0);
+              mjtNum sb = (j == edget[a][0]) ? 1 : ((j == edget[a][1]) ? -1 : 0);
+              if (!sa || !sb) continue;
+              geo += Me[a]*sa*sb;
+            }
+            geo *= scale;
+            blk[0] += geo;
+            blk[4] += geo;
+            blk[8] += geo;
+
             // blk is world-space but the destination dofs are the vertex bodies' own (possibly
             // rotated) slide axes: blk_dof = R_bi^T * blk_world * R_bj, matching the force path
             int bi = m->flex_vertbodyid[m->flex_vertadr[f] + vert[i]];

test/engine/engine_derivative_test.cc

@@ -1922,6 +1922,74 @@ TEST_F(DerivativeTest, FlexBendDerivativesRotated) {
   mj_deleteData(perturbed);
 }
 
+// K_stretch must be the full Hessian of the stretch force, not just its Gauss-Newton part: the
+// geometric (stress-proportional) term is what makes it the Jacobian at finite strain. Uniformly
+// dilating the mesh puts every edge in tension, so the tensile clamp is inactive and the operator
+// is exact -- with only the Gauss-Newton term the finite-difference error is a large fraction of
+// the force. FlexStretchDerivatives covers the near-rest limit, where the two agree anyway.
+TEST_F(DerivativeTest, FlexStretchDerivativesTensile) {
+  static const char* const kXml = R"(
+  <mujoco>
+    <option integrator="implicit"/>
+    <worldbody>
+      <flexcomp name="cloth" type="grid" count="4 4 1" spacing="0.1 0.1 0.1"
+                radius=".01" dim="2" mass="1" pos="0 0 1">
+        <contact selfcollide="none" contype="0" conaffinity="0"/>
+        <elasticity young="1e4" poisson="0.3" thickness="0.01"
+                    elastic2d="stretch" damping="0"/>
+      </flexcomp>
+    </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+  int nv = model->nv;
+  ASSERT_EQ(model->nq, nv);  // all slide dofs
+  MjDataPtr data = MakeData(model);
+  mj_forward(model.get(), data.get());
+
+  // dilate about the flex centroid: every edge stretches by 5%, so every Me is strictly positive
+  mjtNum centroid[3] = {0, 0, 0};
+  int nvert = model->flex_vertnum[0];
+  for (int v = 0; v < nvert; v++) {
+    mju_addTo3(centroid, data->flexvert_xpos + 3*v);
+  }
+  mju_scl3(centroid, centroid, 1.0/nvert);
+  for (int v = 0; v < nvert; v++) {
+    int body = model->flex_vertbodyid[model->flex_vertadr[0] + v];
+    int adr = model->body_dofadr[body];
+    for (int x = 0; x < 3; x++) {
+      data->qpos[adr+x] += 0.05*(data->flexvert_xpos[3*v+x] - centroid[x]);
+    }
+  }
+  mj_forward(model.get(), data.get());
+
+  std::vector<mjtNum> vec(nv), res(nv, 0);
+  for (int i = 0; i < nv; i++) {
+    vec[i] = mju_Halton(i, 2) - 0.5;
+  }
+  mjd_flexStretch_mul(model.get(), data.get(), res.data(), vec.data(), 1, 0);
+
+  mjtNum eps = MjTol(1e-7, 1e-4);
+  mjData* perturbed = mj_copyData(NULL, model.get(), data.get());
+  mju_addToScl(perturbed->qpos, vec.data(), eps, nv);
+  mj_forward(model.get(), perturbed);
+
+  mjtNum max_err = 0, scale = 0;
+  for (int i = 0; i < nv; ++i) {
+    mjtNum fd = -(perturbed->qfrc_passive[i] - data->qfrc_passive[i]) / eps;
+    max_err = mju_max(max_err, mju_abs(res[i] - fd));
+    scale = mju_max(scale, mju_abs(fd));
+  }
+  mj_deleteData(perturbed);
+
+  EXPECT_GT(scale, 1e-3) << "test should exercise nontrivial stretch stiffness";
+  EXPECT_LT(max_err, MjTol(1e-4, 1e-3) * scale)
+      << "mjd_flexStretch_mul is not the Jacobian of the flex stretch force";
+}
+
 // verify mjd_flexStretch_mul (Gauss-Newton Hessian of the standard-flex
 // stretch force) against finite differences of qfrc_passive, plus symmetry,
 // positive semi-definiteness and (s1, s2) scale linearity. The model covers
@@ -1956,9 +2024,9 @@ TEST_F(DerivativeTest, FlexStretchDerivatives) {
 
   MjDataPtr data = MakeData(model);
 
-  // deform both flexes deterministically. Keep the strain small: the operator
+  // deform both flexes deterministically, at small strain. FlexStretchDerivativesTensile
-  // is the Gauss-Newton Hessian, exact to O(strain) (the geometric term is
+  // covers finite strain, where the geometric term of K_stretch is what carries the accuracy;
-  // dropped, see FlexInterpDerivativesDeformed for the analogous property).
+  // the solid (dim=3) flex below has no such term, so the tolerance stays loose here.
   for (int i = 0; i < nv; i++) {
     data->qpos[i] += 5e-4 * (mju_Halton(i, 2) - 0.5);
   }
@@ -1979,9 +2047,8 @@ TEST_F(DerivativeTest, FlexStretchDerivatives) {
     mj_forward(model.get(), data_perturbed);
 
     // qfrc_passive = -dV/dq  =>  -(qfrc_new - qfrc)/eps ~= K * vec.
-    // Compare max error against the force scale: the operator omits the
+    // Compare max error against the force scale rather than entrywise: individual near-zero
-    // geometric (stress-proportional) term, so the residual is O(strain) of
+    // entries are not meaningful, and the dim=3 flex still carries a Gauss-Newton residual.
-    // the overall scale and individual near-zero entries are not meaningful.
     mjtNum max_err = 0, scale = 0;
     for (int i = 0; i < nv; ++i) {
       mjtNum fd =