Add quadratic stiffness to flex.

PiperOrigin-RevId: 827409919
Change-Id: I3dff8ea49fb1726fec4acf5b91138d6d52c4bfba

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
+   Trilinear and quadratic flexes are much faster than the previous two options, and are the preferred choice if the
-   deformations can be captured by the reduced parametriation. For example, see the video on the right comparing `full
+   expected deformations can be captured by the reduced parametriation. For example, see the video on the right
-   <https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper.xml>`__ and `trilinear
+   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"`

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

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>

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">
+  <option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
-    <flag gravity="disable"/>
-  </option>
 
   <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>

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];
+    int bid[729];  // 729 = 27*27
-    mjtNum bweight[64];
+    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;
+        int nb = 0, bid[729], vid[4], nw = 0;
-        mjtNum bweight[32], bw[4];
+        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];

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 xpos[3*mjMAXFLEXNODES], displ[3*mjMAXFLEXNODES], vel[3*mjMAXFLEXNODES];
-      mjtNum frc[mjMAXFLEXNODES], dmp[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};

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 i=0; i < nnode; i++) {
-      for (int j=0; j < 2; j++) {
+      for (int j=0; j < nnode; j++) {
-        for (int k=0; k < 2; k++) {
+        for (int k=0; k < nnode; k++) {
-          if (i == 0) {
+          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);
           }
         }

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;

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)
+  if (order > 3)
-    mju_error("Integration order > 2 not yet supported.");
+    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
+  if (order == 2) {
-double phi(const double s, const double component) {
+    points[0] = -dpdx / sqrt(3) + p0;
-  if (component == 0) {
+    points[1] =  dpdx / sqrt(3) + p0;
-    return 1-s;
+    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
+// evaluate 1-dimensional basis function
-double dphi(const double s, const double component) {
+double phi(const double s, const int i, const int order) {
-  if (component == 0) {
+  if (order == 1) {
-    return -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) {
+                                   double E, double nu, int order) {
-  // only linear elements are supported for now
+  int nbasis = order + 1;
-  int order = 2;
+  int n = pow(nbasis, 3);
-  int n = pow(order, 3);
   int ndof = 3*n;
 
   // compute quadrature points
-  std::vector<double> points(order);     // quadrature points
+  std::vector<double> points(nbasis);     // quadrature points
-  std::vector<double> weight(order);     // quadrature weights
+  std::vector<double> weight(nbasis);     // quadrature weights
-  quadratureGaussLegendre(points.data(), weight.data(), order, 0, 1);
+  quadratureGaussLegendre(points.data(), weight.data(), nbasis, 0, 1);
 
   // compute element transformation
-  double dx = (pos+12)[0] - pos[0];
+  double dx = (pos+3*(n-1))[0] - pos[0];
-  double dy = (pos+ 6)[1] - pos[1];
+  double dy = (pos+3*(n-1))[1] - pos[1];
-  double dz = (pos+ 3)[2] - pos[2];
+  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 ps=0; ps < nbasis; ps++) {
-    for (int pt=0; pt < order; pt++) {
+    for (int pt=0; pt < nbasis; pt++) {
-      for (int pu=0; pu < order; pu++) {
+      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 bx=0; bx < nbasis; bx++) {
-          for (int by=0; by < order; by++) {
+          for (int by=0; by < nbasis; by++) {
-            for (int bz=0; bz < order; bz++) {
+            for (int bz=0; bz < nbasis; bz++) {
               std::array<double, 3> gradient;
-              gradient[0] = dphi(s, bx) *  phi(t, by) *  phi(u, bz);
+              gradient[0] = dphi(s, bx, order) *  phi(t, by, order) *  phi(u, bz, order);
-              gradient[1] =  phi(s, bx) * dphi(t, by) *  phi(u, bz);
+              gradient[1] =  phi(s, bx, order) * dphi(t, by, order) *  phi(u, bz, order);
-              gradient[2] =  phi(s, bx) *  phi(t, by) * dphi(u, bz);
+              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

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