From 3a7aa84e53828f329cdb961ccbd920f4a7b41706 Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Mon, 3 Nov 2025 03:00:18 -0800
Subject: [PATCH] Add quadratic stiffness to flex.

PiperOrigin-RevId: 827409919
Change-Id: I3dff8ea49fb1726fec4acf5b91138d6d52c4bfba
---
 doc/XMLreference.rst                |  16 +++-
 doc/changelog.rst                   |   2 +
 model/flex/bunny_quadratic.xml      |  42 +++++++++++
 model/flex/quadratic.xml            |   7 +-
 src/engine/engine_core_constraint.c |  10 +--
 src/engine/engine_passive.c         |   6 +-
 src/engine/engine_vis_visualize.c   |  24 +++---
 src/user/user_flexcomp.cc           |   7 +-
 src/user/user_mesh.cc               | 109 ++++++++++++++++++----------
 src/xml/xml_native_reader.cc        |   3 -
 10 files changed, 161 insertions(+), 65 deletions(-)
 create mode 100644 model/flex/bunny_quadratic.xml

diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst
index af1f33c2..5dfd736c 100644
--- a/doc/XMLreference.rst
+++ b/doc/XMLreference.rst
@@ -3540,15 +3540,25 @@ saving the XML:
       :align: right
       :width: 240px
 
-   Trilinear flexes are much faster than the previous two options, and are the preferred choice if the expected
-   deformations can be captured by the reduced parametriation. For example, see the video on the right comparing `full
-   <https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper.xml>`__ and `trilinear
+   Trilinear and quadratic flexes are much faster than the previous two options, and are the preferred choice if the
+   expected deformations can be captured by the reduced parametriation. For example, see the video on the right
+   comparing `full <https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper.xml>`__ and `trilinear
    <https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper_trilinear.xml>`__ flexes for modeling
    deformable gripper pads.
 
    Note that the choice of dof parametrization affects the deformation modes of the flex but has no effect on the
    accuracy of the collision geometry, which always takes into account the high-resolution mesh of the flex.
 
+   **quadratic**
+     Three translational dofs per corner, edge, face, and volume of the bounding box of the flex, for a total of 81 dofs
+     for the entire flex, independent of the number of vertices. The positions of the vertices are updated using
+     quadratic interpolation over the bounding box. While this option requires more degrees of freedom than trilinear
+     flexes, it enables curved deformation modes, while the only modes achievable for trilinear flexes are
+     strech/compression and shear.
+
+   Note that a higher interpolation order generally requires a smaller time step for stability, although usually not as
+   large as with the "full" option and a fine mesh.
+
 .. _body-flexcomp-type:
 
 :at:`type`: :at-val:`[grid, box, cylinder, ellipsoid, disc, circle, mesh, gmsh, direct], "grid"`
diff --git a/doc/changelog.rst b/doc/changelog.rst
index adb259ca..3be2ab46 100644
--- a/doc/changelog.rst
+++ b/doc/changelog.rst
@@ -8,6 +8,8 @@ Upcoming version (not yet released)
 General
 ^^^^^^^^^
 
+- Added "quadratic" option to :ref:`flexcomp/dof<body-flexcomp-dof>`. This type of fast :ref:`deformable<CDeformable>`
+  flex object is similar to the "trilinear" option, but it includes curved deformations.
 - Raise an error if there are name collisions also during parsing.
 - Increase Windows stack size to 16MB to enable models with deep nested body hierarchies.
 - Added a new :ref:`mj_extractState` function that allows a subset of a state that was previously returned by
diff --git a/model/flex/bunny_quadratic.xml b/model/flex/bunny_quadratic.xml
new file mode 100644
index 00000000..5b98759f
--- /dev/null
+++ b/model/flex/bunny_quadratic.xml
@@ -0,0 +1,42 @@
+<!-- Copyright 2024 DeepMind Technologies Limited
+
+     Licensed under the Apache License, Version 2.0 (the "License");
+     you may not use this file except in compliance with the License.
+     You may obtain a copy of the License at
+
+         http://www.apache.org/licenses/LICENSE-2.0
+
+     Unless required by applicable law or agreed to in writing, software
+     distributed under the License is distributed on an "AS IS" BASIS,
+     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+     See the License for the specific language governing permissions and
+     limitations under the License.
+-->
+
+<mujoco model="Trilinear">
+  <include file="scene.xml"/>
+
+  <option solver="CG" tolerance="1e-6" timestep=".0005" integrator="implicitfast"/>
+
+  <size memory="100M"/>
+
+  <visual>
+    <map stiffness="100"/>
+  </visual>
+
+  <worldbody>
+    <body>
+      <joint name="press" type="slide" axis="0 0 1" damping="500"/>
+      <geom type="box" size=".02 .2 .2" pos="0 0 .5"/>
+    </body>
+    <flexcomp type="mesh" file="bunny.obj" pos="0 0 -.01" dim="2" euler="90 0 0"
+              radius=".002" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="quadratic">
+      <elasticity young="1e3" poisson="0.1" damping="0.0001" elastic2d="stretch"/>
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+  </worldbody>
+
+  <actuator>
+    <position name="press" joint="press" gear="-1 0 0 0 0 0" ctrlrange="-1 1" kp="1000"/>
+  </actuator>
+</mujoco>
diff --git a/model/flex/quadratic.xml b/model/flex/quadratic.xml
index 02c7b996..a2f4145d 100644
--- a/model/flex/quadratic.xml
+++ b/model/flex/quadratic.xml
@@ -13,12 +13,10 @@
      limitations under the License.
 -->
 
-<mujoco model="Trilinear">
+<mujoco model="Quadratic">
   <include file="scene.xml"/>
 
-  <option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast">
-    <flag gravity="disable"/>
-  </option>
+  <option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
 
   <size memory="10M"/>
 
@@ -33,6 +31,7 @@
     </body>
     <flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1" dim="3"
               radius=".001" rgba="0 .7 .7 1" mass="5" name="softbody" dof="quadratic">
+      <elasticity young="1e4" poisson="0.1" damping="0.0001"/>
       <contact selfcollide="none" internal="false"/>
     </flexcomp>
   </worldbody>
diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c
index fc024029..3915745f 100644
--- a/src/engine/engine_core_constraint.c
+++ b/src/engine/engine_core_constraint.c
@@ -863,8 +863,8 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
   else {
     // get bodies and weights
     int nb = 0;
-    int bid[64];
-    mjtNum bweight[64];
+    int bid[729];  // 729 = 27*27
+    mjtNum bweight[729];
     for (int side=0; side < 2; side++) {
       int nw = 0;
       int vid[4];
@@ -1121,8 +1121,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
       tran = rot = 0;
       for (int side=0; side < 2; side++) {
         // get bodies and weights
-        int nb = 0, bid[32], vid[4], nw = 0;
-        mjtNum bweight[32], bw[4];
+        int nb = 0, bid[729], vid[4], nw = 0;
+        mjtNum bweight[729], bw[4];
 
         // geom
         if (con->geom[side] >= 0) {
@@ -1808,7 +1808,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
     int NV = 0;
     if (nnz) {
       // get bodies
-      int nb = 0, bid[64];
+      int nb = 0, bid[729];
       for (int side=0; side < 2; side++) {
         int nw = 0;
         int vid[4];
diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c
index a8396f4b..95aa16af 100644
--- a/src/engine/engine_passive.c
+++ b/src/engine/engine_passive.c
@@ -206,8 +206,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
     }
 
     if (m->flex_interp[f]) {
-      mjtNum xpos[mjMAXFLEXNODES], displ[mjMAXFLEXNODES], vel[mjMAXFLEXNODES];
-      mjtNum frc[mjMAXFLEXNODES], dmp[mjMAXFLEXNODES];
+      mjtNum xpos[3*mjMAXFLEXNODES], displ[3*mjMAXFLEXNODES], vel[3*mjMAXFLEXNODES];
+      mjtNum frc[3*mjMAXFLEXNODES], dmp[3*mjMAXFLEXNODES];
       mjtNum com[3] = {0};
       mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
       int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
@@ -242,7 +242,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
       // compute the Jacobian at the center of mass
       mjtNum mat[9] = {0};
       mjtNum p[3] = {.5, .5, .5};
-      mju_defGradient(mat, p, xpos, 1);
+      mju_defGradient(mat, p, xpos, m->flex_interp[f]);
 
       // find rotation
       mjtNum quat[4] = {1, 0, 0, 0};
diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c
index c107b9ba..297183c5 100644
--- a/src/engine/engine_vis_visualize.c
+++ b/src/engine/engine_vis_visualize.c
@@ -1390,6 +1390,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
     mjtNum xpos[mjMAXFLEXNODES];
     int nstart = m->flex_nodeadr[f];
     int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
+    int nnode = m->flex_interp[f]+1;
     if (m->flex_centered[f]) {
       for (int i=0; i < m->flex_nodenum[f]; i++) {
         mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
@@ -1400,34 +1401,39 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
         mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
       }
     }
-    for (int i=0; i < 2; i++) {
-      for (int j=0; j < 2; j++) {
-        for (int k=0; k < 2; k++) {
-          if (i == 0) {
+    for (int i=0; i < nnode; i++) {
+      for (int j=0; j < nnode; j++) {
+        for (int k=0; k < nnode; k++) {
+          int nn = nnode*nnode;
+          int offset  = 3*(nn*(i+0) + nnode*(j+0) + k);
+          int offset1 = 3*(nn*(i+1) + nnode*(j+0) + k);
+          int offset2 = 3*(nn*(i+0) + nnode*(j+1) + k);
+          int offset3 = 3*(nn*(i+0) + nnode*(j+0) + (k+1));
+          if (i < nnode-1) {
             mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
             if (!thisgeom) {
               return;
             }
 
-            mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k));
+            mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
             releaseGeom(&thisgeom, scn);
           }
-          if (j == 0) {
+          if (j < nnode-1) {
             mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
             if (!thisgeom) {
               return;
             }
 
-            mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k));
+            mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
             releaseGeom(&thisgeom, scn);
           }
-          if (k == 0) {
+          if (k < nnode-1) {
             mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
             if (!thisgeom) {
               return;
             }
 
-            mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1)));
+            mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
             releaseGeom(&thisgeom, scn);
           }
         }
diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc
index 63a61790..83fa18a7 100644
--- a/src/user/user_flexcomp.cc
+++ b/src/user/user_flexcomp.cc
@@ -535,6 +535,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
     std::vector<double> node(3*(order+1)*(order+1)*(order+1), 0);
     int idx = 0;
     double step = 1.0 / (double)order;
+    double massP2[3] = {1. / 6., 2. / 3., 1. / 6.};
     for (int i=0; i <= order; i++) {
       for (int j=0; j <= order; j++) {
         for (int k=0; k <= order; k++) {
@@ -552,7 +553,11 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
           pb->pos[1] = minmax[1] + j * step * (minmax[4] - minmax[1]);
           pb->pos[2] = minmax[2] + k * step * (minmax[5] - minmax[2]);
           mjuu_zerovec(pb->ipos, 3);
-          pb->mass = mass / 8;
+          if (doftype == mjFCOMPDOF_TRILINEAR) {
+            pb->mass = mass / 8;
+          } else {
+            pb->mass = mass * massP2[i] * massP2[j] * massP2[k];
+          }
           pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
           pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
           pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc
index 775b39d9..8c46840b 100644
--- a/src/user/user_mesh.cc
+++ b/src/user/user_mesh.cc
@@ -3788,33 +3788,69 @@ void inline ComputeBending(double* bending, double* pos, const int v[4], double
 // Gauss Legendre quadrature points in 1 dimension on the interval [a, b]
 void quadratureGaussLegendre(double* points, double* weights,
                              const int order, const double a, const double b) {
-  if (order > 2)
-    mju_error("Integration order > 2 not yet supported.");
+  if (order > 3)
+    mju_error("Integration order > 3 not yet supported.");
 
   // x is on [-1, 1], p on [a, b]
   double p0 = (a+b)/2.;
   double dpdx = (b-a)/2;
-  points[0] = -dpdx/sqrt(3) + p0;
-  points[1] =  dpdx/sqrt(3) + p0;
-  weights[0] = dpdx;
-  weights[1] = dpdx;
-}
 
-// evaluate 1-dimensional basis function
-double phi(const double s, const double component) {
-  if (component == 0) {
-    return 1-s;
+  if (order == 2) {
+    points[0] = -dpdx / sqrt(3) + p0;
+    points[1] =  dpdx / sqrt(3) + p0;
+    weights[0] = dpdx;
+    weights[1] = dpdx;
   } else {
-    return s;
+    points[0] = p0;
+    points[1] = -dpdx / sqrt(3. / 5.) + p0;
+    points[2] =  dpdx / sqrt(3. / 5.) + p0;
+    weights[0] = 8. / 9. * dpdx;
+    weights[1] = 5. / 9. * dpdx;
+    weights[2] = 5. / 9. * dpdx;
   }
 }
 
-// evaluate gradient fo 1-dimensional basis function
-double dphi(const double s, const double component) {
-  if (component == 0) {
-    return -1;
+// evaluate 1-dimensional basis function
+double phi(const double s, const int i, const int order) {
+  if (order == 1) {
+    return i == 0 ? 1 - s : s;
+  } else if (order == 2) {
+    switch (i) {
+      case 0:
+        return 2 * s * s - 3 * s + 1;
+      case 1:
+        return 4 * (s - s * s);
+      case 2:
+        return 2 * s * s - s;
+      default:
+        mjERROR("invalid index %d", i);
+        return 0;
+    }
   } else {
-    return 1;
+    mju_error("Order must be 1 or 2.");
+    return 0;
+  }
+}
+
+// evaluate gradient of 1-dimensional basis function
+double dphi(const double s, const int i, const int order) {
+  if (order == 1) {
+    return i == 0 ? -1 : 1;
+  } else if (order == 2) {
+    switch (i) {
+      case 0:
+        return 4 * s - 3;
+      case 1:
+        return 4 * (1 - 2 * s);
+      case 2:
+        return 4 * s - 1;
+      default:
+        mjERROR("invalid index %d, must be 0, 1, or 2", i);
+        return 0;
+    }
+  } else {
+    mju_error("Order must be 1 or 2.");
+    return 0;
   }
 }
 
@@ -3851,21 +3887,20 @@ double inline trace(const Matrix& tensor) {
 
 void inline ComputeLinearStiffness(std::vector<double>& K,
                                    const double* pos,
-                                   double E, double nu) {
-  // only linear elements are supported for now
-  int order = 2;
-  int n = pow(order, 3);
+                                   double E, double nu, int order) {
+  int nbasis = order + 1;
+  int n = pow(nbasis, 3);
   int ndof = 3*n;
 
   // compute quadrature points
-  std::vector<double> points(order);     // quadrature points
-  std::vector<double> weight(order);     // quadrature weights
-  quadratureGaussLegendre(points.data(), weight.data(), order, 0, 1);
+  std::vector<double> points(nbasis);     // quadrature points
+  std::vector<double> weight(nbasis);     // quadrature weights
+  quadratureGaussLegendre(points.data(), weight.data(), nbasis, 0, 1);
 
   // compute element transformation
-  double dx = (pos+12)[0] - pos[0];
-  double dy = (pos+ 6)[1] - pos[1];
-  double dz = (pos+ 3)[2] - pos[2];
+  double dx = (pos+3*(n-1))[0] - pos[0];
+  double dy = (pos+3*(n-1))[1] - pos[1];
+  double dz = (pos+3*(n-1))[2] - pos[2];
   double detJ = dx * dy * dz;
   double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
 
@@ -3875,9 +3910,9 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
   double mu = E / (2 * (1 + nu));
 
   // loop over quadrature points
-  for (int ps=0; ps < order; ps++) {
-    for (int pt=0; pt < order; pt++) {
-      for (int pu=0; pu < order; pu++) {
+  for (int ps=0; ps < nbasis; ps++) {
+    for (int pt=0; pt < nbasis; pt++) {
+      for (int pu=0; pu < nbasis; pu++) {
         double s = points[ps];
         double t = points[pt];
         double u = points[pu];
@@ -3885,13 +3920,13 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
         int dof = 0;
 
         // cartesian product of basis functions
-        for (int bx=0; bx < order; bx++) {
-          for (int by=0; by < order; by++) {
-            for (int bz=0; bz < order; bz++) {
+        for (int bx=0; bx < nbasis; bx++) {
+          for (int by=0; by < nbasis; by++) {
+            for (int bz=0; bz < nbasis; bz++) {
               std::array<double, 3> gradient;
-              gradient[0] = dphi(s, bx) *  phi(t, by) *  phi(u, bz);
-              gradient[1] =  phi(s, bx) * dphi(t, by) *  phi(u, bz);
-              gradient[2] =  phi(s, bx) *  phi(t, by) * dphi(u, bz);
+              gradient[0] = dphi(s, bx, order) *  phi(t, by, order) *  phi(u, bz, order);
+              gradient[1] =  phi(s, bx, order) * dphi(t, by, order) *  phi(u, bz, order);
+              gradient[2] =  phi(s, bx, order) *  phi(t, by, order) * dphi(u, bz, order);
               F[dof++] = gradient;
             }
           }
@@ -4301,7 +4336,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
       if (min_size > nelem) {
         throw mjCError(this, "Trilinear dofs are require at least %d elements", "", min_size);
       }
-      ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
+      ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson, order_);
     }
 
     // geometrically nonlinear elasticity
diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc
index 93411a8d..7ffdf629 100644
--- a/src/xml/xml_native_reader.cc
+++ b/src/xml/xml_native_reader.cc
@@ -2723,9 +2723,6 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
     ReadAttr(elasticity, "damping", 1, &dflex.damping, text);
     ReadAttr(elasticity, "thickness", 1, &dflex.thickness, text);
     MapValue(elasticity, "elastic2d", &dflex.elastic2d, elastic2d_map, 4);
-    if (fcomp.doftype == mjFCOMPDOF_QUADRATIC) {
-      throw mjXError(elasticity, "elasticity is not yet supported for quadratic flex");
-    }
   }
 
   // check errors