Introduce trilinear flex parametrization.

These flexes use only 24 DOFs (3 per vertex of the bounding box), while colliding with the full high resolution mesh.

On an 8x8x8 cube, the performance using DOFs at all vertices is

```
 Simulation time      : 18.74 s
 Steps per second     : 533
 Realtime factor      : 0.53 x
 Time per step        : 1874.4 µs

 Contacts per step    : 114.88
 Constraints per step : 3322.51
 Degrees of freedom   : 1536
```

With the new implementation, it is the following:

```
 Simulation time      : 1.82 s
 Steps per second     : 5507
 Realtime factor      : 5.51 x
 Time per step        : 181.6 µs

 Contacts per step    : 38.84
 Constraints per step : 155.36
 Degrees of freedom   : 24
```

PiperOrigin-RevId: 721008829
Change-Id: I833df027527db578d86667cc4b24295bcf6f7d22

doc/APIreference/functions.rst

@@ -3572,8 +3572,8 @@ Construct quaternion performing rotation from z-axis to given vector.
 
 .. mujoco-include:: mju_mat2Rot
 
-extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion
+Extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion.
-returns the number of iterations required to converge
+Returns the number of iterations required to converge
 
 .. _mju_euler2Quat:
 

doc/XMLreference.rst

@@ -3487,6 +3487,45 @@ saving the XML:
    radius in 2D, and tetrahedra with radius in 3D. Certain flexcomp types imply a dimensionality, in which case the
    value specified here is ignored.
 
+.. youtube:: uNt3i8hrJu4
+   :align: right
+   :width: 240px
+
+.. _body-flexcomp-dof:
+
+:at:`dof`: :at-val:`[full, radial, trilinear], "full"`
+   The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the
+   different parametrizations with deformable spheres. The three models in the video are respectively
+   `sphere_full <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_full.xml>`__,
+   `sphere_radial <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_radial.xml>`__
+   and `sphere_trilinear <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_trilinear.xml>`__.
+
+   **full**
+     Three translational dofs per vertex. This is the most expressive but also the most expensive option.
+
+   **radial**
+     A single radial translational dof per vertex. Note that unlike in the "full" case, the radial parametrization
+     requires a free joint at the flex's parent in order for free body motion to be possible. This type of
+     parametrization is appropriate for shapes that are relatively spherical.
+
+   **trilinear**
+     Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire
+     flex, independent of the number of vertices. The positions of the vertices are updated using trilinear
+     interpolation over the bounding box.
+
+   .. youtube:: qJFbx-FR7Bc
+      :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
+   <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.
+
 .. _body-flexcomp-type:
 
 :at:`type`: :at-val:`[grid, box, cylinder, ellipsoid, disc, circle, mesh, gmsh, direct], "grid"`
@@ -4081,6 +4120,12 @@ cases, the user will specify a :el:`flexcomp` which will then automatically cons
    Integer group to which the flex belongs. This attribute can be used for custom tags. It is also used by the
    visualizer to enable and disable the rendering of entire groups of flexes.
 
+.. _deformable-flex-node:
+
+:at:`node`: :at-val:`string(nnode), optional`
+   The degrees-of-freedom of the flex.
+   An array of MuJoCo body names (separated by white space) to which each node belongs. The number of body names
+   should equal the number of nodes (nnode). See the flexcomp :ref:`dof<body-flexcomp-dof>` attribute for more details.
 
 .. _flex-edge:
 

doc/XMLschema.rst

@@ -435,17 +435,17 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`name<body-flexcomp-name>`                                 | :ref:`type<body-flexcomp-type>`                                 | :ref:`group<body-flexcomp-group>`                               | :ref:`dim<body-flexcomp-dim>`                                   | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`count<body-flexcomp-count>`                               | :ref:`spacing<body-flexcomp-spacing>`                           | :ref:`radius<body-flexcomp-radius>`                             | :ref:`rigid<body-flexcomp-rigid>`                               | |
+|                                    |    |    | :ref:`dof<body-flexcomp-dof>`                                   | :ref:`count<body-flexcomp-count>`                               | :ref:`spacing<body-flexcomp-spacing>`                           | :ref:`radius<body-flexcomp-radius>`                             | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`mass<body-flexcomp-mass>`                                 | :ref:`inertiabox<body-flexcomp-inertiabox>`                     | :ref:`scale<body-flexcomp-scale>`                               | :ref:`file<body-flexcomp-file>`                                 | |
+|                                    |    |    | :ref:`rigid<body-flexcomp-rigid>`                               | :ref:`mass<body-flexcomp-mass>`                                 | :ref:`inertiabox<body-flexcomp-inertiabox>`                     | :ref:`scale<body-flexcomp-scale>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`point<body-flexcomp-point>`                               | :ref:`element<body-flexcomp-element>`                           | :ref:`texcoord<body-flexcomp-texcoord>`                         | :ref:`material<body-flexcomp-material>`                         | |
+|                                    |    |    | :ref:`file<body-flexcomp-file>`                                 | :ref:`point<body-flexcomp-point>`                               | :ref:`element<body-flexcomp-element>`                           | :ref:`texcoord<body-flexcomp-texcoord>`                         | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`rgba<body-flexcomp-rgba>`                                 | :ref:`flatskin<body-flexcomp-flatskin>`                         | :ref:`pos<body-flexcomp-pos>`                                   | :ref:`quat<body-flexcomp-quat>`                                 | |
+|                                    |    |    | :ref:`material<body-flexcomp-material>`                         | :ref:`rgba<body-flexcomp-rgba>`                                 | :ref:`flatskin<body-flexcomp-flatskin>`                         | :ref:`pos<body-flexcomp-pos>`                                   | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`axisangle<body-flexcomp-axisangle>`                       | :ref:`xyaxes<body-flexcomp-xyaxes>`                             | :ref:`zaxis<body-flexcomp-zaxis>`                               | :ref:`euler<body-flexcomp-euler>`                               | |
+|                                    |    |    | :ref:`quat<body-flexcomp-quat>`                                 | :ref:`axisangle<body-flexcomp-axisangle>`                       | :ref:`xyaxes<body-flexcomp-xyaxes>`                             | :ref:`zaxis<body-flexcomp-zaxis>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`origin<body-flexcomp-origin>`                             |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`euler<body-flexcomp-euler>`                               | :ref:`origin<body-flexcomp-origin>`                             |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_2| flexcomp |br| |_2| |L|        |    | .. table::                                                                                                                                                                                                                                                                   |
@@ -509,7 +509,7 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`material<deformable-flex-material>`                       | :ref:`rgba<deformable-flex-rgba>`                               | :ref:`flatskin<deformable-flex-flatskin>`                       | :ref:`body<deformable-flex-body>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`vertex<deformable-flex-vertex>`                           | :ref:`element<deformable-flex-element>`                         | :ref:`texcoord<deformable-flex-texcoord>`                       |                                                                 | |
+|                                    |    |    | :ref:`vertex<deformable-flex-vertex>`                           | :ref:`element<deformable-flex-element>`                         | :ref:`texcoord<deformable-flex-texcoord>`                       | :ref:`node<deformable-flex-node>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_2| flex |br| |_2| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -5,8 +5,27 @@ Changelog
 Upcoming version (not yet released)
 -----------------------------------
 
+
+Feature promotion
+^^^^^^^^^^^^^^^^^
+.. youtube:: qJFbx-FR7Bc
+   :align: right
+   :width: 240px
+
+- Introduced a new kind of **fast deformable body**, activated by setting :ref:`flexcomp/dof<body-flexcomp-dof>` to
+  "trilinear". This type of :ref:`deformable<CDeformable>` flex object has the same collision geometry as a regular
+  flex, but has far fewer degrees of freedom. Instead of 3 dofs per vertex, only the corners of the bounding box are
+  free to move, with the positions of the interior vertices computed with trilinear interpolation of the 8 corners, for
+  a total of 24 dofs for the entire flex object (or less, if some of the corners are pinned). This limits the types of
+  deformation achievable by the flex, but allows for much faster simulation. 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.
+
 General
 ^^^^^^^
+- Separate collision and deformation meshes for :ref:`flex<deformable-flex>`. This enables a fixed cost for the soft
+  body computations, while preserving the fidelity of high-resolution collisions.
 - Added :ref:`mjs_setDeepCopy` API function. When the deep copy flag is 0, attaching a model will not copy it to the
   parent, so the original references to the child can be used to modify the parent after attachment. The default
   behavior is to perform such a shallow copy. The old behavior of creating a deep copy of the child model while

doc/includes/references.h

@@ -901,6 +901,7 @@ struct mjModel_ {
   int ncam;                       // number of cameras
   int nlight;                     // number of lights
   int nflex;                      // number of flexes
+  int nflexnode;                   // number of dofs in all flexes
   int nflexvert;                  // number of vertices in all flexes
   int nflexedge;                  // number of edges in all flexes
   int nflexelem;                  // number of elements in all flexes
@@ -1153,6 +1154,9 @@ struct mjModel_ {
   int*      flex_dim;             // 1: lines, 2: triangles, 3: tetrahedra    (nflex x 1)
   int*      flex_matid;           // material id for rendering                (nflex x 1)
   int*      flex_group;           // group for visibility                     (nflex x 1)
+  int*      flex_interp;          // interpolation (0: vertex, 1: nodes)      (nflex x 1)
+  int*      flex_nodeadr;         // first node address                       (nflex x 1)
+  int*      flex_nodenum;         // number of nodes                          (nflex x 1)
   int*      flex_vertadr;         // first vertex address                     (nflex x 1)
   int*      flex_vertnum;         // number of vertices                       (nflex x 1)
   int*      flex_edgeadr;         // first edge address                       (nflex x 1)
@@ -1166,6 +1170,7 @@ struct mjModel_ {
   int*      flex_evpairadr;       // first evpair address                     (nflex x 1)
   int*      flex_evpairnum;       // number of evpairs                        (nflex x 1)
   int*      flex_texcoordadr;     // address in flex_texcoord; -1: none       (nflex x 1)
+  int*      flex_nodebodyid;      // node body ids                            (nflexnode x 1)
   int*      flex_vertbodyid;      // vertex body ids                          (nflexvert x 1)
   int*      flex_edge;            // edge vertex ids (2 per edge)             (nflexedge x 2)
   int*      flex_elem;            // element vertex ids (dim+1 per elem)      (nflexelemdata x 1)
@@ -1175,6 +1180,8 @@ struct mjModel_ {
   int*      flex_evpair;          // (element, vertex) collision pairs        (nflexevpair x 2)
   mjtNum*   flex_vert;            // vertex positions in local body frames    (nflexvert x 3)
   mjtNum*   flex_vert0;           // vertex positions in qpos0 on [0, 1]^d    (nflexvert x 3)
+  mjtNum*   flex_node;            // node positions in local body frames      (nflexnode x 3)
+  mjtNum*   flex_node0;           // Cartesian node positions in qpos0        (nflexnode x 3)
   mjtNum*   flexedge_length0;     // edge lengths in qpos0                    (nflexedge x 1)
   mjtNum*   flexedge_invweight0;  // edge inv. weight in qpos0                (nflexedge x 1)
   mjtNum*   flex_radius;          // radius around primitive element          (nflex x 1)
@@ -1983,7 +1990,9 @@ typedef struct mjsFlex_ {          // flex specification
   double thickness;                // thickness (2D only)
 
   // mesh properties
+  mjStringVec* nodebody;           // node body names
   mjStringVec* vertbody;           // vertex body names
+  mjDoubleVec* node;               // node positions
   mjDoubleVec* vert;               // vertex positions
   mjIntVec* elem;                  // element vertex ids
   mjFloatVec* texcoord;            // vertex texture coordinates
@@ -2968,6 +2977,10 @@ struct mjvSceneState_ {
     int* flex_dim;
     int* flex_matid;
     int* flex_group;
+    int* flex_interp;
+    int* flex_nodeadr;
+    int* flex_nodenum;
+    int* flex_nodebodyid;
     int* flex_vertadr;
     int* flex_vertnum;
     int* flex_elem;
@@ -2981,6 +2994,8 @@ struct mjvSceneState_ {
     int* flex_texcoordadr;
     int* flex_bvhadr;
     int* flex_bvhnum;
+    mjtByte* flex_centered;
+    mjtNum* flex_node;
     mjtNum* flex_radius;
     float* flex_rgba;
 

doc/modeling.rst

@@ -1335,9 +1335,7 @@ improved realism and accuracy. The edge-based model could be seen as a "lumped"
 coupling of deformation modes (e.g. shear and volumetric) is averaged in a single quantity. The continuum model enables
 instead to specify shear and volumetic stiffnesses separately using the `Poisson's ratio
 <https://en.wikipedia.org/wiki/Poisson%27s_ratio>`__ of the material. For more details, see the `Saint Venant-Kirchhoff
-<https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ hyperelastic model. This
+<https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ hyperelastic model.
-functionality is currently based on first-party :ref:`engine plugins<exPlugin>` as of MuJoCo 3.0 but may be integrated
-into the engine in future releases.
 
 **Creation and visualization**.
 
@@ -1345,20 +1343,12 @@ into the engine in future releases.
 
    <option timestep=".001"/>
 
-   <extension>
-      <plugin plugin="mujoco.elasticity.solid"/>
-   </extension>
-
    <worldbody>
       <flexcomp type="grid" count="24 4 4" spacing=".1 .1 .1" pos=".1 0 1.5"
                 radius=".0" rgba="0 .7 .7 1" name="softbody" dim="3" mass="7">
          <contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
          <edge damping="1"/>
-         <plugin plugin="mujoco.elasticity.solid">
+         <elasticity poisson="0.2" young="5e4">
-            <config key="poisson" value="0.2"/>
-            <!--Units are in Pa (SI)-->
-            <config key="young" value="5e4"/>
-         </plugin>
       </flexcomp>
    </worldbody>
 

include/mujoco/mjmodel.h

@@ -29,6 +29,7 @@
 #define mjMAXIMP        0.9999    // maximum constraint impedance
 #define mjMAXCONPAIR    50        // maximum number of contacts per geom pair
 #define mjMAXTREEDEPTH  50        // maximum bounding volume hierarchy depth
+#define mjMAXFLEXNODES  27        // maximum number of flex nodes
 
 
 //---------------------------------- sizes ---------------------------------------------------------
@@ -43,6 +44,7 @@
 #define mjNSOLVER       200       // size of one mjData.solver array
 #define mjNISLAND       20        // number of mjData.solver arrays
 
+
 //---------------------------------- enum types (mjt) ----------------------------------------------
 
 typedef enum mjtDisableBit_ {     // disable default feature bitflags
@@ -607,6 +609,7 @@ struct mjModel_ {
   int ncam;                       // number of cameras
   int nlight;                     // number of lights
   int nflex;                      // number of flexes
+  int nflexnode;                   // number of dofs in all flexes
   int nflexvert;                  // number of vertices in all flexes
   int nflexedge;                  // number of edges in all flexes
   int nflexelem;                  // number of elements in all flexes
@@ -859,6 +862,9 @@ struct mjModel_ {
   int*      flex_dim;             // 1: lines, 2: triangles, 3: tetrahedra    (nflex x 1)
   int*      flex_matid;           // material id for rendering                (nflex x 1)
   int*      flex_group;           // group for visibility                     (nflex x 1)
+  int*      flex_interp;          // interpolation (0: vertex, 1: nodes)      (nflex x 1)
+  int*      flex_nodeadr;         // first node address                       (nflex x 1)
+  int*      flex_nodenum;         // number of nodes                          (nflex x 1)
   int*      flex_vertadr;         // first vertex address                     (nflex x 1)
   int*      flex_vertnum;         // number of vertices                       (nflex x 1)
   int*      flex_edgeadr;         // first edge address                       (nflex x 1)
@@ -872,6 +878,7 @@ struct mjModel_ {
   int*      flex_evpairadr;       // first evpair address                     (nflex x 1)
   int*      flex_evpairnum;       // number of evpairs                        (nflex x 1)
   int*      flex_texcoordadr;     // address in flex_texcoord; -1: none       (nflex x 1)
+  int*      flex_nodebodyid;      // node body ids                            (nflexnode x 1)
   int*      flex_vertbodyid;      // vertex body ids                          (nflexvert x 1)
   int*      flex_edge;            // edge vertex ids (2 per edge)             (nflexedge x 2)
   int*      flex_elem;            // element vertex ids (dim+1 per elem)      (nflexelemdata x 1)
@@ -881,6 +888,8 @@ struct mjModel_ {
   int*      flex_evpair;          // (element, vertex) collision pairs        (nflexevpair x 2)
   mjtNum*   flex_vert;            // vertex positions in local body frames    (nflexvert x 3)
   mjtNum*   flex_vert0;           // vertex positions in qpos0 on [0, 1]^d    (nflexvert x 3)
+  mjtNum*   flex_node;            // node positions in local body frames      (nflexnode x 3)
+  mjtNum*   flex_node0;           // Cartesian node positions in qpos0        (nflexnode x 3)
   mjtNum*   flexedge_length0;     // edge lengths in qpos0                    (nflexedge x 1)
   mjtNum*   flexedge_invweight0;  // edge inv. weight in qpos0                (nflexedge x 1)
   mjtNum*   flex_radius;          // radius around primitive element          (nflex x 1)

include/mujoco/mjspec.h

@@ -439,7 +439,9 @@ typedef struct mjsFlex_ {          // flex specification
   double thickness;                // thickness (2D only)
 
   // mesh properties
+  mjStringVec* nodebody;           // node body names
   mjStringVec* vertbody;           // vertex body names
+  mjDoubleVec* node;               // node positions
   mjDoubleVec* vert;               // vertex positions
   mjIntVec* elem;                  // element vertex ids
   mjFloatVec* texcoord;            // vertex texture coordinates

include/mujoco/mjvisualize.h

@@ -516,6 +516,10 @@ struct mjvSceneState_ {
     int* flex_dim;
     int* flex_matid;
     int* flex_group;
+    int* flex_interp;
+    int* flex_nodeadr;
+    int* flex_nodenum;
+    int* flex_nodebodyid;
     int* flex_vertadr;
     int* flex_vertnum;
     int* flex_elem;
@@ -529,6 +533,8 @@ struct mjvSceneState_ {
     int* flex_texcoordadr;
     int* flex_bvhadr;
     int* flex_bvhnum;
+    mjtByte* flex_centered;
+    mjtNum* flex_node;
     mjtNum* flex_radius;
     float* flex_rgba;
 

include/mujoco/mjxmacro.h

@@ -81,6 +81,7 @@
     XMJV( ncam )               \
     XMJV( nlight )             \
     XMJV( nflex )              \
+    X   ( nflexnode )          \
     XMJV( nflexvert )          \
     X   ( nflexedge )          \
     X   ( nflexelem )          \
@@ -326,6 +327,9 @@
     XMJV( int,     flex_dim,              nflex,         1                    ) \
     XMJV( int,     flex_matid,            nflex,         1                    ) \
     XMJV( int,     flex_group,            nflex,         1                    ) \
+    XMJV( int,     flex_interp,           nflex,         1                    ) \
+    XMJV( int,     flex_nodeadr,          nflex,         1                    ) \
+    XMJV( int,     flex_nodenum,          nflex,         1                    ) \
     XMJV( int,     flex_vertadr,          nflex,         1                    ) \
     XMJV( int,     flex_vertnum,          nflex,         1                    ) \
     X   ( int,     flex_edgeadr,          nflex,         1                    ) \
@@ -339,6 +343,7 @@
     X   ( int,     flex_evpairadr,        nflex,         1                    ) \
     X   ( int,     flex_evpairnum,        nflex,         1                    ) \
     XMJV( int,     flex_texcoordadr,      nflex,         1                    ) \
+    XMJV( int,     flex_nodebodyid,       nflexnode,     1                    ) \
     X   ( int,     flex_vertbodyid,       nflexvert,     1                    ) \
     X   ( int,     flex_edge,             nflexedge,     2                    ) \
     XMJV( int,     flex_elem,             nflexelemdata, 1                    ) \
@@ -348,6 +353,8 @@
     X   ( int,     flex_evpair,           nflexevpair,   2                    ) \
     X   ( mjtNum,  flex_vert,             nflexvert,     3                    ) \
     X   ( mjtNum,  flex_vert0,            nflexvert,     3                    ) \
+    XMJV( mjtNum,  flex_node,             nflexnode,     3                    ) \
+    X   ( mjtNum,  flex_node0,            nflexnode,     3                    ) \
     X   ( mjtNum,  flexedge_length0,      nflexedge,     1                    ) \
     X   ( mjtNum,  flexedge_invweight0,   nflexedge,     1                    ) \
     XMJV( mjtNum,  flex_radius,           nflex,         1                    ) \
@@ -358,7 +365,7 @@
     X   ( mjtByte, flex_edgeequality,     nflex,         1                    ) \
     X   ( mjtByte, flex_rigid,            nflex,         1                    ) \
     X   ( mjtByte, flexedge_rigid,        nflexedge,     1                    ) \
-    X   ( mjtByte, flex_centered,         nflex,         1                    ) \
+    XMJV( mjtByte, flex_centered,         nflex,         1                    ) \
     XMJV( mjtByte, flex_flatskin,         nflex,         1                    ) \
     XMJV( int,     flex_bvhadr,           nflex,         1                    ) \
     XMJV( int,     flex_bvhnum,           nflex,         1                    ) \

include/mujoco/mujoco.h

@@ -1125,8 +1125,8 @@ MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale);
 // Construct quaternion performing rotation from z-axis to given vector.
 MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]);
 
-// extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion
+// Extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion.
-// returns the number of iterations required to converge
+// Returns the number of iterations required to converge
 MJAPI int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]);
 
 // Convert sequence of Euler angles (radians) to quaternion.

introspect/functions.py

@@ -7429,7 +7429,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  ),
              ),
          ),
-         doc='extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion returns the number of iterations required to converge',  # pylint: disable=line-too-long
+         doc='Extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion. Returns the number of iterations required to converge',  # pylint: disable=line-too-long
      )),
     ('mju_euler2Quat',
      FunctionDecl(

introspect/structs.py

@@ -898,6 +898,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='int'),
                  doc='number of flexes',
              ),
+             StructFieldDecl(
+                 name='nflexnode',
+                 type=ValueType(name='int'),
+                 doc='number of dofs in all flexes',
+             ),
              StructFieldDecl(
                  name='nflexvert',
                  type=ValueType(name='int'),
@@ -2440,6 +2445,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  doc='group for visibility',
                  array_extent=('nflex',),
              ),
+             StructFieldDecl(
+                 name='flex_interp',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='interpolation (0: vertex, 1: nodes)',
+                 array_extent=('nflex',),
+             ),
+             StructFieldDecl(
+                 name='flex_nodeadr',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='first node address',
+                 array_extent=('nflex',),
+             ),
+             StructFieldDecl(
+                 name='flex_nodenum',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='number of nodes',
+                 array_extent=('nflex',),
+             ),
              StructFieldDecl(
                  name='flex_vertadr',
                  type=PointerType(
@@ -2544,6 +2573,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  doc='address in flex_texcoord; -1: none',
                  array_extent=('nflex',),
              ),
+             StructFieldDecl(
+                 name='flex_nodebodyid',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='node body ids',
+                 array_extent=('nflexnode',),
+             ),
              StructFieldDecl(
                  name='flex_vertbodyid',
                  type=PointerType(
@@ -2615,6 +2652,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  doc='vertex positions in qpos0 on [0, 1]^d',
                  array_extent=('nflexvert', 3),
              ),
+             StructFieldDecl(
+                 name='flex_node',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='node positions in local body frames',
+                 array_extent=('nflexnode', 3),
+             ),
+             StructFieldDecl(
+                 name='flex_node0',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='Cartesian node positions in qpos0',
+                 array_extent=('nflexnode', 3),
+             ),
              StructFieldDecl(
                  name='flexedge_length0',
                  type=PointerType(
@@ -7447,6 +7500,34 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                              ),
                              doc='',
                          ),
+                         StructFieldDecl(
+                             name='flex_interp',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
+                         StructFieldDecl(
+                             name='flex_nodeadr',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
+                         StructFieldDecl(
+                             name='flex_nodenum',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
+                         StructFieldDecl(
+                             name='flex_nodebodyid',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
                          StructFieldDecl(
                              name='flex_vertadr',
                              type=PointerType(
@@ -7538,6 +7619,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                              ),
                              doc='',
                          ),
+                         StructFieldDecl(
+                             name='flex_centered',
+                             type=PointerType(
+                                 inner_type=ValueType(name='mjtByte'),
+                             ),
+                             doc='',
+                         ),
+                         StructFieldDecl(
+                             name='flex_node',
+                             type=PointerType(
+                                 inner_type=ValueType(name='mjtNum'),
+                             ),
+                             doc='',
+                         ),
                          StructFieldDecl(
                              name='flex_radius',
                              type=PointerType(
@@ -10209,6 +10304,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='double'),
                  doc='thickness (2D only)',
              ),
+             StructFieldDecl(
+                 name='nodebody',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjStringVec'),
+                 ),
+                 doc='node body names',
+             ),
              StructFieldDecl(
                  name='vertbody',
                  type=PointerType(
@@ -10216,6 +10318,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='vertex body names',
              ),
+             StructFieldDecl(
+                 name='node',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjDoubleVec'),
+                 ),
+                 doc='node positions',
+             ),
              StructFieldDecl(
                  name='vert',
                  type=PointerType(

model/composite/scene.xml

@@ -30,12 +30,10 @@
     <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
       width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
     <texture name="texcarpet" type="2d" file="carpet.png"/>
-    <texture name="texsponge" type="2d" file="sponge.png"/>
     <texture name="texmarble" type="cube" file="marble.png"/>
 
     <material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
     <material name="matcarpet" texture="texcarpet"/>
-    <material name="matsponge" texture="texsponge" specular="0.3"/>
     <material name="matmarble" texture="texmarble" rgba=".7 .7 .7 1"/>
   </asset>
 

model/flex/bunny.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=".001" 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 .1" dim="2" euler="90 0 0"
+              radius=".001" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="trilinear">
+      <elasticity young="1e3" poisson="0.1" damping="0.001"/>
+      <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/gripper.xml

@@ -27,7 +27,7 @@
         <geom type="box" size=".02 .1 .15" pos=".18 0 -.2" rgba=".2 .2 .2 1"/>
         <flexcomp type="mesh" file="cap.obj" pos=".16 0 -.25" dim="3" euler="0 -90 0"
                   origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="left">
-          <edge equality="true"/>
+          <edge equality="true" solimp="0.95 0.99 0.001 0.5 2"/>
           <contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
           <pin id="0 2 3 4 5 6 7 9 11 13 15 17 19 41 45 46 50 53 57 58 61 176 177 179 180 184 185
                    187 188 192 193 195 196 200 201 203 204 208 209 211 212 214 215 216 217"/>
@@ -38,13 +38,18 @@
         <geom type="box" size=".02 .1 .15" pos="-.18 0 -.2" rgba=".2 .2 .2 1"/>
         <flexcomp type="mesh" file="cap.obj" pos="-.16 0 -.25" dim="3" euler="0 90 0"
                   origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="right">
-          <edge equality="true"/>
+          <edge equality="true" solimp="0.95 0.99 0.001 0.5 2"/>
           <contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
           <pin id="0 2 3 4 5 6 7 9 11 13 15 17 19 41 45 46 50 53 57 58 61 176 177 179 180 184 185
                    187 188 192 193 195 196 200 201 203 204 208 209 211 212 214 215 216 217"/>
         </flexcomp>
       </body>
     </body>
+    <body>
+      <freejoint/>
+      <geom type="box" size=".05 .1 .1" pos="0 0 .1" rgba=".5 .5 0 1" priority="1"
+            contype="2" condim="6"/>
+    </body>
   </worldbody>
 
   <equality>

model/flex/gripper_trilinear.xml

@@ -0,0 +1,71 @@
+<!-- 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="Gripper">
+  <include file="scene.xml"/>
+
+  <option cone="elliptic" impratio="10" integrator="implicitfast"/>
+
+  <worldbody>
+    <body name="hand" pos="0 0 .37">
+      <joint name="lift" type="slide" range="0 1"/>
+      <geom type="box" size=".2 .1 .05" rgba=".2 .2 .2 1"/>
+      <body name="right_gripper">
+        <joint name="right" type="slide" axis="-1 0 0"/>
+        <geom type="box" size=".02 .1 .15" pos=".18 0 -.2" rgba=".2 .2 .2 1"/>
+        <flexcomp type="mesh" file="cap.obj" pos=".16 0 -.25" dim="3" euler="0 -90 0"
+                  origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="left"
+                  dof="trilinear">
+          <elasticity young="1e4" poisson="0.1" damping=".05"/>
+          <contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
+          <pin id="4 5 6 7"/>
+        </flexcomp>
+      </body>
+      <body name="left_gripper">
+        <joint name="left" type="slide" axis="1 0 0"/>
+        <geom type="box" size=".02 .1 .15" pos="-.18 0 -.2" rgba=".2 .2 .2 1"/>
+        <flexcomp type="mesh" file="cap.obj" pos="-.16 0 -.25" dim="3" euler="0 90 0"
+                  origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="right"
+                  dof="trilinear">
+          <elasticity young="1e4" poisson="0.1" damping=".05"/>
+          <contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
+          <pin id="0 1 2 3"/>
+        </flexcomp>
+      </body>
+    </body>
+    <body>
+      <freejoint/>
+      <geom type="box" size=".05 .1 .1" pos="0 0 .1" rgba=".5 .5 0 1" priority="1"
+            contype="2" condim="6"/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <joint joint1="right" joint2="left"/>
+  </equality>
+
+  <tendon>
+    <fixed name="grasp">
+      <joint joint="right" coef="1"/>
+      <joint joint="left" coef="1"/>
+    </fixed>
+  </tendon>
+
+  <actuator>
+    <position name="lift" joint="lift" kp="600" dampratio="1" ctrlrange="0 1"/>
+    <position name="grasp" tendon="grasp" kp="200" dampratio="1" ctrlrange="0 1"/>
+  </actuator>
+
+</mujoco>

model/flex/softbox.xml

@@ -20,12 +20,17 @@
 
   <size memory="30M"/>
 
+  <asset>
+    <texture name="texsponge" type="2d" file="sponge.png"/>
+    <material name="matsponge" texture="texsponge" specular="0.3"/>
+  </asset>
+
   <worldbody>
     <body pos="0 0 1" name="body">
       <freejoint/>
       <geom size=".1" contype="0" conaffinity="0" group="4"/>
       <flexcomp name="softbox" type="box" count="7 7 7" spacing=".04 .04 .04"
-                radius="0.01" dim="3" material="matsponge">
+                radius="0.01" dim="3" material="matsponge" dof="radial">
         <contact internal="false" selfcollide="none"/>
         <edge equality="true"/>
       </flexcomp>

model/flex/sphere_full.xml

@@ -0,0 +1,36 @@
+<!-- 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="Full-flex sphere">
+  <include file="scene.xml"/>
+
+  <option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
+
+  <size memory="10M"/>
+
+  <visual>
+    <map stiffness="500"/>
+  </visual>
+
+  <worldbody>
+    <geom type="box" pos="1.5 0 0.25" size=".5 2 .25"/>
+    <geom type="box" pos="0 0 0.25" size="2 2 .05" euler="0 15 0"/>
+    <flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" pos="-.5 0 1" dim="3"
+              radius=".001" rgba="0 .7 .7 1" mass="5" name="slow">
+      <edge equality="true"/>
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+  </worldbody>
+</mujoco>

model/flex/sphere_radial.xml

@@ -0,0 +1,41 @@
+<!-- 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="Radial flex sphere">
+  <include file="scene.xml"/>
+
+  <option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
+
+  <size memory="10M"/>
+
+  <visual>
+    <map stiffness="500"/>
+  </visual>
+
+  <worldbody>
+    <geom type="box" pos="1.5 0 0.25" size=".5 2 .25"/>
+    <geom type="box" pos="0 0 0.25" size="2 2 .05" euler="0 15 0"/>
+    <body name="body" pos="-.5 0 1">
+      <freejoint/>
+      <geom size=".1" contype="0" conaffinity="0" group="4"/>
+      <flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" dim="3"
+                radius=".001" rgba="0 .7 .7 1" mass="5" name="radial" dof="radial">
+        <edge equality="true" solimp="0 0.9 0.01" solref=".02 1"/>
+        <!-- <elasticity young="6e2" poisson="0.3" damping="0.05"/> -->
+        <contact selfcollide="none" internal="false"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+</mujoco>

model/flex/sphere_trilinear.xml

@@ -0,0 +1,45 @@
+<!-- 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 flex spheres">
+  <include file="scene.xml"/>
+
+  <option solver="CG" tolerance="1e-6" timestep="1e-3" integrator="implicitfast"/>
+
+  <size memory="10M"/>
+
+  <visual>
+    <map stiffness="100"/>
+    <!-- The trilinear bounding box is visualized by turning on "Flex Tree" (mjVIS_FLEXBVH).
+         In order to make the visualization clearer,
+         we make the regular collision BVH visualization invisible: -->
+    <rgba bv="0 0 0 0" bvactive="0 0 0 0"/>
+  </visual>
+
+  <worldbody>
+    <geom type="box" pos="1.5 0 0.25" size=".5 2 .25"/>
+    <geom type="box" pos="0 0 0.25" size="2 2 .05" euler="0 15 0"/>
+    <flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" pos="-.5 -.5 1" dim="3"
+              radius=".001" rgba="0 .7 .7 1" mass="5" name="soft" dof="trilinear">
+      <elasticity young="1e3" poisson="0.49" damping="0.1"/>
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+    <flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" pos="-.5 .5 1" dim="3"
+              radius=".001" rgba="0 .7 .7 1" mass="5" name="rubber" dof="trilinear">
+      <elasticity young="1e4" poisson="0.4" damping="3e-3"/>
+      <contact selfcollide="none" internal="false" solref="-5000 -10"/>
+    </flexcomp>
+  </worldbody>
+</mujoco>

model/flex/trilinear.xml

@@ -0,0 +1,38 @@
+<!-- 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=".001" integrator="implicitfast"/>
+
+  <size memory="10M"/>
+
+  <visual>
+    <map stiffness="100"/>
+    <rgba bv="0 0 0 0" bvactive="0 0 0 0"/>
+  </visual>
+
+  <worldbody>
+    <body mocap="true" pos="-.1 .05 0" zaxis=".5 0 1">
+      <geom type="capsule" size=".1 .1" group="1" condim="1"/>
+    </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="trilinear">
+      <elasticity young="1e4" poisson="0.1" damping="0.01"/>
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+  </worldbody>
+</mujoco>

src/engine/engine_collision_driver.c

@@ -1859,7 +1859,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
   const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
   const int* bdata = m->flex_vertbodyid + m->flex_vertadr[f];
   for (int i=0; i <= dim; i++) {
-    if (b == bdata[edata[i]]) {
+    if (b >= 0 && b == bdata[edata[i]]) {
       return;
     }
   }
@@ -1983,7 +1983,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
   for (int i1=0; i1 <= dim1; i1++) {
     int b1 = bdata1[edata1[i1]];
     for (int i2=0; i2 <= dim2; i2++) {
-      if (b1 == bdata2[edata2[i2]]) {
+      if (b1 >= 0 && b1 == bdata2[edata2[i2]]) {
         return;
       }
     }

src/engine/engine_core_constraint.c

@@ -181,7 +181,7 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
   for (int i=0; i <= dim; i++) {
     mjtNum dist = mju_dist3(point, vert+3*edata[i]);
     weight[i] = 1.0/(mju_max(mjMINVAL, dist));
-    body[i] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[i]];
+    body[i] = m->flex_vertadr[f] + edata[i];
 
     // check if element vertex matches v
     if (edata[i] == v) {
@@ -206,6 +206,30 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
 
 
 
+// compute body weights for a given contact vertex, return #bodies
+static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v,
+                             const mjtNum point[3], int* body, mjtNum* weight, mjtNum bw) {
+  mjtNum* coord = m->flex_vert0 + 3*v;
+  int nstart = m->flex_nodeadr[f];
+  int nend = m->flex_nodeadr[f] + m->flex_nodenum[f];
+  int nb = 0;
+
+  for (int i = nstart; i < nend; i++) {
+    mjtNum w = ((i-nstart)&1 ? coord[2] : 1-coord[2]) *
+               ((i-nstart)&2 ? coord[1] : 1-coord[1]) *
+               ((i-nstart)&4 ? coord[0] : 1-coord[0]);
+    if (w < 1e-5) {
+      continue;
+    }
+    if (weight) weight[nb] = w * bw;
+    body[nb++] = m->flex_nodebodyid[i];
+  }
+
+  return nb;
+}
+
+
+
 // add contact to d->contact list; return 0 if success; 1 if buffer full
 int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
   // if nconmax is specified and ncon >= nconmax, warn and return error
@@ -971,8 +995,8 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
       con->efc_address = d->nefc;
 
       // special case: single body on each side
-      if ((con->geom[0] >= 0 || con->vert[0] >= 0) &&
+      if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
-          (con->geom[1] >= 0 || con->vert[1] >= 0)) {
+          (con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
         // get bodies
         int bid[2];
         for (int side=0; side < 2; side++) {
@@ -995,9 +1019,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
       else {
         // get bodies and weights
         int nb = 0;
-        int bid[8];
+        int bid[64];
-        mjtNum bweight[8];
+        mjtNum bweight[64];
         for (int side=0; side < 2; side++) {
+          int nw = 0;
+          int vid[4];
+          mjtNum bw[4];
+
           // geom
           if (con->geom[side] >= 0) {
             bid[nb] = m->geom_bodyid[con->geom[side]];
@@ -1007,22 +1035,32 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
 
           // flex vert
           else if (con->vert[side] >= 0) {
-            bid[nb] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
+            vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
-            bweight[nb] = side ? +1 : -1;
+            bw[0] = side ? +1 : -1;
-            nb++;
+            nw = 1;
           }
 
           // flex elem
           else {
-            int nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
+            nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
-                                       con->vert[1-side], con->pos, bid+nb, bweight+nb);
+                                   con->vert[1-side], con->pos, vid, bw);
 
             // negative sign for first side of contact
             if (side == 0) {
-              mju_scl(bweight+nb, bweight+nb, -1, nw);
+              mju_scl(bw, bw, -1, nw);
             }
+          }
 
-            nb += nw;
+          // get body or node ids and weights
+          for (int k=0; k < nw; k++) {
+            if (m->flex_interp[con->flex[side]] == 0) {
+              bid[nb] = m->flex_vertbodyid[vid[k]];
+              bweight[nb] = bw[k];
+              nb++;
+            } else {
+              nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
+                                      con->pos, bid+nb, bweight+nb, bw[k]);
+            }
           }
         }
 
@@ -1201,8 +1239,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, bid[4];
+        int nb = 0, bid[32], vid[4], nw = 0;
-        mjtNum bweight[4];
+        mjtNum bweight[32], bw[4];
 
         // geom
         if (con->geom[side] >= 0) {
@@ -1213,15 +1251,27 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
 
         // flex vert
         else if (con->vert[side] >= 0) {
-          bid[0] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
+          vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
-          bweight[0] = 1;
+          bw[0] = 1;
-          nb = 1;
+          nw = 1;
         }
 
         // flex elem
         else {
-          nb = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
+          nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
-                                 con->vert[1-side], con->pos, bid, bweight);
+                                 con->vert[1-side], con->pos, vid, bw);
+        }
+
+        // get body or node ids and weights
+        for (int k=0; k < nw; k++) {
+          if (m->flex_interp[con->flex[side]] == 0) {
+            bid[k] = m->flex_vertbodyid[vid[k]];
+            bweight[k] = bw[k];
+            nb++;
+          } else {
+            nb = mj_vertBodyWeight(m, d, con->flex[side], vid[k],
+                                   con->pos, bid, bweight, bw[k]);
+          }
         }
 
         // add weighted average over bodies
@@ -1881,8 +1931,11 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
     int NV = 0;
     if (nnz) {
       // get bodies
-      int nb = 0, bid[8];
+      int nb = 0, bid[64];
       for (int side=0; side < 2; side++) {
+        int nw = 0;
+        int vid[4];
+
         // geom
         if (con->geom[side] >= 0) {
           bid[nb++] = m->geom_bodyid[con->geom[side]];
@@ -1890,7 +1943,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
 
         // flex vert
         else if (con->vert[side] >= 0) {
-          bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
+          vid[nw++] = m->flex_vertadr[con->flex[side]] + con->vert[side];
         }
 
         // flex elem
@@ -1899,7 +1952,18 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
           int fdim = m->flex_dim[f];
           const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1);
           for (int k=0; k <= fdim; k++) {
-            bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[k]];
+            vid[nw++] = m->flex_vertadr[f] + edata[k];
+          }
+        }
+
+        // get body or node ids and weights
+        for (int k=0; k < nw; k++) {
+          if (m->flex_interp[con->flex[side]] == 0) {
+            bid[nb] = m->flex_vertbodyid[vid[k]];
+            nb++;
+          } else {
+            nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
+                                    con->pos, bid+nb, NULL, 0);
           }
         }
       }

src/engine/engine_core_smooth.c

@@ -453,19 +453,51 @@ void mj_flex(const mjModel* m, mjData* d) {
   for (int f=0; f < m->nflex; f++) {
     int vstart = m->flex_vertadr[f];
     int vend = m->flex_vertadr[f] + m->flex_vertnum[f];
+    int nstart = m->flex_nodeadr[f];
+    int nend = m->flex_nodeadr[f] + m->flex_nodenum[f];
 
-    // centered: copy body position
+    // 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs
-    if (m->flex_centered[f]) {
+    if (m->flex_interp[f] == 0) {
-      for (int i=vstart; i < vend; i++) {
+      // centered: copy body position
-        mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
+      if (m->flex_centered[f]) {
+        for (int i=vstart; i < vend; i++) {
+          mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
+        }
+      }
+
+      // non-centered: map from local to global
+      else {
+        for (int i=vstart; i < vend; i++) {
+          mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
+          mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
+        }
       }
     }
 
-    // non-centered: map from local to global
+    // trilinear interpolation
     else {
+      mjtNum nodexpos[mjMAXFLEXNODES];
+      if (m->flex_centered[f]) {
+        for (int i=nstart; i < nend; i++) {
+          mju_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]);
+        }
+      } else {
+        for (int i=nstart; i < nend; i++) {
+          int j = i - nstart;
+          mju_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i);
+          mju_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]);
+        }
+      }
+
       for (int i=vstart; i < vend; i++) {
-        mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
+        mju_zero3(d->flexvert_xpos+3*i);
-        mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
+        mjtNum* coord = m->flex_vert0 + 3*i;
+        for (int j=0; j < nend-nstart; j++) {
+          mjtNum coef = (j&1 ? coord[2] : 1-coord[2]) *
+                        (j&2 ? coord[1] : 1-coord[1]) *
+                        (j&4 ? coord[0] : 1-coord[0]);
+          mju_addToScl3(d->flexvert_xpos+3*i, nodexpos+3*j, coef);
+        }
       }
     }
   }
@@ -542,7 +574,7 @@ void mj_flex(const mjModel* m, mjData* d) {
   // compute lengths and Jacobians of edges
   for (int f=0; f < m->nflex; f++) {
     // skip if edges cannot generate forces
-    if (m->flex_rigid[f]) {
+    if (m->flex_rigid[f] || m->flex_interp[f]) {
       continue;
     }
 

src/engine/engine_io.c

@@ -460,7 +460,7 @@ static void freeModelBuffers(mjModel* m) {
 void mj_makeModel(mjModel** dest,
     int nq, int nv, int nu, int na, int nbody, int nbvh,
     int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam,
-    int nlight, int nflex, int nflexvert, int nflexedge, int nflexelem,
+    int nlight, int nflex, int nflexnode, int nflexvert, int nflexedge, int nflexelem,
     int nflexelemdata, int nflexelemedge, int nflexshelldata, int nflexevpair, int nflextexcoord,
     int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
     int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
@@ -503,6 +503,7 @@ void mj_makeModel(mjModel** dest,
   m->ncam = ncam;
   m->nlight = nlight;
   m->nflex = nflex;
+  m->nflexnode = nflexnode;
   m->nflexvert = nflexvert;
   m->nflexedge = nflexedge;
   m->nflexelem = nflexelem;
@@ -634,7 +635,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
     mj_makeModel(&dest,
       src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
       src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
-      src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
+      src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge,
       src->nflexelem, src->nflexelemdata, src->nflexelemedge, src->nflexshelldata,
       src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
       src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
@@ -828,7 +829,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
                ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
                ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
                ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
-               ints[63], ints[64]);
+               ints[63], ints[64], ints[65]);
   if (!m || m->nbuffer != sizes[getnsize()-1]) {
     mju_warning("Corrupted model, wrong size parameters");
     mj_deleteModel(m);

src/engine/engine_io.h

@@ -53,7 +53,7 @@ void mj_defaultStatistic(mjStatistic* stat);
 // allocate mjModel
 void mj_makeModel(mjModel** dest,
     int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
-    int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
+    int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexnode, int nflexvert,
     int nflexedge, int nflexelem, int nflexelemdata, int nflexelemedge, int nflexshelldata,
     int nflexevpair, int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal,
     int nmeshtexcoord, int nmeshface, int nmeshgraph, int nskin, int nskinvert, int nskintexvert,

src/engine/engine_passive.c

@@ -122,6 +122,88 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
       continue;
     }
 
+    if (m->flex_interp[f]) {
+      mjtNum xpos[mjMAXFLEXNODES], displ[mjMAXFLEXNODES], vel[mjMAXFLEXNODES];
+      mjtNum frc[mjMAXFLEXNODES], dmp[mjMAXFLEXNODES];
+      mjtNum com[3] = {0};
+      mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
+      int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
+      int nstart = m->flex_nodeadr[f];
+
+      // compute positions
+      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]);
+          mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]);
+        }
+      } else {
+        mjtNum screw[6];
+        for (int i=0; i < m->flex_nodenum[f]; i++) {
+          mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
+          mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
+          mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0);
+          mju_copy3(vel + 3*i, screw + 3);
+        }
+      }
+
+      // compute center of mass
+      for (int i = 0; i < m->flex_nodenum[f]; i++) {
+        mju_addToScl3(com, xpos+3*i, 1.0/m->flex_nodenum[f]);
+      }
+
+      // re-center positions using center of mass
+      for (int i = 0; i < m->flex_nodenum[f]; i++) {
+        mju_addToScl3(xpos+3*i, com, -1);
+      }
+
+      // compute the Jacobian at the center of mass
+      mjtNum mat[9] = {0};
+      mjtNum p[3] = {.5, .5, .5};
+      mju_defGradient(mat, p, xpos, 1);
+
+      // find rotation
+      mjtNum quat[4] = {1, 0, 0, 0};
+      mju_mat2Rot(quat, mat);
+      mju_negQuat(quat, quat);
+
+      // rotate vertices to quat and add reference center of mass
+      for (int i = 0; i < m->flex_nodenum[f]; i++) {
+        mju_rotVecQuat(xpos+3*i, xpos+3*i, quat);
+        mju_addTo3(xpos+3*i, p);
+        mju_rotVecQuat(vel+3*i, vel+3*i, quat);
+      }
+
+      // compute displacement
+      for (int i = 0; i < m->flex_nodenum[f]; i++) {
+        mju_addScl3(displ+3*i, xpos+3*i, xpos0+3*i, -1);
+      }
+
+      // compute force in the stretch frame
+      mju_mulMatVec(frc, k, displ, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
+
+      // compute damping force in stretch frame
+      mju_mulMatVec(dmp, k, vel, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
+
+      // rotate forces to global frame and add to qfrc
+      mju_negQuat(quat, quat);
+      for (int i = 0; i < m->flex_nodenum[f]; i++) {
+        mjtNum qfrc[3], qdmp[3];
+        mju_rotVecQuat(qfrc, frc+3*i, quat);
+        mju_rotVecQuat(qdmp, dmp+3*i, quat);
+        mju_scl3(qdmp, qdmp, m->flex_damping[f]);
+        if (m->flex_centered[f]) {
+          mju_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc);
+          mju_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
+        } else {
+          mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring);
+          mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper);
+        }
+      }
+
+      // do not continue with the rest of the flex passive forces
+      continue;
+    }
+
     int nedge = (dim == 2) ? 3 : 6;
     int nvert = (dim == 2) ? 3 : 4;
     const int* elem = m->flex_elem + m->flex_elemdataadr[f];

src/engine/engine_setconst.c

@@ -213,6 +213,10 @@ static void set0(mjModel* m, mjData* d) {
   if (nv) {
     // compute flexedge_invweight0
     for (int f=0; f < m->nflex; f++) {
+      if (m->flex_interp[f]) {
+        continue;
+      }
+
       for (int i=m->flex_edgeadr[f]; i < m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) {
         // bodies connected by edge
         int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]];
@@ -498,13 +502,22 @@ static void setStat(mjModel* m, mjData* d) {
 
   // adjust body size for flex edges involving body
   for (int f=0; f < m->nflex; f++) {
+    mjtNum meanedge = 0;
     for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
+      meanedge += m->flexedge_length0[e] / m->flex_edgenum[f];
+      if (m->flex_interp[f]) {
+        continue;
+      }
+
       int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]];
       int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]];
 
       body[b1] = mju_max(body[b1], m->flexedge_length0[e]);
       body[b2] = mju_max(body[b2], m->flexedge_length0[e]);
     }
+    for (int v=m->flex_nodeadr[f]; v < m->flex_nodeadr[f]+m->flex_node[f]; v++) {
+      body[m->flex_nodebodyid[v]] = mju_max(body[m->flex_nodebodyid[v]], meanedge);
+    }
   }
 
   // compute meansize, make sure all sizes are above min

src/engine/engine_util_misc.c

@@ -452,6 +452,51 @@ void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]) {
 
 
 
+// ----------------------------- Flex interpolation ------------------------------------------------
+
+mjtNum static inline phi(mjtNum s, int i) {
+  if (i == 0) {
+    return 1-s;
+  } else {
+    return s;
+  }
+}
+
+mjtNum static inline dphi(mjtNum s, int i) {
+  if (i == 0) {
+    return -1;
+  } else {
+    return 1;
+  }
+}
+
+// evaluate the deformation gradient at p using the nodal dof values
+void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order) {
+  mjtNum gradient[3];
+  mju_zero(res, 9);
+  for (int i = 0; i <= order; i++) {
+    for (int j = 0; j <= order; j++) {
+      for (int k = 0; k <= order; k++) {
+        int idx = 4*i + 2*j + k;
+        gradient[0] = dphi(p[0], i) *  phi(p[1], j) *  phi(p[2], k);
+        gradient[1] =  phi(p[0], i) * dphi(p[1], j) *  phi(p[2], k);
+        gradient[2] =  phi(p[0], i) *  phi(p[1], j) * dphi(p[2], k);
+        res[0] += dof[3*idx+0] * gradient[0];
+        res[1] += dof[3*idx+0] * gradient[1];
+        res[2] += dof[3*idx+0] * gradient[2];
+        res[3] += dof[3*idx+1] * gradient[0];
+        res[4] += dof[3*idx+1] * gradient[1];
+        res[5] += dof[3*idx+1] * gradient[2];
+        res[6] += dof[3*idx+2] * gradient[0];
+        res[7] += dof[3*idx+2] * gradient[1];
+        res[8] += dof[3*idx+2] * gradient[2];
+      }
+    }
+  }
+}
+
+
+
 //------------------------------ actuator models ---------------------------------------------------
 
 // normalized muscle length-gain curve

src/engine/engine_util_misc.h

@@ -53,6 +53,11 @@ MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]);
 // all 3 semi-axes of a geom
 MJAPI void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]);
 
+// ----------------------------- Flex interpolation ------------------------------------------------
+
+// evaluate the deformation gradient at p using the nodal dof values
+MJAPI void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order);
+
 // ----------------------------- Base64 -----------------------------------------------------------
 
 // encode data as Base64 into buf (including padding and null char)

src/engine/engine_vis_interact.c

@@ -828,9 +828,24 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
       // update if closer intersection found
       if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) {
         flexdist = newdist;
-        flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
+        if (m->flex_interp[i]) {
+          mjtNum* vert0 = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid);
+          int l = vert0[0] > 0.5 ? 1 : 0;
+          int j = vert0[1] > 0.5 ? 1 : 0;
+          int k = vert0[2] > 0.5 ? 1 : 0;
+          int nodeid = 4*l+2*j+k;
+          flexbodyid = m->flex_nodebodyid[m->flex_nodeadr[i] + nodeid];
+          if (m->flex_centered[i]) {
+            mju_copy3(flexpnt, d->xpos + 3*flexbodyid);
+          } else {
+            mju_mulMatVec3(flexpnt, d->xmat + 9*flexbodyid, m->flex_node + 3*nodeid);
+            mju_addTo3(flexpnt, d->xpos + 3*flexbodyid);
+          }
+        } else {
+          flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
+          mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
+        }
         *flexid = i;
-        mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
       }
     }
   }

src/engine/engine_vis_visualize.c

@@ -713,6 +713,47 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
           FINISH
         }
       }
+
+      if (!m->flex_interp[f]) {
+        continue;
+      }
+
+      // control points box
+      mjtNum xpos[mjMAXFLEXNODES];
+      int nstart = m->flex_nodeadr[f];
+      int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
+      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]);
+        }
+      } else {
+        for (int i=0; i < m->flex_nodenum[f]; i++) {
+          mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
+          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 (scn->ngeom >= scn->maxgeom) break;
+            if (i == 0) {
+              START
+              mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k));
+              FINISH
+            }
+            if (j == 0) {
+              START
+              mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k));
+              FINISH
+            }
+            if (k == 0) {
+              START
+              mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1)));
+              FINISH
+            }
+          }
+        }
+      }
     }
   }
 

src/user/user_flexcomp.cc

@@ -86,6 +86,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
   mjuu_setvec(quat, 1, 0, 0, 0);
   rigid = false;
   centered = false;
+  doftype = mjFCOMPDOF_FULL;
 
   mjs_defaultPlugin(&plugin);
   mjs_defaultOrientation(&alt);
@@ -102,10 +103,6 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
   mjCModel* model = static_cast<mjCBody*>(body->element)->model;
   mjsCompiler* compiler = static_cast<mjCBody*>(body->element)->compiler;
   mjsFlex* dflex = def.spec.flex;
-
-  bool radial = (type == mjFCOMPTYPE_BOX ||
-                 type == mjFCOMPTYPE_CYLINDER ||
-                 type == mjFCOMPTYPE_ELLIPSOID);
   bool direct = (type == mjFCOMPTYPE_DIRECT ||
                  type == mjFCOMPTYPE_MESH ||
                  type == mjFCOMPTYPE_GMSH);
@@ -122,7 +119,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
 
   // check counts
   for (int i=0; i < 3; i++) {
-    if (count[i] < 1 || ((radial && count[i] < 2) && dflex->dim == 3)) {
+    if (count[i] < 1 || ((doftype == mjFCOMPDOF_RADIAL && count[i] < 2) && dflex->dim == 3)) {
       return comperr(error, "Count too small", error_sz);
     }
   }
@@ -260,6 +257,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
     point[3*i+2] = newp[2];
   }
 
+  // compute bounding box of points
+  double minmax[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
+  for (int i=0; i < npnt; i++) {
+    for (int j=0; j < 3; j++) {
+      minmax[j+0] = std::min(minmax[j+0], point[3*i+j]);
+      minmax[j+3] = std::max(minmax[j+3], point[3*i+j]);
+    }
+  }
+
   // construct pinned array
   pinned = vector<bool>(npnt, rigid);
 
@@ -337,7 +343,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
     }
 
     // center of radial body is always pinned
-    if (radial) {
+    if (doftype == mjFCOMPDOF_RADIAL) {
       pinned[0] = true;
     }
 
@@ -434,8 +440,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
       continue;
     }
 
-    // pinned: parent body
+    // pinned or trilinear: parent body
-    if (pinned[i]) {
+    if (pinned[i] || doftype == mjFCOMPDOF_TRILINEAR) {
       mjs_appendString(pf->vertbody, mjs_getString(body->name));
 
       // add plugin
@@ -448,7 +454,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
       }
     }
 
-    // not pinned: new body
+    // not pinned and not trilinear: new body
     else {
       // add new body at vertex coordinates
       mjsBody* pb = mjs_addBody(body, 0);
@@ -465,7 +471,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
       pb->explicitinertial = true;
 
       // add radial slider
-      if (radial) {
+      if (doftype == mjFCOMPDOF_RADIAL) {
         mjsJoint* jnt = mjs_addJoint(pb, 0);
 
         // set properties
@@ -476,7 +482,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
       }
 
       // add three orthogonal sliders
-      else {
+      else if (doftype == mjFCOMPDOF_FULL) {
         for (int j=0; j < 3; j++) {
           // add joint to body
           mjsJoint* jnt = mjs_addJoint(pb, 0);
@@ -513,7 +519,54 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
     }
   }
 
-  if (!centered) {
+  // create nodal mesh for trilinear interpolation
+  if (doftype == mjFCOMPDOF_TRILINEAR) {
+    std::vector<double> node(24, 0);
+    for (int i=0; i < 2; i++) {
+      for (int j=0; j < 2; j++) {
+        for (int k=0; k < 2; k++) {
+          if (pinned[i*4+j*2+k]) {
+            node[3*(i*4+j*2+k)+0] = i == 0 ? minmax[0] : minmax[3];
+            node[3*(i*4+j*2+k)+1] = j == 0 ? minmax[1] : minmax[4];
+            node[3*(i*4+j*2+k)+2] = k == 0 ? minmax[2] : minmax[5];
+            mjs_appendString(pf->nodebody, mjs_getString(body->name));
+            continue;
+          }
+
+          mjsBody* pb = mjs_addBody(body, 0);
+          pb->pos[0] = i == 0 ? minmax[0] : minmax[3];
+          pb->pos[1] = j == 0 ? minmax[1] : minmax[4];
+          pb->pos[2] = k == 0 ? minmax[2] : minmax[5];
+          mjuu_zerovec(pb->ipos, 3);
+          pb->mass = mass / 8;
+          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;
+          pb->explicitinertial = true;
+
+          for (int d=0; d < 3; d++) {
+            mjsJoint* jnt = mjs_addJoint(pb, 0);
+            jnt->type = mjJNT_SLIDE;
+            mjuu_setvec(jnt->pos, 0, 0, 0);
+            mjuu_setvec(jnt->axis, 0, 0, 0);
+            jnt->axis[d] = 1;
+          }
+
+          // construct node name, add to nodebody
+          char txt[100];
+          mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), i, j, k);
+          mjs_setString(pb->name, txt);
+          mjs_appendString(pf->nodebody, mjs_getString(pb->name));
+        }
+      }
+    }
+
+    if (!centered) {
+      mjs_setDouble(pf->node, node.data(), node.size());
+    }
+  }
+
+  if (!centered || doftype == mjFCOMPDOF_TRILINEAR) {
     mjs_setDouble(pf->vert, point.data(), point.size());
   }
 

src/user/user_flexcomp.h

@@ -40,6 +40,15 @@ typedef enum _mjtFcompType {
 } mjtFcompType;
 
 
+typedef enum _mjtDof {
+  mjFCOMPDOF_FULL = 0,
+  mjFCOMPDOF_RADIAL,
+  mjFCOMPDOF_TRILINEAR,
+
+  mjNFCOMPDOFS
+} mjtDof;
+
+
 class mjCFlexcomp {
  public:
   mjCFlexcomp(void);
@@ -73,6 +82,7 @@ class mjCFlexcomp {
   double inertiabox;              // size of inertia box for each body
   bool equality;                  // create edge equality constraint
   std::string file;               // mesh/gmsh file name
+  mjtDof doftype;                 // dof type, all vertices or trilinear interpolation
 
   // pin specifications
   std::vector<int> pinid;         // ids of points to pin

src/user/user_mesh.cc

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <algorithm>
+#include <array>
 #include <climits>
 #include <cmath>
 #include <csetjmp>
@@ -2402,7 +2403,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
 
 
 
-//--------------------- elasticity implementation --------------------------------------------------
+//-------------------------- nonlinear elasticity --------------------------------------------------
 
 // hash function for std::pair
 struct PairHash
@@ -2628,6 +2629,153 @@ void inline ComputeStiffness(std::vector<double>& stiffness,
   MetricTensor<T>(stiffness.data(), t, mu, la, basis);
 }
 
+//----------------------------- linear elasticity --------------------------------------------------
+
+// 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.");
+
+  // 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;
+  } else {
+    return s;
+  }
+}
+
+// evaluate gradient fo 1-dimensional basis function
+double dphi(const double s, const double component) {
+  if (component == 0) {
+    return -1;
+  } else {
+    return 1;
+  }
+}
+
+typedef std::array<std::array<double, 3>, 3> Matrix;
+
+// symmetrize a tensor
+Matrix inline sym(const Matrix& tensor) {
+  Matrix eps;
+  for (int i = 0; i < 3; i++) {
+    for (int j = 0; j < 3; j++) {
+      eps[i][j] = (tensor[i][j] + tensor[j][i]) / 2;
+    }
+  }
+  return eps;
+}
+
+// compute tensor inner product
+Matrix inline inner(const Matrix& tensor1, const Matrix& tensor2) {
+  Matrix inner;
+  for (int i = 0; i < 3; i++) {
+    for (int j = 0; j < 3; j++) {
+      inner[i][j] = tensor1[i][0] * tensor2[0][j] +
+                    tensor1[i][1] * tensor2[1][j] +
+                    tensor1[i][2] * tensor2[2][j];
+    }
+  }
+  return inner;
+}
+
+// compute trace of a tensor
+double inline trace(const Matrix& tensor) {
+  return tensor[0][0] + tensor[1][1] + tensor[2][2];
+}
+
+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 = std::pow(order, 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);
+
+  // 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 detJ = dx * dy * dz;
+  double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
+
+  // compute stiffness matrix
+  std::vector<std::array<double, 3>> F(n);
+  double la = E * nu / (1 + nu) / (1 - 2 * nu);
+  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++) {
+        double s = points[ps];
+        double t = points[pt];
+        double u = points[pu];
+        double dvol = weight[ps] * weight[pt] * weight[pu] * detJ;
+        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++) {
+              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);
+              F[dof++] = gradient;
+            }
+          }
+        }
+
+        if (dof != n) {  // SHOULD NOT OCCUR
+          throw mjCError(NULL, "incorrect number of basis functions");
+        }
+
+        // tensor contraction of the gradients of elastic strains
+        // (d(F+F')/dx : d(F+F')/dx)
+        for (int i=0; i < n; i++) {
+          for (int j=0; j < n; j++) {
+            Matrix du;
+            Matrix dv;
+            du.fill({0, 0, 0});
+            dv.fill({0, 0, 0});
+            for (int k=0; k < 3; k++) {
+              for (int l=0; l < 3; l++) {
+                du[k][0] = invJ[0] * F[i][0];
+                du[k][1] = invJ[1] * F[i][1];
+                du[k][2] = invJ[2] * F[i][2];
+                dv[l][0] = invJ[0] * F[j][0];
+                dv[l][1] = invJ[1] * F[j][1];
+                dv[l][2] = invJ[2] * F[j][2];
+                K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol;
+                K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol;
+                mjuu_zerovec(du[k].data(), 3);
+                mjuu_zerovec(dv[l].data(), 3);
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
 //------------------ class mjCFlex implementation --------------------------------------------------
 
 // constructor
@@ -2641,6 +2789,7 @@ mjCFlex::mjCFlex(mjCModel* _model) {
 
   // clear internal variables
   nvert = 0;
+  nnode = 0;
   nedge = 0;
   nelem = 0;
   matid = -1;
@@ -2673,13 +2822,17 @@ void mjCFlex::PointToLocal() {
   spec.name = &name;
   spec.material = &spec_material_;
   spec.vertbody = &spec_vertbody_;
+  spec.nodebody = &spec_nodebody_;
   spec.vert = &spec_vert_;
+  spec.node = &spec_node_;
   spec.texcoord = &spec_texcoord_;
   spec.elem = &spec_elem_;
   spec.info = &info;
   material = nullptr;
   vertbody = nullptr;
+  nodebody = nullptr;
   vert = nullptr;
+  node = nullptr;
   texcoord = nullptr;
   elem = nullptr;
 }
@@ -2690,6 +2843,9 @@ void mjCFlex::NameSpace(const mjCModel* m) {
   for (auto& name : spec_vertbody_) {
     name = m->prefix + name + m->suffix;
   }
+  for (auto& name : spec_nodebody_) {
+    name = m->prefix + name + m->suffix;
+  }
 }
 
 
@@ -2699,7 +2855,9 @@ void mjCFlex::CopyFromSpec() {
   spec.info = &info;
   material_ = spec_material_;
   vertbody_ = spec_vertbody_;
+  nodebody_ = spec_nodebody_;
   vert_ = spec_vert_;
+  node_ = spec_node_;
   texcoord_ = spec_texcoord_;
   elem_ = spec_elem_;
 
@@ -2722,6 +2880,8 @@ void mjCFlex::DelTexcoord() {
 
 
 void mjCFlex::ResolveReferences(const mjCModel* m) {
+  vertbodyid.clear();
+  nodebodyid.clear();
   for (const auto& vertbody : vertbody_) {
     mjCBase* pbody = m->FindObject(mjOBJ_BODY, vertbody);
     if (pbody) {
@@ -2730,12 +2890,21 @@ void mjCFlex::ResolveReferences(const mjCModel* m) {
       throw mjCError(this, "unknown body '%s' in flex", vertbody.c_str());
     }
   }
+  for (const auto& nodebody : nodebody_) {
+    mjCBase* pbody = m->FindObject(mjOBJ_BODY, nodebody);
+    if (pbody) {
+      nodebodyid.push_back(pbody->id);
+    } else {
+      throw mjCError(this, "unknown body '%s' in flex", nodebody.c_str());
+    }
+  }
 }
 
 
 // compiler
 void mjCFlex::Compile(const mjVFS* vfs) {
   CopyFromSpec();
+  interpolated = !nodebody_.empty();
 
   // set nelem; check sizes
   if (dim<1 || dim>3) {
@@ -2747,8 +2916,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   if (elem_.size() % (dim+1)) {
       throw mjCError(this, "elem size must be multiple of (dim+1)");
   }
-  if (vertbody_.empty()) {
+  if (vertbody_.empty() && !interpolated) {
-      throw mjCError(this, "vertbody is empty");
+      throw mjCError(this, "vertbody and nodebody are both empty");
   }
   if (vert_.size() % 3) {
       throw mjCError(this, "vert size must be a multiple of 3");
@@ -2756,6 +2925,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   if (edgestiffness>0 && dim>1) {
     throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
   }
+  if (interpolated && selfcollide != mjFLEXSELF_NONE) {
+    throw mjCError(this, "trilinear interpolation cannot do self-collision");
+  }
+  if (interpolated && internal) {
+    throw mjCError(this, "trilinear interpolation cannot do internal collisions");
+  }
   nelem = (int)elem_.size()/(dim+1);
 
   // set nvert, rigid, centered; check size
@@ -2773,6 +2948,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     throw mjCError(this, "not enough vertices");
   }
 
+  // set nnode
+  nnode = (int)nodebody_.size();
+  if (nnode && nnode!=8) {
+    throw mjCError(this, "number of nodes must be 2^dim, it is %d", "", nnode);
+  }
+
   // check elem vertex ids
   for (const auto& elem : elem_) {
     if (elem<0 || elem>=nvert) {
@@ -2812,7 +2993,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   }
 
   // determine rigid if not already set
-  if (!rigid) {
+  if (!rigid && !interpolated) {
     rigid = true;
     for (unsigned i=1; i < vertbodyid.size(); i++) {
       if (vertbodyid[i]!=vertbodyid[0]) {
@@ -2823,7 +3004,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   }
 
   // determine centered if not already set
-  if (!centered) {
+  if (!centered && !interpolated) {
     centered = true;
     for (const auto& vert : vert_) {
       if (vert!=0) {
@@ -2833,6 +3014,16 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     }
   }
 
+  if (!centered && interpolated) {
+    centered = true;
+    for (const auto& node : node_) {
+      if (node!=0) {
+        centered = false;
+        break;
+      }
+    }
+  }
+
   // compute global vertex positions
   vertxpos = std::vector<double> (3*nvert);
   for (int i=0; i < nvert; i++) {
@@ -2841,11 +3032,32 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
 
     // add vertex offset within body if not centered
-    if (!centered) {
+    if (!centered || interpolated) {
       double offset[3];
       mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
       mjuu_addtovec(vertxpos.data()+3*i, offset, 3);
     }
+
+    if (interpolated) {
+      // this should happen in ResolveReferences but we need a body id in this loop to compute
+      // the global vertex position, this is a hack since it is the id of the parent body
+      vertbodyid[i] = -1;
+    }
+  }
+
+  // compute global node positions
+  std::vector<double> nodexpos = std::vector<double> (3*nnode);
+  for (int i=0; i < nnode; i++) {
+    // get body id, set nodexpos = body.xpos0
+    int b = nodebodyid[i];
+    mjuu_copyvec(nodexpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
+
+    // add node offset within body if not centered
+    if (!centered) {
+      double offset[3];
+      mjuu_rotVecQuat(offset, node_.data()+3*i, model->Bodies()[b]->xquat0);
+      mjuu_addtovec(nodexpos.data()+3*i, offset, 3);
+    }
   }
 
   // reorder tetrahedra so right-handed face orientation is outside
@@ -2909,7 +3121,17 @@ void mjCFlex::Compile(const mjVFS* vfs) {
       throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
     }
     stiffness.assign(21*nelem, 0);
+    if (interpolated) {
+      int min_size = ceil(nodexpos.size()*nodexpos.size() / 21);
+      if (min_size > nelem) {
+        throw mjCError(this, "Trilinear dofs are require at least %d elements", "", min_size);
+      }
+      ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
+    }
     for (unsigned int t = 0; t < nelem; t++) {
+      if (interpolated) {
+        continue;
+      }
       if (dim==2) {
         ComputeStiffness<Stencil2D>(stiffness, vertxpos,
                                     elem_.data() + (dim + 1) * t, t, young,
@@ -2928,6 +3150,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   useredge = VectorToString(edgeidx_);
 
   for (const auto& vbodyid : vertbodyid) {
+    if (vbodyid < 0) {
+      continue;
+    }
     if (model->Bodies()[vbodyid]->plugin.element) {
       mjCPlugin* plugin_instance =
           static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.element);
@@ -2954,6 +3179,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
       vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / size + 0.5;
     }
   }
+
+  // store node cartesian positions
+  node0_.assign(3*nnode, 0);
+  for (int i=0; i < nnode; i++) {
+    mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3);
+  }
 }
 
 

src/user/user_model.cc

@@ -823,6 +823,7 @@ void mjCModel::Clear() {
   nv = 0;
   nu = 0;
   na = 0;
+  nflexnode = 0;
   nflexvert = 0;
   nflexedge = 0;
   nflexelem = 0;
@@ -1733,6 +1734,7 @@ void mjCModel::SetSizes() {
 
   // flex counts
   for (int i=0; i<nflex; i++) {
+    nflexnode += flexes_[i]->nnode;
     nflexvert += flexes_[i]->nvert;
     nflexedge += flexes_[i]->nedge;
     nflexelem += flexes_[i]->nelem;
@@ -2746,7 +2748,7 @@ int mjCModel::CountNJmom(const mjModel* m) {
 
 // copy objects outside kinematic tree
 void mjCModel::CopyObjects(mjModel* m) {
-  int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
+  int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr;
   int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr;
   int bonevert_adr, graph_adr, data_adr, bvh_adr;
 
@@ -2826,6 +2828,7 @@ void mjCModel::CopyObjects(mjModel* m) {
 
   // flexes
   vert_adr = 0;
+  node_adr = 0;
   edge_adr = 0;
   elem_adr = 0;
   elemdata_adr = 0;
@@ -2865,6 +2868,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->flex_dim[i] = pfl->dim;
     m->flex_vertadr[i] = vert_adr;
     m->flex_vertnum[i] = pfl->nvert;
+    m->flex_nodeadr[i] = node_adr;
+    m->flex_nodenum[i] = pfl->nnode;
     m->flex_edgeadr[i] = edge_adr;
     m->flex_edgenum[i] = pfl->nedge;
     m->flex_elemadr[i] = elem_adr;
@@ -2924,16 +2929,27 @@ void mjCModel::CopyObjects(mjModel* m) {
     }
 
     // copy or set vert
-    if (pfl->centered) {
+    if (pfl->centered && !pfl->interpolated) {
       mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
     }
     else {
       mjuu_copyvec(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert);
     }
 
+    // copy or set node
+    if (pfl->centered && pfl->interpolated) {
+      mjuu_zerovec(m->flex_node + 3*node_adr, 3*pfl->nnode);
+    }
+    else if (pfl->interpolated) {
+      mjuu_copyvec(m->flex_node + 3*node_adr, pfl->node_.data(), 3*pfl->nnode);
+    }
+
     // copy vert0
     mjuu_copyvec(m->flex_vert0 + 3*vert_adr, pfl->vert0_.data(), 3*pfl->nvert);
 
+    // copy node0
+    mjuu_copyvec(m->flex_node0 + 3*node_adr, pfl->node0_.data(), 3*pfl->nnode);
+
     // copy or set vertbodyid
     if (pfl->rigid) {
       for (int k=0; k<pfl->nvert; k++) {
@@ -2944,6 +2960,18 @@ void mjCModel::CopyObjects(mjModel* m) {
       memcpy(m->flex_vertbodyid + vert_adr, pfl->vertbodyid.data(), pfl->nvert*sizeof(int));
     }
 
+    // copy or set nodebodyid
+    if (pfl->rigid) {
+      for (int k=0; k<pfl->nnode; k++) {
+        m->flex_nodebodyid[node_adr + k] = pfl->nodebodyid[0];
+      }
+    } else {
+      memcpy(m->flex_nodebodyid + node_adr, pfl->nodebodyid.data(), pfl->nnode*sizeof(int));
+    }
+
+    // set interpolation type, only two types for now
+    m->flex_interp[i] = pfl->interpolated;
+
     // convert edge pairs to int array, set edge rigid
     for (int k=0; k<pfl->nedge; k++) {
       m->flex_edge[2*(edge_adr+k)] = pfl->edge[k].first;
@@ -2951,8 +2979,9 @@ void mjCModel::CopyObjects(mjModel* m) {
 
       if (pfl->rigid) {
         m->flexedge_rigid[edge_adr+k] = 1;
-      } else {
+      } else if (!pfl->interpolated) {
         // check if vertex body weldids are the same
+        // unsupported by trilinear interpolation
         int b1 = pfl->vertbodyid[pfl->edge[k].first];
         int b2 = pfl->vertbodyid[pfl->edge[k].second];
         m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid);
@@ -2961,6 +2990,7 @@ void mjCModel::CopyObjects(mjModel* m) {
 
     // advance counters
     vert_adr += pfl->nvert;
+    node_adr += pfl->nnode;
     edge_adr += pfl->nedge;
     elem_adr += pfl->nelem;
     elemdata_adr += (pfl->dim+1) * pfl->nelem;
@@ -4317,7 +4347,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
   // create low-level model
   mj_makeModel(&m,
                nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
-               ncam, nlight, nflex, nflexvert, nflexedge, nflexelem,
+               ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem,
                nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord,
                nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
                nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,

src/user/user_model.h

@@ -86,6 +86,7 @@ class mjCModel_ : public mjsElement {
   int nbvh;            // number of total boundary volume hierarchies
   int nbvhstatic;      // number of static boundary volume hierarchies
   int nbvhdynamic;     // number of dynamic boundary volume hierarchies
+  int nflexnode;       // number of nodes in all flexes
   int nflexvert;       // number of vertices in all flexes
   int nflexedge;       // number of edges in all flexes
   int nflexelem;       // number of elements in all flexes

src/user/user_objects.h

@@ -746,12 +746,15 @@ class mjCLight : public mjCLight_, private mjsLight {
 class mjCFlex_ : public mjCBase {
  protected:
   int nvert;                              // number of verices
+  int nnode;                              // number of nodes
   int nedge;                              // number of edges
   int nelem;                              // number of elements
   int matid;                              // material id
   bool rigid;                             // all vertices attached to the same body
   bool centered;                          // all vertices coordinates (0,0,0)
+  bool interpolated;                      // vertices are interpolated from nodes
   std::vector<int> vertbodyid;            // vertex body ids
+  std::vector<int> nodebodyid;            // node body ids
   std::vector<std::pair<int, int>> edge;  // edge vertex ids
   std::vector<int> shell;                 // shell fragment vertex ids (dim per fragment)
   std::vector<int> elemlayer;             // element layer (distance from border)
@@ -764,14 +767,18 @@ class mjCFlex_ : public mjCBase {
 
   // variable-size data
   std::vector<std::string> vertbody_;     // vertex body names
+  std::vector<std::string> nodebody_;     // node body names
   std::vector<double> vert_;              // vertex positions
+  std::vector<double> node_;              // node positions
   std::vector<int> elem_;                 // element vertex ids
   std::vector<float> texcoord_;           // vertex texture coordinates
   std::string material_;                  // name of material used for rendering
 
   std::string spec_material_;
   std::vector<std::string> spec_vertbody_;
+  std::vector<std::string> spec_nodebody_;
   std::vector<double> spec_vert_;
+  std::vector<double> spec_node_;
   std::vector<int> spec_elem_;
   std::vector<float> spec_texcoord_;
 };
@@ -804,6 +811,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
   const std::vector<double>& get_elemaabb() const { return elemaabb_; }
   const std::vector<int>& get_elem() const { return elem_; }
   const std::vector<float>& get_texcoord() const { return texcoord_; }
+  const std::vector<std::string>& get_nodebody() const { return nodebody_; }
 
   bool HasTexcoord() const;               // texcoord not null
   void DelTexcoord();                     // delete texcoord
@@ -816,6 +824,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
   void CreateShellPair(void);             // create shells and evpairs
 
   std::vector<double> vert0_;             // vertex positions in [0, 1]^d in the bounding box
+  std::vector<double> node0_;             // node Cartesian positions
 };
 
 

src/xml/xml_native_reader.cc

@@ -311,7 +311,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
               {"config", "*", "2", "key", "value"},
             {">"},
         {">"},
-        {"flexcomp", "*", "25", "name", "type", "group", "dim",
+        {"flexcomp", "*", "26", "name", "type", "group", "dim", "dof",
             "count", "spacing", "radius", "rigid", "mass", "inertiabox",
             "scale", "file", "point", "element", "texcoord", "material", "rgba",
             "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"},
@@ -331,8 +331,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
 
     {"deformable", "*", "0"},
     {"<"},
-        {"flex", "*", "11", "name", "group", "dim", "radius", "material",
+        {"flex", "*", "12", "name", "group", "dim", "radius", "material",
-            "rgba", "flatskin", "body", "vertex", "element", "texcoord"},
+            "rgba", "flatskin", "body", "vertex", "element", "texcoord", "node"},
         {"<"},
             {"contact", "?", "13", "contype", "conaffinity", "condim", "priority",
                 "friction", "solmix", "solref", "solimp", "margin", "gap",
@@ -803,6 +803,14 @@ const mjMap fcomp_map[mjNFCOMPTYPES] = {
 };
 
 
+// flexcomp dof type
+const mjMap fdof_map[mjNFCOMPDOFS] = {
+  {"full",        mjFCOMPDOF_FULL},
+  {"radial",      mjFCOMPDOF_RADIAL},
+  {"trilinear",   mjFCOMPDOF_TRILINEAR}
+};
+
+
 // flex selfcollide type
 const mjMap flexself_map[5] = {
   {"none",        mjFLEXSELF_NONE},
@@ -1324,7 +1332,7 @@ void mjXReader::Statistic(XMLElement* section) {
 
 // flex element parser
 void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
-  string text, name, material;
+  string text, name, material, nodebody;
   int n;
 
   // read attributes
@@ -1347,6 +1355,9 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
   if (ReadAttrTxt(elem, "body", text, true)) {
     mjs_setStringVec(flex->vertbody, text.c_str());
   }
+  if (ReadAttrTxt(elem, "node", nodebody)) {
+    mjs_setStringVec(flex->nodebody, nodebody.c_str());
+  }
   auto vert = ReadAttrVec<double>(elem, "vertex");
   if (vert.has_value()) {
     mjs_setDouble(flex->vert, vert->data(), vert->size());
@@ -2678,6 +2689,11 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
     fcomp.texcoord = std::move(texcoord.value());
   }
 
+  // dof type
+  if (MapValue(elem, "dof", &n, fdof_map, mjNFCOMPDOFS)) {
+    fcomp.doftype = (mjtDof)n;
+  }
+
   // edge
   XMLElement* edge = FirstChildElement(elem, "edge");
   if (edge) {

src/xml/xml_native_writer.cc

@@ -158,6 +158,10 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
     text = VectorToString(flex->get_texcoord());
     WriteAttrTxt(elem, "texcoord", text);
   }
+  if (!flex->get_nodebody().empty()) {
+    text = VectorToString(flex->get_nodebody());
+    WriteAttrTxt(elem, "node", text);
+  }
 
   // contact subelement
   XMLElement* cont = InsertEnd(elem, "contact");

test/engine/engine_util_misc_test.cc

@@ -32,6 +32,7 @@ namespace {
 using ::testing::DoubleNear;
 using ::testing::HasSubstr;
 using ::testing::Ne;
+using ::testing::Pointwise;
 using ::testing::StrEq;
 using ::testing::ElementsAreArray;
 
@@ -210,6 +211,94 @@ TEST_F(UtilMiscTest, MuscleGainLength) {
   EXPECT_EQ(mju_muscleGainLength(2.0,  lmin, lmax), 0);
 }
 
+// --------------------------------- Interpolation -----------------------------
+
+using InterpolationTest = MujocoTest;
+
+TEST_F(InterpolationTest, mju_defGradient) {
+  int order = 1;
+  mjtNum mat[9];
+  mjtNum p1[3] = {.5, .5, .5};
+  mjtNum p2[3] = {.25, .25, .25};
+  mjtNum dof0[24] = {0, 0, 0,  0, 0, 1,  0, 1, 0,  0, 1, 1,
+                     1, 0, 0,  1, 0, 1,  1, 1, 0,  1, 1, 1};
+
+  // identity
+  mjtNum dof1[24];
+  for (int i = 0; i < 24; ++i) dof1[i] = dof0[i];
+  mju_defGradient(mat, p1, dof1, order);
+  EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
+
+  // translation
+  mjtNum dof2[24];
+  for (int i = 0; i < 24; ++i) dof2[i] = 2 + dof0[i];
+  mju_defGradient(mat, p1, dof2, order);
+  EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
+  mju_defGradient(mat, p2, dof2, order);
+  EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
+
+  // constant stretch
+  mjtNum dof3[24];
+  for (int i = 0; i < 24; ++i) dof3[i] = 2*dof0[i];
+  mju_defGradient(mat, p1, dof3, order);
+  EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2}));
+  mju_defGradient(mat, p2, dof3, order);
+  EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2}));
+
+  // axial stretch
+  mjtNum dof4[24];
+  for (int i = 0; i < 24; ++i) dof4[i] = (i%3 == 1 ? 2 : 1)*dof0[i];
+  mju_defGradient(mat, p1, dof4, order);
+  EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1}));
+  mju_defGradient(mat, p2, dof4, order);
+  EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1}));
+
+  // z-axis 90 degree rotation
+  mjtNum dof5[24];
+  for (int i = 0; i < 8; ++i) {
+    mjtNum quat[4] = {0, 0, 0, 1};
+    mjtNum axis[3] = {0, 0, 1};
+    mju_axisAngle2Quat(quat, axis, mjPI/2);
+    mju_rotVecQuat(dof5 + 3*i, dof0 + 3*i, quat);
+  }
+  mju_defGradient(mat, p1, dof5, order);
+  EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
+  mju_defGradient(mat, p2, dof5, order);
+  EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
+
+  // z-axis 30 degree rotation
+  mjtNum dof6[24];
+  mjtNum rot6[9];
+  for (int i = 0; i < 8; ++i) {
+    mjtNum quat[4];
+    mjtNum axis[3] = {0, 0, 1};
+    mju_axisAngle2Quat(quat, axis, mjPI/6);
+    mju_rotVecQuat(dof6 + 3*i, dof0 + 3*i, quat);
+    mju_quat2Mat(rot6, quat);
+  }
+  mju_defGradient(mat, p1, dof6, order);
+  EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
+  mju_defGradient(mat, p2, dof6, order);
+  EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
+
+  // z-axis CoM rotation
+  mjtNum dof7[24];
+  mjtNum rot7[9];
+  for (int i = 0; i < 8; ++i) {
+    mjtNum quat[4];
+    mjtNum axis[3] = {0, 0, 1};
+    mjtNum offset[3] = {-.5, -.5, 0};
+    mju_axisAngle2Quat(quat, axis, mjPI/6);
+    mju_add3(dof7 + 3*i, dof0 + 3*i, offset);
+    mju_rotVecQuat(dof7 + 3*i, dof0 + 3*i, quat);
+    mju_quat2Mat(rot7, quat);
+  }
+  mju_defGradient(mat, p1, dof7, order);
+  EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
+  mju_defGradient(mat, p2, dof7, order);
+  EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
+}
+
 // --------------------------------- Base64 ------------------------------------
 
 using Base64Test = MujocoTest;

test/testdata/flex.xml

@@ -29,7 +29,7 @@
       <freejoint/>
 
       <flexcomp name="f1" type="ellipsoid" rgba=".8 .2 .2 1" radius="0.001" count="4 4 4"
-                spacing=".025 .025 .025" dim="3" mass="1">
+                spacing=".025 .025 .025" dim="3" mass="1" dof="radial">
         <edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
       </flexcomp>
     </body>
@@ -44,6 +44,12 @@
               zaxis="1 1 1" count="3 3 3" mass="1" spacing="0.02 0.03 0.04">
       <edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
     </flexcomp>
+
+    <flexcomp type="grid" count="8 8 8" spacing=".007 .007 .007" pos="0 0 1.5" dim="3"
+              radius=".0001" rgba="0 .7 .7 1" mass=".25" name="softbody" dof="trilinear">
+      <elasticity young="1e4" poisson="0.1" damping="0.01"/>
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
   </worldbody>
 
 </mujoco>

test/user/user_flex_test.cc

@@ -28,14 +28,15 @@
 namespace mujoco {
 namespace {
 
+using ::testing::DoubleNear;
 using ::testing::IsNull;
 using ::testing::NotNull;
 using ::testing::HasSubstr;
+using ::testing::Pointwise;
 using UserFlexTest = MujocoTest;
 
 
 
-
 TEST_F(UserFlexTest, ParentMustHaveName) {
   static constexpr char xml[] = R"(
   <mujoco>
@@ -282,6 +283,158 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) {
   mj_deleteData(d);
 }
 
+TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
+  std::array<char, 1024> error;
+  static constexpr char xml_selfcoll[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
+      <contact selfcollide="auto" internal="false"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  mjModel* m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size());
+  EXPECT_THAT(m1, IsNull()) << error.data();
+  EXPECT_THAT(error.data(),
+              HasSubstr("trilinear interpolation cannot do self-collision"));
+  static constexpr char xml_internal[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
+      <contact selfcollide="none" internal="true"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  mjModel* m2 = LoadModelFromString(xml_internal, error.data(), error.size());
+  EXPECT_THAT(m2, IsNull()) << error.data();
+  EXPECT_THAT(error.data(),
+              HasSubstr("trilinear interpolation cannot do internal"));
+}
+
+TEST_F(UserFlexTest, TrilinearInterpolation) {
+  static constexpr char xml_trilinear[] = R"(
+  <mujoco>
+  <worldbody>
+    <geom type="plane" pos="0 0 -.5" size="10 10 .1"/>
+    <flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m1 = LoadModelFromString(xml_trilinear, error.data(), error.size());
+  ASSERT_THAT(m1, NotNull()) << error.data();
+  mjData* d1 = mj_makeData(m1);
+  mj_step(m1, d1);
+
+  static constexpr char xml_linear[] = R"(
+  <mujoco>
+  <worldbody>
+    <geom type="plane" pos="0 0 -.5" size="10 10 .1"/>
+    <flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3">
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  mjModel* m2 = LoadModelFromString(xml_linear, error.data(), error.size());
+  ASSERT_THAT(m2, NotNull()) << error.data();
+  mjData* d2 = mj_makeData(m2);
+  mj_step(m2, d2);
+
+  EXPECT_EQ(m1->nflexvert, m2->nflexvert);
+  for (int i = 0; i < 3*m1->nflexvert; ++i) {
+    EXPECT_EQ(m1->flex_vert[i], d2->flexvert_xpos[i]);
+    EXPECT_EQ(m1->flex_vert0[i], m2->flex_vert0[i]);
+    EXPECT_EQ(d1->flexvert_xpos[i], d2->flexvert_xpos[i]);
+  }
+
+  EXPECT_EQ(m1->nM, m2->nM);
+  for (int i = 0; i < m1->nM; ++i) {
+    EXPECT_EQ(d1->qM[i], d2->qM[i]);
+  }
+
+  EXPECT_EQ(m1->nbody, m2->nbody);
+  for (int i = 0; i < m2->nbody; ++i) {
+    if (i == 0) {
+      continue;
+    }
+    EXPECT_EQ(m1->body_mass[i], m2->body_mass[i]);
+    for (int j = 0; j < 2; ++j) {
+      EXPECT_EQ(m1->body_invweight0[i*2+j], m2->body_invweight0[i*2+j]);
+    }
+    for (int j = 0; j < 10; ++j) {
+      EXPECT_NEAR(d1->cinert[10*i+j], d2->cinert[i*10+j], 1e-5) << i;
+      EXPECT_NEAR(d1->crb[10*i+j], d2->crb[i*10+j], 1e-5) << i;
+    }
+  }
+
+  EXPECT_EQ(d1->ncon, 4);
+  EXPECT_EQ(d2->ncon, 4);
+  for (int i = 0; i < d1->ncon; ++i) {
+    EXPECT_EQ(d1->contact[i].dist, d2->contact[i].dist);
+    EXPECT_EQ(d1->contact[i].mu, d2->contact[i].mu);
+    for (int j = 0; j < 5; ++j) {
+      EXPECT_EQ(d1->contact[i].friction[j], d2->contact[i].friction[j]);
+    }
+    for (int j = 0; j < 3; ++j) {
+      EXPECT_EQ(d1->contact[i].pos[j], d2->contact[i].pos[j]);
+    }
+    for (int j = 0; j < 9; ++j) {
+      EXPECT_EQ(d1->contact[i].frame[j], d2->contact[i].frame[j]);
+    }
+    for (int j = 0; j < 36; ++j) {
+      EXPECT_EQ(d1->contact[i].H[j], d2->contact[i].H[j]);
+    }
+  }
+
+  EXPECT_EQ(d1->nefc, 4*(d1->contact[0].dim-1)*2);
+  EXPECT_EQ(d2->nefc, 4*(d2->contact[0].dim-1)*2);
+  EXPECT_EQ(d1->nJ, d2->nJ);
+  for (int i = 0; i < d1->nefc; ++i) {
+    EXPECT_EQ(d1->efc_diagApprox[i], d2->efc_diagApprox[i]);
+    EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]);
+  }
+
+  mj_deleteModel(m1);
+  mj_deleteModel(m2);
+  mj_deleteData(d1);
+  mj_deleteData(d2);
+}
+
+TEST_F(UserFlexTest, StiffnessMatrix) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="grid" count="3 3 3" spacing="1 1 1" dim="3" dof="trilinear">
+      <contact selfcollide="none" internal="false"/>
+      <elasticity young="1"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  EXPECT_NE(m->flex_stiffness[0], 0);
+  EXPECT_EQ(m->nflexnode, 8);
+
+  // constants are in the kernel
+  mjtNum ones[24], zeros[24], res[24];
+  for (int i = 0; i < 3*m->nflexnode; ++i) {
+    zeros[i] = 0;
+    ones[i] = 1;
+  }
+  mju_mulMatVec(res, m->flex_stiffness, ones, 3*m->nflexnode, 3*m->nflexnode);
+  EXPECT_THAT(res, Pointwise(DoubleNear(1e-8), zeros));
+
+  mj_deleteModel(m);
+}
+
 TEST_F(UserFlexTest, LoadMSHBinary_41_Success) {
   const std::string xml_path =
       GetTestDataFilePath("user/testdata/cube_41_binary_vol_gmshApp.xml");

unity/Runtime/Bindings/MjBindings.cs

@@ -41,6 +41,7 @@ public const double mjMINIMP = 0.0001;
 public const double mjMAXIMP = 0.9999;
 public const int mjMAXCONPAIR = 50;
 public const int mjMAXTREEDEPTH = 50;
+public const int mjMAXFLEXNODES = 27;
 public const int mjNEQDATA = 11;
 public const int mjNDYN = 10;
 public const int mjNGAIN = 10;
@@ -5191,6 +5192,7 @@ public unsafe struct mjModel_ {
   public int ncam;
   public int nlight;
   public int nflex;
+  public int nflexnode;
   public int nflexvert;
   public int nflexedge;
   public int nflexelem;
@@ -5411,6 +5413,9 @@ public unsafe struct mjModel_ {
   public int* flex_dim;
   public int* flex_matid;
   public int* flex_group;
+  public int* flex_interp;
+  public int* flex_nodeadr;
+  public int* flex_nodenum;
   public int* flex_vertadr;
   public int* flex_vertnum;
   public int* flex_edgeadr;
@@ -5424,6 +5429,7 @@ public unsafe struct mjModel_ {
   public int* flex_evpairadr;
   public int* flex_evpairnum;
   public int* flex_texcoordadr;
+  public int* flex_nodebodyid;
   public int* flex_vertbodyid;
   public int* flex_edge;
   public int* flex_elem;
@@ -5433,6 +5439,8 @@ public unsafe struct mjModel_ {
   public int* flex_evpair;
   public double* flex_vert;
   public double* flex_vert0;
+  public double* flex_node;
+  public double* flex_node0;
   public double* flexedge_length0;
   public double* flexedge_invweight0;
   public double* flex_radius;
@@ -6296,6 +6304,10 @@ public unsafe struct model {
   public int* flex_dim;
   public int* flex_matid;
   public int* flex_group;
+  public int* flex_interp;
+  public int* flex_nodeadr;
+  public int* flex_nodenum;
+  public int* flex_nodebodyid;
   public int* flex_vertadr;
   public int* flex_vertnum;
   public int* flex_elem;
@@ -6309,6 +6321,8 @@ public unsafe struct model {
   public int* flex_texcoordadr;
   public int* flex_bvhadr;
   public int* flex_bvhnum;
+  public byte* flex_centered;
+  public double* flex_node;
   public double* flex_radius;
   public float* flex_rgba;
   public int* hfield_pathadr;