GitHub Pull Request: #3617

--
a97aea7 by Alessio <small.quail@gmail.com>:

Fix flex bending attachments and cached frame couplings

COPYBARA_INTEGRATE_REVIEW=#3617 from smallquail:fix-flex-bending-frames a97aea7
PiperOrigin-RevId: 988168360
Change-Id: Ica3135946e5279e3d3848e35da38088f47b6d2e1

Author: smallquail

doc/XMLreference.rst

@@ -4544,6 +4544,10 @@ stress-strain relationship. See also :ref:`deformable <CDeformable>` objects and
    "both": bending and stretching. Bending is not yet supported by :ref:`dof<body-flexcomp-dof>` **trilinear** and
    **quadratic**.
 
+   For non-rigid, non-interpolated flexes with bending stiffness, each vertex must be attached to a fixed body or a body with
+   three slide joints along its local positive X, Y and Z axes, in that order. Jointless welded children of such bodies
+   are supported. The slide body's ancestors must be fixed; articulated and mocap attachments are not supported.
+
 .. _flex-contact:
 
 :el-prefix:`flex/` |-| **contact** |?|

doc/changelog.rst

@@ -12,6 +12,9 @@ General
 - Assets in a :ref:`meshdir<compiler-meshdir>` or :ref:`texturedir<compiler-texturedir>` now resolve correctly in
   :ref:`.mjz <MJZArchives>` archives when rewritten or when the directory path is absolute, uses ``..``, or uses a URI
   scheme.
+- The cached flex bending factor now retains cross-coordinate couplings between differently oriented vertex bodies.
+- Unsupported attachments of flex vertices with bending stiffness now produce a compiler error, including vertices
+  welded to ball joints, non-XYZ slide joints, or bodies with moving ancestors.
 
 Version 3.14.0 (September 22, 2026)
 -----------------------------------

src/engine/engine_setconst.c

@@ -1365,9 +1365,8 @@ static void setSpring(mjModel* m, mjData* d) {
 // reverse-Cholesky of M + (h^2 + h*damping)*K_bend over
 // the dofs of unpinned vertices of standard dim-2 flexes with bending. The matrix is constant
 // (flat-rest bending stiffness, point masses), so the factor is computed here once and reused
-// by the implicit-flex constraint solve every step. Bending couples only same-coordinate dofs,
-// so the pattern is three interleaved copies of the vertex flap adjacency. Row order (flex
-// order, vertex order, coordinate fastest) and the resulting fill count must match the
+// by the implicit-flex constraint solve every step. Bending couples full 3x3 blocks when vertex
+// frames differ. Row order (body order, coordinate fastest) and the fill count must match the
 // compiler's symbolic sizing (checked below).
 static void setEfm0Factor(mjModel* m, mjData* d) {
   int nbd = m->nefm0dof;
@@ -1415,10 +1414,9 @@ static void setEfm0Factor(mjModel* m, mjData* d) {
   }
 
   // assemble the bending-only stiffness K = (h^2 + h*damping)*K_bend over all dofs with the
-  // shared stencil walker from engine_derivative: bending values are configuration-independent
-  // and stretch/interp are gated off, so the call is valid at set-constants time (d is used
-  // for stack scratch only). As in the per-step metric, the h^2 and h*damping parts enter only
-  // when the spring and damper forces are enabled
+  // shared stencil walker from engine_derivative. The compiler requires fixed vertex frames,
+  // whose orientations were computed by setSpring. As in the per-step metric, the h^2 and
+  // h*damping parts enter only when the spring and damper forces are enabled.
   mjtNum s1 = mjDISABLED(mjDSBL_SPRING) ? 0 : h*h;
   mjtNum s2 = mjDISABLED(mjDSBL_DAMPER) ? 0 : h;
   int nv = m->nv;
@@ -1439,16 +1437,16 @@ static void setEfm0Factor(mjModel* m, mjData* d) {
     dofrow[m->efm0_dofid[r]] = r;
   }
 
-  // compact B to covered rows, keeping same-coordinate entries only: bending blocks are
-  // isotropic (q * I3), so the off-coordinate entries of assemble's 3x3 block pattern are
-  // structurally zero and dropping them preserves the pattern the compiler sized.
+  // Compact K_bend to covered rows, retaining full off-diagonal blocks Q_ij * R_i^T * R_j.
+  // Only same-body blocks are diagonal, even when several vertices share a weld parent.
+  // Keep structural zeros between bodies so the pattern is independent of their orientations.
   // H = M + (h^2+h*d)*K_bend in compact dof indices: lower CSR (values) + upper CSR (pattern)
   int nHl = 0, nHu = 0;
   for (int r=0; r < nbd; r++) {
     int dof = m->efm0_dofid[r];
     for (int c=0; c < K_rownnz[dof]; c++) {
       int rc = dofrow[K_colind[K_rowadr[dof] + c]];
-      if (rc < 0 || (rc - r) % 3 != 0) continue;  // uncovered or off-coordinate
+      if (rc < 0 || (rc != r && rc/3 == r/3)) continue;
       if (rc < r) nHl++;
       else if (rc > r) nHu++;
     }
@@ -1472,7 +1470,7 @@ static void setEfm0Factor(mjModel* m, mjData* d) {
     for (int c=0; c < K_rownnz[dof]; c++) {
       int adr = K_rowadr[dof] + c;
       int rc = dofrow[K_colind[adr]];
-      if (rc < 0 || (rc - r) % 3 != 0) continue;
+      if (rc < 0 || (rc != r && rc/3 == r/3)) continue;
       if (rc < r) {
         Hl_colind[ladr] = rc;
         Hl_val[ladr++] = K_val[adr];

src/user/user_mesh.cc

@@ -4162,19 +4162,6 @@ void mjCFlex::ResolveReferences(const mjCModel* m) {
     mjCBody* pbody = static_cast<mjCBody*>(m->FindObject(mjOBJ_BODY, vertbody));
     if (pbody) {
       vertbodyid.push_back(pbody->id);
-      // pinned vertices with bending are only valid for static (jointless) pin
-      // bodies: the runtime treats pin velocity as zero, which is only correct
-      // for static bodies.
-      if (!pbody->joints.empty() &&
-          pbody->joints.size() != 3 &&
-          dim == 2 &&
-          (elastic2d == 1 || elastic2d == 3) &&
-          !interpolated) {
-        throw mjCError(this,
-                       "pinned flex vertices with bending require a static (jointless) "
-                       "pin body, body '%s' has joints",
-                       vertbody.c_str());
-      }
     } else {
       throw mjCError(this, "unknown body '%s' in flex", vertbody.c_str());
     }
@@ -4824,6 +4811,36 @@ void mjCFlex::Compile(const mjVFS* vfs) {
 
     // bending stiffness (2D only)
     if (dim == 2 && (elastic2d == 1 || elastic2d == 3) && !interpolated) {
+      // The bending kernels use R * qvel and R^T * force on the weld parent's XYZ slides.
+      // Validate compiled axes (including joint frames), and exclude articulated or mocap
+      // ancestors: their motion is absent from that mapping and makes the cached factor vary.
+      if (!rigid) {
+        for (int bid : vertbodyid) {
+          const mjCBody* body      = model->Bodies()[bid];
+          const mjCBody* weld      = model->Bodies()[body->weldid];
+          bool           supported = weld->joints.empty();
+          if (weld->joints.size() == 3) {
+            supported = true;
+            for (int j = 0; j < 3; j++) {
+              const mjCJoint* joint  = weld->joints[j];
+              supported             &= joint->type == mjJNT_SLIDE;
+              for (int k = 0; k < 3; k++) {
+                supported &= std::abs(joint->axis[k] - (j == k)) <= mjEPS;
+              }
+            }
+          }
+          for (const mjCBody* ancestor = weld; ancestor; ancestor = ancestor->parent) {
+            supported &= !ancestor->mocap;
+            if (ancestor != weld) { supported &= ancestor->joints.empty(); }
+          }
+          if (!supported) {
+            throw mjCError(this,
+                           "flex bending requires a fixed body or three XYZ slide joints "
+                           "with fixed ancestors; unsupported vertex body '%s'",
+                           body->name.c_str());
+          }
+        }
+      }
       bending.assign(nedge * 17, 0);
 
       for (unsigned int e = 0; e < nedge; e++) {

src/user/user_model.cc

@@ -2122,8 +2122,7 @@ void mjCModel::SetSizes() {
   }
 
   // bending factor sizes: symbolic reverse-Cholesky count on the (M + K_bend) pattern.
-  // Bending couples only same-coordinate dofs of unpinned flap vertices, so the pattern is
-  // three interleaved copies of the vertex flap adjacency across all qualifying flexes.
+  // Each flap couples full 3x3 blocks: its vertex bodies can have different orientations.
   // The count must match the symbolic factorization performed in mj_setConst (asserted there).
   std::vector<int> body_slot(bodies_.size(), -1);
   for (int i = 0; i < nflex; i++) {
@@ -2165,13 +2164,16 @@ void mjCModel::SetSizes() {
       }
     }
 
-    // dof-level upper-triangle pattern: row 3*s+k has columns {3*t+k : t > s, t in adj(s)}
+    // Expand each off-diagonal vertex block to all coordinate pairs. Diagonal blocks are
+    // diagonal (R_b^T * R_b = I); any off-coordinate factor fill is counted symbolically.
     int                           n = 3 * nfree;
     std::vector<std::vector<int>> upper(n);
     for (int s = 0; s < nfree; s++) {
       for (int t : adj[s]) {
         if (t > s) {
-          for (int k = 0; k < 3; k++) { upper[3 * s + k].push_back(3 * t + k); }
+          for (int k = 0; k < 3; k++) {
+            for (int l = 0; l < 3; l++) { upper[3 * s + k].push_back(3 * t + l); }
+          }
         }
       }
     }

src/user/user_objects.h

@@ -704,6 +704,7 @@ class mjCJoint : public mjCJoint_, private mjsJoint {
   friend class mjCDef;
   friend class mjCEquality;
   friend class mjCBody;
+  friend class mjCFlex;
   friend class mjCModel;
   friend class mjCSensor;
   friend class mjXWriter;