Add constraint island discovery

PiperOrigin-RevId: 557067599
Change-Id: Ic41e1d0efef02b7a79142518afe49cf9d4e74725

doc/APIreference/functions.rst

@@ -840,6 +840,15 @@ mj_makeConstraint
 
 Construct constraints.
 
+.. _mj_island:
+
+mj_island
+~~~~~~~~~
+
+.. mujoco-include:: mj_island
+
+Find constraint islands.
+
 .. _mj_projectConstraint:
 
 mj_projectConstraint

doc/XMLreference.rst

@@ -2070,6 +2070,12 @@ from its default.
    This flag disables all computations related to sensors. When disabled, sensor values will remain constant, either
    zeros if disabled at the start of simulation, or, if disabled at runtime, whatever value was last computed.
 
+.. _option-flag-midphase:
+
+:at:`midphase`: :at-val:`[disable, enable], "enable"`
+   This flag disables the mid-phase collision filtering using a static AABB bounding volume hierarchy (a BVH binary
+   tree). If disabled, all geoms pairs that are allowed to collide are checked for collisions.
+
 .. _option-flag-override:
 
 :at:`override`: :at-val:`[disable, enable], "disable"`
@@ -2108,12 +2114,13 @@ from its default.
    function. If a new contact is detected it is added, allowing for up to 4 additional contact points. This feature is
    currently considered experimental, and both the behavior and the way it is activated may change in the future.
 
-.. _option-flag-midphase:
+.. _option-flag-island:
-
-:at:`midphase`: :at-val:`[disable, enable], "enable"`
-   This flag disables the mid-phase collision filtering using a static AABB bounding volume hierarchy (a BVH binary
-   tree). If disabled, all geoms pairs that are allowed to collide are checked for collisions.
 
+:at:`island`: :at-val:`[disable, enable], "disable"`
+   This flag enables discovery of constraint islands: disjoint sets of constraints and
+   degrees-of-freedom that do not interact. The flag currently has no effect on the physics pipeline, but enabling it
+   allows for `island visualization <https://youtu.be/Vc1tq0fFvQA>`__.
+   In a future release, the constraint solver will exploit the disjoint nature of constraint islands.
 
 .. _body:
 

doc/XMLschema.rst

@@ -227,7 +227,7 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`sensor<option-flag-sensor>`                               | :ref:`override<option-flag-override>`                           | :ref:`energy<option-flag-energy>`                               | :ref:`fwdinv<option-flag-fwdinv>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`sensornoise<option-flag-sensornoise>`                     | :ref:`multiccd<option-flag-multiccd>`                           |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`sensornoise<option-flag-sensornoise>`                     | :ref:`multiccd<option-flag-multiccd>`                           | :ref:`island<option-flag-island>`                               |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | mujoco |br| |L|                    |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -8,7 +8,16 @@ Upcoming version (not yet released)
 General
 ^^^^^^^
 
-1. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
+.. youtube:: Vc1tq0fFvQA
+   :align: right
+   :width: 240px
+
+1. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints
+   and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to
+   exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag<option-flag-island>`
+   which will be removed after the refactor. If island discovery is enabled, geoms, contacts and
+   tendons will be colored according to the corresponding island, see video:
+#. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
    the exact formula rather than with Euler integration.
 #. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
    actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep.
@@ -19,8 +28,9 @@ General
 Python bindings
 ^^^^^^^^^^^^^^^
 
-3. Fixed issue where calling ``update_scene`` with invalid camera name uses the default camera.
+4. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
-   (`#870 <https://github.com/deepmind/mujoco/issues/870>`_)
+   camera name used the default camera.
+
 
 Version 2.3.7 (July 20, 2023)
 -----------------------------

doc/includes/references.h

@@ -162,6 +162,7 @@ struct mjData_ {
   int     nefc;              // number of constraints
   int     nnzJ;              // number of non-zeros in constraint Jacobian
   int     ncon;              // number of detected contacts
+  int     nisland;           // number of detected constraint islands
 
   // global properties
   mjtNum  time;              // simulation time
@@ -342,6 +343,14 @@ struct mjData_ {
   mjtNum* efc_D;             // constraint mass                                  (nefc x 1)
   mjtNum* efc_R;             // inverse constraint mass                          (nefc x 1)
 
+  // computed by mj_island
+  int*    island_dofadr;     // address of first dof in island                   (nisland x 1)
+  int*    island_efcadr;     // address of first constraint in island            (nisland x 1)
+  int*    dof_island;        // island id of this dof; -1: none                  (nv x 1)
+  int*    dof_islandnext;    // address of next dof in island; -1: last or none  (nv x 1)
+  int*    efc_island;        // island id of this constraint                     (nefc x 1)
+  int*    efc_islandnext;    // address of next constraint in island; -1: last   (nefc x 1)
+
   // computed by mj_projectConstraint (dual solver)
   int*    efc_AR_rownnz;     // number of non-zeros in AR                        (nefc x 1)
   int*    efc_AR_rowadr;     // row start address in colind array                (nefc x 1)
@@ -387,8 +396,9 @@ typedef enum mjtEnableBit_ {      // enable optional feature bitflags
   mjENBL_SENSORNOISE  = 1<<3,     // add noise to sensor data
                                   // experimental features:
   mjENBL_MULTICCD     = 1<<4,     // multi-point convex collision detection
+  mjENBL_ISLAND       = 1<<5,     // constraint island discovery
 
-  mjNENABLE           = 5         // number of enable flags
+  mjNENABLE           = 6         // number of enable flags
 } mjtEnableBit;
 typedef enum mjtJoint_ {          // type of degree of freedom
   mjJNT_FREE          = 0,        // global position and orientation (quat)       (7)
@@ -802,7 +812,7 @@ struct mjModel_ {
   int nu;                         // number of actuators/controls = dim(ctrl)
   int na;                         // number of activation states = dim(act)
   int nbody;                      // number of bodies
-  int nbvh;                       // number of total bounding volumes in all bodies
+  int nbvh;                       // number of bounding volumes in all bodies
   int njnt;                       // number of joints
   int ngeom;                      // number of geoms
   int nsite;                      // number of sites
@@ -856,13 +866,14 @@ struct mjModel_ {
   int nM;                         // number of non-zeros in sparse inertia matrix
   int nD;                         // number of non-zeros in sparse dof-dof matrix
   int nB;                         // number of non-zeros in sparse body-dof matrix
+  int ntree;                      // number of kinematic trees under world body
   int nemax;                      // number of potential equality-constraint rows
   int njmax;                      // number of available rows in constraint Jacobian
   int nconmax;                    // number of potential contacts in contact list
   int nstack;                     // number of fields in mjData stack
   int nuserdata;                  // number of extra fields in mjData
   int nsensordata;                // number of fields in sensor data vector
-  int npluginstate;               // number of fields in the plugin state vector
+  int npluginstate;               // number of fields in plugin state vector
 
   int nbuffer;                    // number of bytes in buffer
 
@@ -890,6 +901,7 @@ struct mjModel_ {
   int*      body_jntadr;          // start addr of joints; -1: no joints      (nbody x 1)
   int*      body_dofnum;          // number of motion degrees of freedom      (nbody x 1)
   int*      body_dofadr;          // start addr of dofs; -1: no dofs          (nbody x 1)
+  int*      body_treeid;          // id of body's kinematic tree; -1: static  (nbody x 1)
   int*      body_geomnum;         // number of geoms                          (nbody x 1)
   int*      body_geomadr;         // start addr of geoms; -1: no geoms        (nbody x 1)
   mjtByte*  body_simple;          // body is simple (has diagonal M)          (nbody x 1)
@@ -936,6 +948,7 @@ struct mjModel_ {
   int*      dof_bodyid;           // id of dof's body                         (nv x 1)
   int*      dof_jntid;            // id of dof's joint                        (nv x 1)
   int*      dof_parentid;         // id of dof's parent; -1: none             (nv x 1)
+  int*      dof_treeid;           // id of dof's kinematic tree               (nv x 1)
   int*      dof_Madr;             // dof address in M-diagonal                (nv x 1)
   int*      dof_simplenum;        // number of consecutive simple dofs        (nv x 1)
   mjtNum*   dof_solref;           // constraint solver reference:frictionloss (nv x mjNREF)
@@ -1663,6 +1676,7 @@ typedef enum mjtLabel_ {          // object labeling
   mjLABEL_SELPNT,                 // coordinates of selection point
   mjLABEL_CONTACTPOINT,           // contact information
   mjLABEL_CONTACTFORCE,           // magnitude of contact force
+  mjLABEL_ISLAND,                 // id of island
 
   mjNLABEL                        // number of label types
 } mjtLabel;
@@ -1694,6 +1708,7 @@ typedef enum mjtVisFlag_ {        // flags enabling model element visualization
   mjVIS_PERTFORCE,                // perturbation force
   mjVIS_PERTOBJ,                  // perturbation object
   mjVIS_CONTACTPOINT,             // contact points
+  mjVIS_ISLAND,                   // constraint islands
   mjVIS_CONTACTFORCE,             // contact force
   mjVIS_CONTACTSPLIT,             // split contact force into normal and tangent
   mjVIS_TRANSPARENT,              // make dynamic geoms more transparent
@@ -2080,6 +2095,7 @@ struct mjvSceneState_ {
 
     int nefc;
     int ncon;
+    int nisland;
 
     mjtNum time;
 
@@ -2114,6 +2130,9 @@ struct mjvSceneState_ {
     mjtNum* wrap_xpos;
 
     mjtByte* bvh_active;
+    int* island_dofadr;
+    int* dof_island;
+    int* efc_island;
 
     mjContact* contact;
     mjtNum* efc_force;
@@ -2204,6 +2223,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result);
 void mj_rnePostConstraint(const mjModel* m, mjData* d);
 void mj_collision(const mjModel* m, mjData* d);
 void mj_makeConstraint(const mjModel* m, mjData* d);
+void mj_island(const mjModel* m, mjData* d);
 void mj_projectConstraint(const mjModel* m, mjData* d);
 void mj_referenceConstraint(const mjModel* m, mjData* d);
 void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,

include/mujoco/mjdata.h

@@ -189,6 +189,7 @@ struct mjData_ {
   int     nefc;              // number of constraints
   int     nnzJ;              // number of non-zeros in constraint Jacobian
   int     ncon;              // number of detected contacts
+  int     nisland;           // number of detected constraint islands
 
   // global properties
   mjtNum  time;              // simulation time
@@ -369,6 +370,14 @@ struct mjData_ {
   mjtNum* efc_D;             // constraint mass                                  (nefc x 1)
   mjtNum* efc_R;             // inverse constraint mass                          (nefc x 1)
 
+  // computed by mj_island
+  int*    island_dofadr;     // address of first dof in island                   (nisland x 1)
+  int*    island_efcadr;     // address of first constraint in island            (nisland x 1)
+  int*    dof_island;        // island id of this dof; -1: none                  (nv x 1)
+  int*    dof_islandnext;    // address of next dof in island; -1: last or none  (nv x 1)
+  int*    efc_island;        // island id of this constraint                     (nefc x 1)
+  int*    efc_islandnext;    // address of next constraint in island; -1: last   (nefc x 1)
+
   // computed by mj_projectConstraint (dual solver)
   int*    efc_AR_rownnz;     // number of non-zeros in AR                        (nefc x 1)
   int*    efc_AR_rowadr;     // row start address in colind array                (nefc x 1)

include/mujoco/mjmodel.h

@@ -70,8 +70,9 @@ typedef enum mjtEnableBit_ {      // enable optional feature bitflags
   mjENBL_SENSORNOISE  = 1<<3,     // add noise to sensor data
                                   // experimental features:
   mjENBL_MULTICCD     = 1<<4,     // multi-point convex collision detection
+  mjENBL_ISLAND       = 1<<5,     // constraint island discovery
 
-  mjNENABLE           = 5         // number of enable flags
+  mjNENABLE           = 6         // number of enable flags
 } mjtEnableBit;
 
 
@@ -557,7 +558,7 @@ struct mjModel_ {
   int nu;                         // number of actuators/controls = dim(ctrl)
   int na;                         // number of activation states = dim(act)
   int nbody;                      // number of bodies
-  int nbvh;                       // number of total bounding volumes in all bodies
+  int nbvh;                       // number of bounding volumes in all bodies
   int njnt;                       // number of joints
   int ngeom;                      // number of geoms
   int nsite;                      // number of sites
@@ -611,13 +612,14 @@ struct mjModel_ {
   int nM;                         // number of non-zeros in sparse inertia matrix
   int nD;                         // number of non-zeros in sparse dof-dof matrix
   int nB;                         // number of non-zeros in sparse body-dof matrix
+  int ntree;                      // number of kinematic trees under world body
   int nemax;                      // number of potential equality-constraint rows
   int njmax;                      // number of available rows in constraint Jacobian
   int nconmax;                    // number of potential contacts in contact list
   int nstack;                     // number of fields in mjData stack
   int nuserdata;                  // number of extra fields in mjData
   int nsensordata;                // number of fields in sensor data vector
-  int npluginstate;               // number of fields in the plugin state vector
+  int npluginstate;               // number of fields in plugin state vector
 
   int nbuffer;                    // number of bytes in buffer
 
@@ -645,6 +647,7 @@ struct mjModel_ {
   int*      body_jntadr;          // start addr of joints; -1: no joints      (nbody x 1)
   int*      body_dofnum;          // number of motion degrees of freedom      (nbody x 1)
   int*      body_dofadr;          // start addr of dofs; -1: no dofs          (nbody x 1)
+  int*      body_treeid;          // id of body's kinematic tree; -1: static  (nbody x 1)
   int*      body_geomnum;         // number of geoms                          (nbody x 1)
   int*      body_geomadr;         // start addr of geoms; -1: no geoms        (nbody x 1)
   mjtByte*  body_simple;          // body is simple (has diagonal M)          (nbody x 1)
@@ -691,6 +694,7 @@ struct mjModel_ {
   int*      dof_bodyid;           // id of dof's body                         (nv x 1)
   int*      dof_jntid;            // id of dof's joint                        (nv x 1)
   int*      dof_parentid;         // id of dof's parent; -1: none             (nv x 1)
+  int*      dof_treeid;           // id of dof's kinematic tree               (nv x 1)
   int*      dof_Madr;             // dof address in M-diagonal                (nv x 1)
   int*      dof_simplenum;        // number of consecutive simple dofs        (nv x 1)
   mjtNum*   dof_solref;           // constraint solver reference:frictionloss (nv x mjNREF)

include/mujoco/mjvisualize.h

@@ -79,6 +79,7 @@ typedef enum mjtLabel_ {          // object labeling
   mjLABEL_SELPNT,                 // coordinates of selection point
   mjLABEL_CONTACTPOINT,           // contact information
   mjLABEL_CONTACTFORCE,           // magnitude of contact force
+  mjLABEL_ISLAND,                 // id of island
 
   mjNLABEL                        // number of label types
 } mjtLabel;
@@ -114,6 +115,7 @@ typedef enum mjtVisFlag_ {        // flags enabling model element visualization
   mjVIS_PERTFORCE,                // perturbation force
   mjVIS_PERTOBJ,                  // perturbation object
   mjVIS_CONTACTPOINT,             // contact points
+  mjVIS_ISLAND,                   // constraint islands
   mjVIS_CONTACTFORCE,             // contact force
   mjVIS_CONTACTSPLIT,             // split contact force into normal and tangent
   mjVIS_TRANSPARENT,              // make dynamic geoms more transparent
@@ -540,6 +542,7 @@ struct mjvSceneState_ {
 
     int nefc;
     int ncon;
+    int nisland;
 
     mjtNum time;
 
@@ -574,6 +577,9 @@ struct mjvSceneState_ {
     mjtNum* wrap_xpos;
 
     mjtByte* bvh_active;
+    int* island_dofadr;
+    int* dof_island;
+    int* efc_island;
 
     mjContact* contact;
     mjtNum* efc_force;

include/mujoco/mjxmacro.h

@@ -61,7 +61,7 @@
 //-------------------------------- mjModel ---------------------------------------------------------
 
 // int fields of mjModel
-#define MJMODEL_INTS        \
+#define MJMODEL_INTS           \
     X   ( nq )                 \
     X   ( nv )                 \
     XMJV( nu )                 \
@@ -122,6 +122,7 @@
     X   ( nemax )              \
     X   ( njmax )              \
     X   ( nconmax )            \
+    X   ( ntree )              \
     X   ( nstack )             \
     X   ( nuserdata )          \
     XMJV( nsensordata )        \
@@ -153,7 +154,7 @@
 // pointer fields of mjModel
 // XMJV means that the field is required to construct mjvScene
 // (by default we define XMJV to be the same as X)
-#define MJMODEL_POINTERS                                                     \
+#define MJMODEL_POINTERS                                                        \
     X   ( mjtNum,  qpos0,                 nq,            1                    ) \
     X   ( mjtNum,  qpos_spring,           nq,            1                    ) \
     XMJV( int,     body_parentid,         nbody,         1                    ) \
@@ -164,6 +165,7 @@
     XMJV( int,     body_jntadr,           nbody,         1                    ) \
     X   ( int,     body_dofnum,           nbody,         1                    ) \
     X   ( int,     body_dofadr,           nbody,         1                    ) \
+    X   ( int,     body_treeid,           nbody,         1                    ) \
     XMJV( int,     body_geomnum,          nbody,         1                    ) \
     XMJV( int,     body_geomadr,          nbody,         1                    ) \
     X   ( mjtByte, body_simple,           nbody,         1                    ) \
@@ -204,6 +206,7 @@
     X   ( int,     dof_bodyid,            nv,            1                    ) \
     X   ( int,     dof_jntid,             nv,            1                    ) \
     X   ( int,     dof_parentid,          nv,            1                    ) \
+    X   ( int,     dof_treeid,            nv,            1                    ) \
     X   ( int,     dof_Madr,              nv,            1                    ) \
     X   ( int,     dof_simplenum,         nv,            1                    ) \
     X   ( mjtNum,  dof_solref,            nv,            mjNREF               ) \
@@ -555,35 +558,35 @@
 #define MJ_D(n) n
 
 // array of contacts
-#define MJDATA_ARENA_POINTERS_CONTACT      \
+#define MJDATA_ARENA_POINTERS_CONTACT             \
     X( mjContact, contact, MJ_D(ncon), 1 )
 
 // array fields of mjData that are used in the primal problem
-#define MJDATA_ARENA_POINTERS_PRIMAL              \
+#define MJDATA_ARENA_POINTERS_PRIMAL               \
-  X(int,      efc_type,          MJ_D(nefc),  1)  \
+  X( int,      efc_type,          MJ_D(nefc),  1)  \
-  X(int,      efc_id,            MJ_D(nefc),  1)  \
+  X( int,      efc_id,            MJ_D(nefc),  1)  \
-  X(int,      efc_J_rownnz,      MJ_D(nefc),  1)  \
+  X( int,      efc_J_rownnz,      MJ_D(nefc),  1)  \
-  X(int,      efc_J_rowadr,      MJ_D(nefc),  1)  \
+  X( int,      efc_J_rowadr,      MJ_D(nefc),  1)  \
-  X(int,      efc_J_rowsuper,    MJ_D(nefc),  1)  \
+  X( int,      efc_J_rowsuper,    MJ_D(nefc),  1)  \
-  X(int,      efc_J_colind,      MJ_D(nnzJ),  1)  \
+  X( int,      efc_J_colind,      MJ_D(nnzJ),  1)  \
-  X(int,      efc_JT_rownnz,     MJ_M(nv),    1)  \
+  X( int,      efc_JT_rownnz,     MJ_M(nv),    1)  \
-  X(int,      efc_JT_rowadr,     MJ_M(nv),    1)  \
+  X( int,      efc_JT_rowadr,     MJ_M(nv),    1)  \
-  X(int,      efc_JT_rowsuper,   MJ_M(nv),    1)  \
+  X( int,      efc_JT_rowsuper,   MJ_M(nv),    1)  \
-  X(int,      efc_JT_colind,     MJ_D(nnzJ),  1)  \
+  X( int,      efc_JT_colind,     MJ_D(nnzJ),  1)  \
-  X(mjtNum,   efc_J,             MJ_D(nnzJ),  1)  \
+  X( mjtNum,   efc_J,             MJ_D(nnzJ),  1)  \
-  X(mjtNum,   efc_JT,            MJ_D(nnzJ),  1)  \
+  X( mjtNum,   efc_JT,            MJ_D(nnzJ),  1)  \
-  X(mjtNum,   efc_pos,           MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_pos,           MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_margin,        MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_margin,        MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_frictionloss,  MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_frictionloss,  MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_diagApprox,    MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_diagApprox,    MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_KBIP,          MJ_D(nefc),  4)  \
+  X( mjtNum,   efc_KBIP,          MJ_D(nefc),  4)  \
-  X(mjtNum,   efc_D,             MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_D,             MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_R,             MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_R,             MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_vel,           MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_vel,           MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_aref,          MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_aref,          MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_b,             MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_b,             MJ_D(nefc),  1)  \
-  X(mjtNum,   efc_force,         MJ_D(nefc),  1)  \
+  X( mjtNum,   efc_force,         MJ_D(nefc),  1)  \
-  X(int,      efc_state,         MJ_D(nefc),  1)
+  X( int,      efc_state,         MJ_D(nefc),  1)
 
 // array fields of mjData that are used in the dual problem
 #define MJDATA_ARENA_POINTERS_DUAL                            \
@@ -592,11 +595,21 @@
     X( int,       efc_AR_colind,     MJ_D(nefc), MJ_D(nefc) ) \
     X( mjtNum,    efc_AR,            MJ_D(nefc), MJ_D(nefc) )
 
+// array fields of mjData that are used for constraint islands
+#define MJDATA_ARENA_POINTERS_ISLAND                \
+    X( int,   island_dofadr,     MJ_D(nisland), 1 ) \
+    X( int,   island_efcadr,     MJ_D(nisland), 1 ) \
+    X( int,   dof_island,        MJ_M(nv),      1 ) \
+    X( int,   dof_islandnext,    MJ_M(nv),      1 ) \
+    X( int,   efc_island,        MJ_D(nefc),    1 ) \
+    X( int,   efc_islandnext,    MJ_D(nefc),    1 )
+
 // array fields of mjData that live in d->arena
 #define MJDATA_ARENA_POINTERS              \
     MJDATA_ARENA_POINTERS_CONTACT          \
     MJDATA_ARENA_POINTERS_PRIMAL           \
-    MJDATA_ARENA_POINTERS_DUAL
+    MJDATA_ARENA_POINTERS_DUAL             \
+    MJDATA_ARENA_POINTERS_ISLAND
 
 
 // scalar fields of mjData
@@ -621,6 +634,7 @@
     X( int,       nefc               ) \
     X( int,       nnzJ               ) \
     X( int,       ncon               ) \
+    X( int,       nisland            ) \
     X( mjtNum,    time               )
 
 

include/mujoco/mujoco.h

@@ -343,6 +343,9 @@ MJAPI void mj_collision(const mjModel* m, mjData* d);
 // Construct constraints.
 MJAPI void mj_makeConstraint(const mjModel* m, mjData* d);
 
+// Find constraint islands.
+MJAPI void mj_island(const mjModel* m, mjData* d);
+
 // Compute inverse constraint inertia efc_AR.
 MJAPI void mj_projectConstraint(const mjModel* m, mjData* d);
 

introspect/enums.py

@@ -54,7 +54,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjENBL_FWDINV', 4),
              ('mjENBL_SENSORNOISE', 8),
              ('mjENBL_MULTICCD', 16),
-             ('mjNENABLE', 5),
+             ('mjENBL_ISLAND', 32),
+             ('mjNENABLE', 6),
          ]),
      )),
     ('mjtJoint',
@@ -501,7 +502,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjLABEL_SELPNT', 12),
              ('mjLABEL_CONTACTPOINT', 13),
              ('mjLABEL_CONTACTFORCE', 14),
-             ('mjNLABEL', 15),
+             ('mjLABEL_ISLAND', 15),
+             ('mjNLABEL', 16),
          ]),
      )),
     ('mjtFrame',
@@ -540,17 +542,18 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjVIS_PERTFORCE', 12),
              ('mjVIS_PERTOBJ', 13),
              ('mjVIS_CONTACTPOINT', 14),
-             ('mjVIS_CONTACTFORCE', 15),
+             ('mjVIS_ISLAND', 15),
-             ('mjVIS_CONTACTSPLIT', 16),
+             ('mjVIS_CONTACTFORCE', 16),
-             ('mjVIS_TRANSPARENT', 17),
+             ('mjVIS_CONTACTSPLIT', 17),
-             ('mjVIS_AUTOCONNECT', 18),
+             ('mjVIS_TRANSPARENT', 18),
-             ('mjVIS_COM', 19),
+             ('mjVIS_AUTOCONNECT', 19),
-             ('mjVIS_SELECT', 20),
+             ('mjVIS_COM', 20),
-             ('mjVIS_STATIC', 21),
+             ('mjVIS_SELECT', 21),
-             ('mjVIS_SKIN', 22),
+             ('mjVIS_STATIC', 22),
-             ('mjVIS_MIDPHASE', 23),
+             ('mjVIS_SKIN', 23),
-             ('mjVIS_MESHBVH', 24),
+             ('mjVIS_MIDPHASE', 24),
-             ('mjNVISFLAG', 25),
+             ('mjVIS_MESHBVH', 25),
+             ('mjNVISFLAG', 26),
          ]),
      )),
     ('mjtRndFlag',

introspect/enums_test.py

@@ -43,7 +43,8 @@ class EnumsTest(absltest.TestCase):
                                           ('mjENBL_FWDINV', 1<<2),
                                           ('mjENBL_SENSORNOISE', 1<<3),
                                           ('mjENBL_MULTICCD', 1<<4),
-                                          ('mjNENABLE', 5)))
+                                          ('mjENBL_ISLAND', 1<<5),
+                                          ('mjNENABLE', 6)))
 
   # values mostly increment by one with occasional overrides
   def test_mjtGeom(self):  # pylint: disable=invalid-name

introspect/functions.py

@@ -1706,6 +1706,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Construct constraints.',
      )),
+    ('mj_island',
+     FunctionDecl(
+         name='mj_island',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='m',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjModel', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData'),
+                 ),
+             ),
+         ),
+         doc='Find constraint islands.',
+     )),
     ('mj_projectConstraint',
      FunctionDecl(
          name='mj_projectConstraint',

introspect/structs.py

@@ -813,7 +813,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
              StructFieldDecl(
                  name='nbvh',
                  type=ValueType(name='int'),
-                 doc='number of total bounding volumes in all bodies',
+                 doc='number of bounding volumes in all bodies',
              ),
              StructFieldDecl(
                  name='njnt',
@@ -1070,6 +1070,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='int'),
                  doc='number of non-zeros in sparse body-dof matrix',
              ),
+             StructFieldDecl(
+                 name='ntree',
+                 type=ValueType(name='int'),
+                 doc='number of kinematic trees under world body',
+             ),
              StructFieldDecl(
                  name='nemax',
                  type=ValueType(name='int'),
@@ -1103,7 +1108,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
              StructFieldDecl(
                  name='npluginstate',
                  type=ValueType(name='int'),
-                 doc='number of fields in the plugin state vector',
+                 doc='number of fields in plugin state vector',
              ),
              StructFieldDecl(
                  name='nbuffer',
@@ -1202,6 +1207,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='start addr of dofs; -1: no dofs          (nbody x 1)',
              ),
+             StructFieldDecl(
+                 name='body_treeid',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc="id of body's kinematic tree; -1: static  (nbody x 1)",
+             ),
              StructFieldDecl(
                  name='body_geomnum',
                  type=PointerType(
@@ -1482,6 +1494,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc="id of dof's parent; -1: none             (nv x 1)",
              ),
+             StructFieldDecl(
+                 name='dof_treeid',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc="id of dof's kinematic tree               (nv x 1)",
+             ),
              StructFieldDecl(
                  name='dof_Madr',
                  type=PointerType(
@@ -3578,6 +3597,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='int'),
                  doc='number of detected contacts',
              ),
+             StructFieldDecl(
+                 name='nisland',
+                 type=ValueType(name='int'),
+                 doc='number of detected constraint islands',
+             ),
              StructFieldDecl(
                  name='time',
                  type=ValueType(name='mjtNum'),
@@ -4291,6 +4315,48 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='inverse constraint mass                          (nefc x 1)',  # pylint: disable=line-too-long
              ),
+             StructFieldDecl(
+                 name='island_dofadr',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='address of first dof in island                   (nisland x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='island_efcadr',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='address of first constraint in island            (nisland x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='dof_island',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='island id of this dof; -1: none                  (nv x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='dof_islandnext',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='address of next dof in island; -1: last or none  (nv x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='efc_island',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='island id of this constraint                     (nefc x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='efc_islandnext',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='address of next constraint in island; -1: last   (nefc x 1)',  # pylint: disable=line-too-long
+             ),
              StructFieldDecl(
                  name='efc_AR_rownnz',
                  type=PointerType(
@@ -4808,7 +4874,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  name='flags',
                  type=ArrayType(
                      inner_type=ValueType(name='mjtByte'),
-                     extents=(25,),
+                     extents=(26,),
                  ),
                  doc='visualization flags (indexed by mjtVisFlag)',
              ),
@@ -6195,6 +6261,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                              type=ValueType(name='int'),
                              doc='',
                          ),
+                         StructFieldDecl(
+                             name='nisland',
+                             type=ValueType(name='int'),
+                             doc='',
+                         ),
                          StructFieldDecl(
                              name='time',
                              type=ValueType(name='mjtNum'),
@@ -6375,6 +6446,27 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                              ),
                              doc='',
                          ),
+                         StructFieldDecl(
+                             name='island_dofadr',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
+                         StructFieldDecl(
+                             name='dof_island',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
+                         StructFieldDecl(
+                             name='efc_island',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
                          StructFieldDecl(
                              name='contact',
                              type=PointerType(

python/mujoco/bindings_test.py

@@ -801,7 +801,7 @@ Euler integrator, semi-implicit in velocity.
     self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1<<1)
     self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1<<2)
     self.assertEqual(mujoco.mjtEnableBit.mjENBL_SENSORNOISE, 1<<3)
-    self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 5)
+    self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6)
     self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0)
     self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1)
     self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2)

python/mujoco/functions.cc

@@ -253,6 +253,7 @@ PYBIND11_MODULE(_functions, pymodule) {
   Def<traits::mj_rnePostConstraint>(pymodule);
   Def<traits::mj_collision>(pymodule);
   Def<traits::mj_makeConstraint>(pymodule);
+  Def<traits::mj_island>(pymodule);
   Def<traits::mj_projectConstraint>(pymodule);
   Def<traits::mj_referenceConstraint>(pymodule);
   Def<traits::mj_constraintUpdate>(

python/mujoco/structs.cc

@@ -710,6 +710,7 @@ void MjDataWrapper::Serialize(std::ostream& output) const {
   X(nnzJ);
   X(nefc);
   X(ncon);
+  X(nisland);
   X(time);
   X(energy);
 #undef X
@@ -734,10 +735,12 @@ void MjDataWrapper::Serialize(std::ostream& output) const {
 
     MJDATA_ARENA_POINTERS_CONTACT
     MJDATA_ARENA_POINTERS_PRIMAL
-
     if (this->metadata_.is_dual) {
       MJDATA_ARENA_POINTERS_DUAL
     }
+    if (this->ptr_->nisland) {
+      MJDATA_ARENA_POINTERS_ISLAND
+    }
 #undef MJ_M
 #define MJ_M(x) x
 #undef MJ_D
@@ -799,6 +802,7 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) {
   X(nnzJ);
   X(nefc);
   X(ncon);
+  X(nisland);
   X(time);
   X(energy);
 #undef X
@@ -825,10 +829,12 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) {
 
     MJDATA_ARENA_POINTERS_CONTACT
     MJDATA_ARENA_POINTERS_PRIMAL
-
     if (metadata.is_dual) {
       MJDATA_ARENA_POINTERS_DUAL
     }
+    if (d->nisland) {
+      MJDATA_ARENA_POINTERS_ISLAND
+    }
 #undef MJ_M
 #define MJ_M(x) x
 #undef MJ_D
@@ -1830,6 +1836,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
 
   MJDATA_ARENA_POINTERS_PRIMAL
   MJDATA_ARENA_POINTERS_DUAL
+  MJDATA_ARENA_POINTERS_ISLAND
 
 #undef MJ_M
 #define MJ_M(x) (x)

simulate/simulate.cc

@@ -541,6 +541,13 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
       mju::strcat_arr(content, tmp);
       mju::strcat_arr(title, "\nFwdInv");
     }
+
+    // add islands if enabled
+    if (mjENABLED(mjENBL_ISLAND)) {
+      mju::sprintf_arr(tmp, "\n%d", d->nisland);
+      mju::strcat_arr(content, tmp);
+      mju::strcat_arr(title, "\nIslands");
+    }
   }
 }
 
@@ -667,7 +674,7 @@ void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) {
       2,
       &(sim->opt.label),
       "None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n"
-      "Actuator\nConstraint\nSkin\nSelection\nSel Pnt\nContact\nForce"
+      "Actuator\nConstraint\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland"
     },
     {
       mjITEM_SELECT,

src/engine/CMakeLists.txt

@@ -37,6 +37,8 @@ set(MUJOCO_ENGINE_SRCS
     engine_forward.h
     engine_inverse.c
     engine_inverse.h
+    engine_island.c
+    engine_island.h
     engine_io.c
     engine_io.h
     engine_macro.h

src/engine/engine_core_constraint.c

@@ -46,15 +46,18 @@
 
 //-------------------------- utility functions -----------------------------------------------------
 
-// internal function for clearing arena pointers for efc_ arrays in mjData
+// clear arena pointers in mjData
 static inline void clearEfc(mjData* d) {
 #define X(type, name, nr, nc) d->name = NULL;
   MJDATA_ARENA_POINTERS
 #undef X
   d->nefc = 0;
-  d->contact = d->arena;
+  d->nisland = 0;
+  d->contact = (mjContact*) d->arena;
 }
 
+
+
 // determine type of friction cone
 int mj_isPyramidal(const mjModel* m) {
   if (m->opt.cone == mjCONE_PYRAMIDAL) {
@@ -1606,16 +1609,15 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
 
   // precount sizes for constraint Jacobian matrices
   int *nnz = mj_isSparse(m) ? &(d->nnzJ) : NULL;
-
   int ne_allocated = mj_ne(m, d, nnz);
   int nf_allocated = mj_nf(m, d, nnz);
-
   int nefc_allocated = ne_allocated + nf_allocated + mj_nl(m, d, nnz) + mj_nc(m, d, nnz);
   if (!mj_isSparse(m)) {
     d->nnzJ = nefc_allocated * m->nv;
   }
   d->nefc = nefc_allocated;
 
+  // ========== begin arena allocation
 #undef MJ_M
 #define MJ_M(n) m->n
 #undef MJ_D
@@ -1623,6 +1625,8 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
 
   // move arena pointer to end of contact array
   d->parena = d->ncon * sizeof(mjContact);
+
+  // poison remaining memory
 #ifdef ADDRESS_SANITIZER
   ASAN_POISON_MEMORY_REGION(
     (char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena);
@@ -1648,6 +1652,7 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
 #define MJ_M(n) n
 #undef MJ_D
 #define MJ_D(n) n
+  // ========== end arena allocation
 
   // reset nefc for the instantiation functions,
   // and instantiate all elements of Jacobian
@@ -2103,129 +2108,3 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
     *cost = s;
   }
 }
-
-
-
-//---------------------------- constraint islands --------------------------------------------------
-
-// comparison function for lexicographic edge sorting
-quicksortfunc(edgecompare, context, edge0, edge1) {
-  int* e0 = (int*)edge0;
-  int* e1 = (int*)edge1;
-  int v00 = e0[0];
-  int v10 = e1[0];
-
-  if (v00 < v10) {
-    return -1;
-  }
-
-  if (v00 == v10) {
-    int v01 = e0[1];
-    int v11 = e1[1];
-
-    if (v01 < v11) {
-      return -1;
-    }
-
-    if (v01 == v11) {
-      return 0;
-    }
-  }
-
-  return 1;
-}
-
-
-
-// construct sparse matrix from unsorted edge array, return number of nonzeros
-int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr) {
-  if (!ne) {
-    return 0;
-  }
-
-  // sort edges
-  mjQUICKSORT(edge, ne, 2*sizeof(int), edgecompare, NULL);
-
-  // construct sparse
-  int nnz = 0;  // number of nonzeros
-  int e = 0;    // current edge
-  for (int r=0; r < nr; r++) {
-    // init row
-    rownnz[r] = 0;
-    rowadr[r] = nnz;
-
-    // copy values while making unique and checking indices
-    while (e < ne && edge[2*e] == r) {
-      int v0 = edge[2*e];
-      int v1 = edge[2*e + 1];
-
-      // skip if duplicate
-      if (rownnz[r] && v0 == edge[2*e - 2] && v1 == edge[2*e - 1]) {
-        e++;
-        continue;
-      }
-
-      // check for invalid indices
-      if (v0 < 0 || v0 >= nr) mju_error("invalid row index %d in edge %d", v0, e);
-      if (v1 < 0 || v1 >= nr) mju_error("invalid column index %d in edge %d", v1, e);
-
-      // copy column index, increment nnz, e, rownnz
-      colind[nnz++] = edge[2*(e++) + 1];
-      rownnz[r]++;
-    }
-  }
-
-  return nnz;
-}
-
-
-// find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
-//   arguments:
-//     island  (nr)   - island index assigned to vertex, -1 if vertex has no edges
-//     nr             - number of rows/columns of adjacency matrix
-//     rownnz  (nr)   - matrix row nonzeros
-//     rowadr  (nr)   - matrix row addresses
-//     colind  (nnz)  - matrix column indices
-//     stack   (nnz)  - stack space
-//   returns number of islands
-int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
-                 int* stack) {
-  // initialize island count, set ids to -1
-  int nisland = 0;
-  for (int i=0; i < nr; i++) island[i] = -1;
-
-  // iterate over vertices, discover islands
-  for (int i=0; i < nr; i++) {
-    // vertex already in island or singleton with no edges: skip
-    if (island[i] != -1 || !rownnz[i]) {
-      continue;
-    }
-
-    // push i onto stack
-    int nstack = 0;
-    stack[nstack++] = i;
-
-    // DFS traversal of island
-    while (nstack) {
-      // pop v from stack
-      int v = stack[--nstack];
-
-      // if v is already assigned, continue
-      if (island[v] != -1) {
-        continue;
-      }
-
-      // assign v to current island
-      island[v] = nisland;
-
-      // push adjacent vertices onto stack
-      memcpy(stack + nstack, colind + rowadr[v], rownnz[v]*sizeof(int));
-      nstack += rownnz[v];
-    }
-
-    // island is filled: increment nisland
-    nisland++;
-  }
-
-  return nisland;
-}

src/engine/engine_core_constraint.h

@@ -18,6 +18,7 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjxmacro.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -115,12 +116,6 @@ MJAPI void mj_referenceConstraint(const mjModel* m, mjData* d);
 MJAPI void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
                                mjtNum cost[1], int flg_coneHessian);
 
-// construct sparse matrix from unsorted edge array, return number of nonzeros
-MJAPI int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr);
-
-MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
-                       int* scratch);
-
 #ifdef __cplusplus
 }
 #endif

src/engine/engine_forward.c

@@ -27,6 +27,7 @@
 #include "engine/engine_core_smooth.h"
 #include "engine/engine_derivative.h"
 #include "engine/engine_inverse.h"
+#include "engine/engine_island.h"
 #include "engine/engine_io.h"
 #include "engine/engine_macro.h"
 #include "engine/engine_passive.h"
@@ -114,6 +115,9 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
 
   TM_RESTART;
   mj_makeConstraint(m, d);
+  if (mjENABLED(mjENBL_ISLAND)) {
+    mj_island(m, d);
+  }
   mj_transmission(m, d);
   TM_END(mjTIMER_POS_MAKE);
 

src/engine/engine_io.c

@@ -1321,6 +1321,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   d->nefc = 0;
   d->nnzJ = 0;
   d->ncon = 0;
+  d->nisland = 0;
 
   // clear global properties
   d->time = 0;

src/engine/engine_island.c

@@ -0,0 +1,521 @@
+// Copyright 2023 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.
+
+#include "engine/engine_island.h"
+
+#include <stdio.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <mujoco/mjdata.h>
+#include <mujoco/mjmacro.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mjxmacro.h>
+#include "engine/engine_core_constraint.h"
+#include "engine/engine_crossplatform.h"
+#include "engine/engine_io.h"
+#include "engine/engine_support.h"
+#include "engine/engine_util_errmem.h"
+
+#ifdef MEMORY_SANITIZER
+  #include <sanitizer/msan_interface.h>
+#endif
+
+// clear island-related arena pointers in mjData
+static void clearIsland(mjData* d, size_t parena) {
+#define X(type, name, nr, nc) d->name = NULL;
+  MJDATA_ARENA_POINTERS_ISLAND
+#undef X
+  d->nefc = 0;
+  d->nisland = 0;
+  d->parena = parena;
+
+  // poison remaining memory
+#ifdef ADDRESS_SANITIZER
+  ASAN_POISON_MEMORY_REGION(
+    (char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena);
+#endif
+}
+
+// comparison function for lexicographic edge sorting
+quicksortfunc(edgecompare, context, edge0, edge1) {
+  int* e0 = (int*)edge0;
+  int* e1 = (int*)edge1;
+  int v00 = e0[0];
+  int v10 = e1[0];
+
+  if (v00 < v10) {
+    return -1;
+  }
+
+  if (v00 == v10) {
+    int v01 = e0[1];
+    int v11 = e1[1];
+
+    if (v01 < v11) {
+      return -1;
+    }
+
+    if (v01 == v11) {
+      return 0;
+    }
+  }
+
+  return 1;
+}
+
+
+
+// construct sparse matrix from non-unique, unsorted edge array, return number of nonzeros
+int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr) {
+  if (!ne) {
+    return 0;
+  }
+
+  // sort edges
+  mjQUICKSORT(edge, ne, 2*sizeof(int), edgecompare, NULL);
+
+  // construct sparse
+  int nnz = 0;  // number of nonzeros
+  int e = 0;    // current edge
+  for (int r=0; r < nr; r++) {
+    // init row
+    rownnz[r] = 0;
+    rowadr[r] = nnz;
+
+    // copy values while making unique and checking indices
+    while (e < ne && edge[2*e] == r) {
+      int v0 = edge[2*e];
+      int v1 = edge[2*e + 1];
+
+      // skip if duplicate
+      if (rownnz[r] && v0 == edge[2*e - 2] && v1 == edge[2*e - 1]) {
+        e++;
+        continue;
+      }
+
+      // check for invalid indices
+      if (v0 < 0 || v0 >= nr) mjERROR("invalid row index %d in edge %d", v0, e);
+      if (v1 < 0 || v1 >= nr) mjERROR("invalid column index %d in edge %d", v1, e);
+
+      // copy column index, increment nnz, e, rownnz
+      colind[nnz++] = edge[2*(e++) + 1];
+      rownnz[r]++;
+    }
+  }
+
+  return nnz;
+}
+
+
+// find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
+//   arguments:
+//     island  (nr)   - island index assigned to vertex, -1 if vertex has no edges
+//     nr             - number of rows/columns of adjacency matrix
+//     rownnz  (nr)   - matrix row nonzeros
+//     rowadr  (nr)   - matrix row addresses
+//     colind  (nnz)  - matrix column indices
+//     stack   (nnz)  - stack space
+//   returns number of islands
+int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
+                 int* stack) {
+  // initialize island count, set ids to -1
+  int nisland = 0;
+  for (int i=0; i < nr; i++) island[i] = -1;
+
+  // iterate over vertices, discover islands
+  for (int i=0; i < nr; i++) {
+    // vertex already in island or singleton with no edges: skip
+    if (island[i] != -1 || !rownnz[i]) {
+      continue;
+    }
+
+    // push i onto stack
+    int nstack = 0;
+    stack[nstack++] = i;
+
+    // DFS traversal of island
+    while (nstack) {
+      // pop v from stack
+      int v = stack[--nstack];
+
+      // if v is already assigned, continue
+      if (island[v] != -1) {
+        continue;
+      }
+
+      // assign v to current island
+      island[v] = nisland;
+
+      // push adjacent vertices onto stack
+      memcpy(stack + nstack, colind + rowadr[v], rownnz[v]*sizeof(int));
+      nstack += rownnz[v];
+    }
+
+    // island is filled: increment nisland
+    nisland++;
+  }
+
+  return nisland;
+}
+
+
+
+// return upper bound on number of tree-tree edges
+static int countMaxEdge(const mjModel* m, const mjData* d) {
+  int nedge_max = 0;
+  nedge_max += 2*d->ncon;  // contact: 2 edges
+  nedge_max += 2*d->ne;    // equality: 2 edges
+  nedge_max += d->nf;      // joint friction: 1 edge (always within same tree)
+
+  // tendon limits and friction add up to tendon_num edges
+  for (int i=0; i < m->ntendon; i++) {
+    if (m->tendon_frictionloss[i]) {
+      nedge_max += m->tendon_num[i];
+    }
+    if (m->tendon_limited[i]) {
+      nedge_max += m->tendon_num[i];
+    }
+  }
+
+  return nedge_max;
+}
+
+
+
+// add tree-tree edge array: check size, add flipped edge if non-self
+static int addEdge(int* edge, int nedge, int tree1, int tree2, int nedge_max) {
+  // handle the static tree
+  if (tree1 == -1 && tree2 == -1) {
+    mjERROR("self-edge of the static tree");  // SHOULD NOT OCCUR
+    return 0;
+  }
+  if (tree1 == -1) tree1 = tree2;
+  if (tree2 == -1) tree2 = tree1;
+
+  // previous edge
+  int p1 = nedge ? edge[2*nedge - 2] : -1;
+  int p2 = nedge ? edge[2*nedge - 1] : -1;
+
+  // === self edge
+  if (tree1 == tree2) {
+    // same as previous edge, return
+    if (nedge && tree1 == p1 && tree1 == p2) {
+      return nedge;
+    }
+
+    // check size
+    if (nedge >= nedge_max) {
+      mjERROR("edge array too small");
+      return 0;
+    }
+
+    // add tree1-tree1 self-edge
+    edge[2*nedge + 0] = tree1;
+    edge[2*nedge + 1] = tree1;
+    return nedge + 1;
+  }
+
+  // === non-self edge
+  if (nedge && ((tree1 == p1 && tree2 == p2) || (tree1 == p2 && tree2 == p1))) {
+    // same as previous edge, return
+    return nedge;
+  }
+
+  // check size
+  if (nedge + 2 > nedge_max) {
+    mjERROR("edge array too small");
+    return 0;
+  }
+
+  // add tree1-tree2 and tree2-tree1
+  edge[2*nedge + 0] = tree1;
+  edge[2*nedge + 1] = tree2;
+  edge[2*nedge + 2] = tree2;
+  edge[2*nedge + 3] = tree1;
+  return nedge + 2;
+}
+
+
+
+// return id of next tree in Jacobian row i that is different from tree, -1 if not found
+//   write the index of the found tree to *index if given
+//   start search from *index if given, otherwise 0
+//   if J is (dense/sparse) *index is the (column/nonzro) index, respectively
+static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *index) {
+  int tree_next = -1;
+  int j0 = index ? *index : 0;  // start searching at *index if given, otherwise 0
+  int j;  // loop variable, saved to *index
+
+  // sparse
+  if (mj_isSparse(m)) {
+    int rownnz = d->efc_J_rownnz[i];
+    int* colind = d->efc_J_colind + d->efc_J_rowadr[i];
+
+    // loop over remaining nonzeros, look for different tree
+    for (j=j0; j < rownnz; j++) {
+      int tree_j = m->dof_treeid[colind[j]];
+      if (tree_j != tree) {
+        // found different tree
+        tree_next = tree_j;
+        break;
+      }
+    }
+  }
+
+  // dense
+  else {
+    int nv = m->nv;
+
+    // scan row, look for different tree
+    for (j=j0; j < nv; j++) {
+      if (d->efc_J[nv*i + j]) {
+        int tree_j = m->dof_treeid[j];
+        if (tree_j != tree) {
+          // found different tree
+          tree_next = tree_j;
+          break;
+        }
+      }
+    }
+  }
+
+  // save last index
+  if (index) *index = j;
+
+  return tree_next;
+}
+
+
+
+// find tree-tree edges
+static int findEdges(const mjModel* m, const mjData* d, int* edge, int nedge_max) {
+  int nefc = d->nefc;
+  int efc_type = -1;
+  int efc_id = -1;
+  int tree1, tree2;
+
+  int nedge = 0;
+  for (int i=0; i < nefc; i++) {
+    // row i is still in the same constraint: skip
+    if (efc_type == d->efc_type[i] && efc_id == d->efc_id[i]) {
+      continue;
+    }
+    efc_type = d->efc_type[i];
+    efc_id = d->efc_id[i];
+
+    // ==== fast handling of special cases
+
+    // joint friction
+    if (efc_type == mjCNSTR_FRICTION_DOF) {
+      tree1 = m->dof_treeid[efc_id];
+      nedge = addEdge(edge, nedge, tree1, tree1, nedge_max);
+      continue;
+    }
+
+    // joint limit
+    if (efc_type == mjCNSTR_LIMIT_JOINT) {
+      tree1 = m->dof_treeid[m->jnt_dofadr[efc_id]];
+      nedge = addEdge(edge, nedge, tree1, tree1, nedge_max);
+      continue;
+    }
+
+    // contact
+    if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
+        efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
+        efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
+      tree1 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
+      tree2 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
+      nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
+      continue;
+    }
+
+    // connect or weld constraints
+    if (efc_type == mjCNSTR_EQUALITY) {
+      mjtEq eq_type = m->eq_type[efc_id];
+      if (eq_type == mjEQ_CONNECT || eq_type == mjEQ_WELD) {
+        tree1 = m->body_treeid[m->eq_obj1id[efc_id]];
+        tree2 = m->body_treeid[m->eq_obj2id[efc_id]];
+        nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
+        continue;
+      }
+    }
+
+    // ==== generic case: scan Jacobian
+    int index = 0;
+    tree1 = treeNext(m, d, -1, i, &index);
+    tree2 = treeNext(m, d, tree1, i, &index);
+
+    if (tree2 == -1) {
+      // 1 tree found: add self-edge
+      nedge = addEdge(edge, nedge, tree1, tree1, nedge_max);
+    } else {
+      // 2 trees found: add edge, keep scanning and adding until no more trees
+      nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
+      int tree3 = treeNext(m, d, tree2, i, &index);
+      while (tree3 > -1 && tree3 != tree2) {
+        tree1 = tree2;
+        tree2 = tree3;
+        nedge = addEdge(edge, nedge, tree1, tree2, nedge_max);
+        tree3 = treeNext(m, d, tree2, i, &index);
+      }
+    }
+  }
+
+  return nedge;
+}
+
+// discover islands:
+//   nisland, island_dofadr, dof_island, dof_islandnext, island_efcadr, efc_island, efc_islandnext
+void mj_island(const mjModel* m, mjData* d) {
+  int nv = m->nv, nefc = d->nefc, ntree=m->ntree;
+
+  // no constraints: quick return
+  if (!nefc) {
+    d->nisland = 0;
+    return;
+  }
+
+  mjMARKSTACK;
+
+  // allocate edge array
+  int nedge_max = countMaxEdge(m, d);
+  int* edge = mj_stackAllocInt(d, 2*nedge_max);
+
+  // find tree-tree edges
+  int nedge = findEdges(m, d, edge, nedge_max);
+
+  // TODO: b/295296178 - don't add flipped edges in findEdges, symmetrize in mj_edge2sparse instead
+
+  // construct adjacency matrix from edges
+  int* rownnz = mj_stackAllocInt(d, ntree);
+  int* rowadr = mj_stackAllocInt(d, ntree);
+  int* colind = mj_stackAllocInt(d, nedge);
+  int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, nedge, ntree);
+
+  // discover islands
+  int* tree_island = mj_stackAllocInt(d, ntree);  // id of island assigned to tree
+  int* stack = mj_stackAllocInt(d, nnz);
+  d->nisland = mj_floodFill(tree_island, ntree, rownnz, rowadr, colind, stack);
+
+  // ========== begin arena allocation of MJDATA_ARENA_POINTERS_ISLAND
+#undef MJ_M
+#define MJ_M(n) m->n
+#undef MJ_D
+#define MJ_D(n) d->n
+
+  size_t parena_old = d->parena;
+
+#define X(type, name, nr, nc)                                             \
+  d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \
+  if (!d->name) {                                                         \
+    mj_warning(d, mjWARN_CNSTRFULL, d->nstack * sizeof(mjtNum));          \
+    clearIsland(d, parena_old);                                           \
+    mjFREESTACK;                                                          \
+    return;                                                               \
+  }
+
+  MJDATA_ARENA_POINTERS_ISLAND
+
+#undef X
+
+#undef MJ_M
+#define MJ_M(n) n
+#undef MJ_D
+#define MJ_D(n) n
+  // ========== end arena allocation
+
+  // prepare island_last: id of last element in each island
+  int* island_last = mj_stackAllocInt(d, d->nisland);
+  for (int i=0; i < d->nisland; i++) {
+    island_last[i] = -1;
+  }
+
+  // compute island_dofadr, dof_island, dof_islandnext
+  int nisland_found = 0;
+  for (int i=0; i < nv; i++) {
+    // dof_island
+    int island = tree_island[m->dof_treeid[i]];;
+    d->dof_island[i] = island;
+
+    // island_dofadr, dof_islandnext
+    if (island == -1) {
+      // dof is not in any island (unconstrained)
+      d->dof_islandnext[i] = -1;
+      continue;
+    } else {
+      int last = island_last[island];
+      if (last == -1) {
+        // first dof: set island_dofadr, increment nisland_found
+        d->island_dofadr[island] = i;
+        nisland_found++;
+      } else {
+        // subsequent dof: point last dof to i
+        d->dof_islandnext[last] = i;
+      }
+      island_last[island] = i;
+    }
+  }
+
+  // sanity check, SHOULD NOT OCCUR
+  if (nisland_found != d->nisland) {
+    mjERROR("not all islands assigned to dofs");
+  }
+
+  // finalize dof_islandnext: mark last dof in each island with -1
+  for (int i=0; i < d->nisland; i++) {
+    d->dof_islandnext[island_last[i]] = -1;
+  }
+
+  // reset island_last
+  for (int i=0; i < d->nisland; i++) {
+    island_last[i] = -1;
+  }
+
+  // compute island_efcadr, efc_island, efc_islandnext
+  nisland_found = 0;
+  for (int i=0; i < nefc; i++) {
+    // efc_island
+    int island = tree_island[treeNext(m, d, -1, i, NULL)];
+    d->efc_island[i] = island;
+
+    // island_efcadr, efc_islandnext
+    if (island == -1) {
+      mjERROR("constraint %d not in any island", i);  // SHOULD NOT OCCUR
+    } else {
+      int last = island_last[island];
+      if (last == -1) {
+        // first constraint: set island_efcadr, increment nisland_found
+        d->island_efcadr[island] = i;
+        nisland_found++;
+      } else {
+        // subsequent constraint: point last constraint to i
+        d->efc_islandnext[last] = i;
+      }
+      island_last[island] = i;
+    }
+  }
+
+  // sanity check, SHOULD NOT OCCUR
+  if (nisland_found != d->nisland) {
+    mjERROR("not all islands assigned to constraints");
+  }
+
+  // finalize efc_islandnext: mark last constraint in each island with -1
+  for (int i=0; i < d->nisland; i++) {
+    d->efc_islandnext[island_last[i]] = -1;
+  }
+
+  mjFREESTACK;
+}

src/engine/engine_island.h

@@ -0,0 +1,46 @@
+// Copyright 2023 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.
+
+#ifndef MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_
+#define MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_
+
+#include <mujoco/mjdata.h>
+#include <mujoco/mjexport.h>
+#include <mujoco/mjmodel.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+//-------------------------- utility functions -----------------------------------------------------
+
+// construct sparse matrix from non-unique, unsorted edge array, return number of nonzeros
+MJAPI int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr);
+
+// find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
+MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
+                       int* scratch);
+
+//-------------------------- top-level API for island construction ---------------------------------
+
+// discover islands:
+//   nisland, island_dofadr, dof_island, dof_islandnext, island_efcadr, efc_island, efc_islandnext
+MJAPI void mj_island(const mjModel* m, mjData* d);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_

src/engine/engine_print.c

@@ -507,10 +507,11 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
     fprintf(fp, " %s\n", m->names + m->name_tendonadr[i]);
     object_class = &m->ntendon;
     MJMODEL_POINTERS
-    fprintf(fp, "  path         \n");
+    fprintf(fp, "  path\n");
+    fprintf(fp, "    type  objid  prm\n");
     for (int j=0; j < m->tendon_num[i]; j++) {
       int k = m->tendon_adr[i]+j;
-      fprintf(fp, "    %d %d ", m->wrap_type[k], m->wrap_objid[k]);
+      fprintf(fp, "    %d     %d     ", m->wrap_type[k], m->wrap_objid[k]);
       fprintf(fp, float_format, m->wrap_prm[k]);
       fprintf(fp, "\n");
     }
@@ -1052,6 +1053,45 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
   printArray("CFRC_INT", m->nbody, 6, d->cfrc_int, fp, float_format);
   printArray("CFRC_EXT", m->nbody, 6, d->cfrc_ext, fp, float_format);
 
+  if (d->nisland) {
+    fprintf(fp, NAME_FORMAT, "ISLAND_DOFADR");
+    for (int i = 0; i < d->nisland; i++) {
+      fprintf(fp, " %d", d->island_dofadr[i]);
+    }
+    fprintf(fp, "\n\n");
+
+    fprintf(fp, NAME_FORMAT, "ISLAND_EFCADR");
+    for (int i = 0; i < d->nisland; i++) {
+      fprintf(fp, " %d", d->island_efcadr[i]);
+    }
+    fprintf(fp, "\n\n");
+
+    fprintf(fp, NAME_FORMAT, "DOF_ISLAND");
+    for (int i = 0; i < m->nv; i++) {
+      fprintf(fp, " %d", d->dof_island[i]);
+    }
+    fprintf(fp, "\n\n");
+
+    fprintf(fp, NAME_FORMAT, "DOF_ISLANDNEXT");
+    for (int i = 0; i < m->nv; i++) {
+      fprintf(fp, " %d", d->dof_islandnext[i]);
+    }
+    fprintf(fp, "\n\n");
+
+    fprintf(fp, NAME_FORMAT, "EFC_ISLAND");
+    for (int i = 0; i < d->nefc; i++) {
+      fprintf(fp, " %d", d->efc_island[i]);
+    }
+    fprintf(fp, "\n\n");
+
+    fprintf(fp, NAME_FORMAT, "EFC_ISLANDNEXT");
+    for (int i = 0; i < d->nefc; i++) {
+      fprintf(fp, " %d", d->efc_islandnext[i]);
+    }
+    fprintf(fp, "\n\n");
+
+  }
+
 #ifdef MEMORY_SANITIZER
   // restore poisoned status
   __msan_copy_shadow(d->buffer, shadow, d->nbuffer);

src/engine/engine_support.c

@@ -66,7 +66,8 @@ const char* mjENABLESTRING[mjNENABLE] = {
   "Energy",
   "Fwdinv",
   "Sensornoise",
-  "MultiCCD"
+  "MultiCCD",
+  "Island"
 };
 
 

src/engine/engine_vis_init.c

@@ -47,7 +47,8 @@ const char* mjLABELSTRING[mjNLABEL] = {
   "Selection",
   "SelPoint",
   "Contact",
-  "ContactForce"
+  "ContactForce",
+  "Island"
 };
 
 
@@ -81,6 +82,7 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
   {"Pertur&b Force",  "0", "B"},
   {"Perturb &Object", "1", "O"},
   {"&Contact Point",  "0", "C"},
+  {"Island",          "1", ""},   // TODO(b/295296178): turn off after islands are on by default.
   {"Contact &Force",  "0", "F"},
   {"Contact S&plit",  "0", "P"},
   {"&Transparent",    "0", "T"},

src/engine/engine_vis_state.c

@@ -173,6 +173,7 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData*
     memcpy(d->warning, scnstate->data.warning, sizeof(d->warning));
     d->nefc = scnstate->data.nefc;
     d->ncon = scnstate->data.ncon;
+    d->nisland = scnstate->data.nisland;
     d->time = scnstate->data.time;
 
   #define X(dtype, var, dim0, dim1)
@@ -183,6 +184,12 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData*
 
     d->contact = scnstate->data.contact;
     d->efc_force = scnstate->data.efc_force;
+
+    if (d->nisland) {
+      d->island_dofadr = scnstate->data.island_dofadr;
+      d->dof_island = scnstate->data.dof_island;
+      d->efc_island = scnstate->data.efc_island;
+    }
   }
 }
 
@@ -317,6 +324,15 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
       efc_address += dim;
     }
   }
+
+  // Copy island data.
+  scnstate->data.nisland = d->nisland;
+  if (d->nisland) {
+    memcpy(scnstate->data.island_dofadr, d->island_dofadr, sizeof(int) * d->nisland);
+    memcpy(scnstate->data.dof_island, d->dof_island, sizeof(int) * m->nv);
+    memcpy(scnstate->data.efc_island, d->efc_island, sizeof(int) * d->nefc);
+  }
+
 }
 
 

src/engine/engine_vis_visualize.c

@@ -84,7 +84,13 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) {
 // advance counter
 #define FINISH { scn->ngeom++; }
 
-
+// assign pseudo-random rgba to constraint island using Halton sequence
+static void islandColor(float rgba[4], int islanddofadr) {
+  rgba[0] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 2);
+  rgba[1] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 3);
+  rgba[2] = 0.1f + 0.8f*mju_Halton(islanddofadr + 1, 5);
+  rgba[3] = 1;
+}
 
 // add contact-related geoms in mjvObject
 static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
@@ -102,7 +108,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
     return;
   }
 
-  // loop over contacts included in impulse solver
+  // loop over contacts
   for (int i=0; i < d->ncon; i++) {
     // get pointer
     con = d->contact + i;
@@ -121,11 +127,21 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
       mju_n2f(thisgeom->pos, con->pos, 3);
       mju_n2f(thisgeom->mat, mat, 9);
 
-      // different colors for included and excluded contacts
+      int efc_adr = d->contact[i].efc_address;
-      if (d->contact[i].efc_address >= 0) {
+
-        f2f(thisgeom->rgba, m->vis.rgba.contactpoint, 4);
+      // override standard colors if visualizing islands
-      } else {
+      if (vopt->flags[mjVIS_ISLAND] && d->nisland && efc_adr >= 0) {
-        f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4);
+        // set color using island's first dof
+        islandColor(thisgeom->rgba, d->island_dofadr[d->efc_island[efc_adr]]);
+      }
+
+      // otherwise regular colors (different for included and excluded contacts)
+      else {
+        if (efc_adr >= 0) {
+          f2f(thisgeom->rgba, m->vis.rgba.contactpoint, 4);
+        } else {
+          f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4);
+        }
       }
 
       // label contacting geom names or ids
@@ -1095,6 +1111,28 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
     }
   }
 
+  // island labels
+  objtype = mjOBJ_UNKNOWN;
+  category = mjCAT_DECOR;
+  if ((category & catmask) && (vopt->label == mjLABEL_ISLAND) && d->nisland) {
+    for (int i=1; i < m->nbody; i++) {
+      int weld_id = m->body_weldid[i];
+      if (m->body_dofnum[weld_id]) {
+        int islandid = d->dof_island[m->body_dofadr[weld_id]];
+        if (islandid > -1) {
+          START
+
+          thisgeom->type = mjGEOM_LABEL;
+          mju_n2f(thisgeom->pos, d->xipos+3*i, 3);
+          mju_n2f(thisgeom->mat, d->ximat+9*i, 9);
+          mjSNPRINTF(thisgeom->label, "%d", islandid);
+
+          FINISH
+        }
+      }
+    }
+  }
+
   // geom
   int planeid = -1;
   for (int i=0; i < m->ngeom; i++) {
@@ -1126,8 +1164,23 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
       // copy rbound from model
       thisgeom->modelrbound = (float)m->geom_rbound[i];
 
-      // set material properties
+      // set material properties, override if visualizing islands
-      setMaterial(m, thisgeom, m->geom_matid[i], m->geom_rgba+4*i, vopt->flags);
+      float* rgba = m->geom_rgba+4*i;
+      float rgba_island[4] = {.5, .5, .5, 1};
+      int geom_matid = m->geom_matid[i];
+      if (vopt->flags[mjVIS_ISLAND] && d->nisland) {
+        geom_matid = -1;
+        rgba = rgba_island;
+        int weld_id = m->body_weldid[m->geom_bodyid[i]];
+        if (m->body_dofnum[weld_id]) {
+          int island = d->dof_island[m->body_dofadr[weld_id]];
+          if (island > -1) {
+            // color using island's first dof
+            islandColor(rgba_island, d->island_dofadr[island]);
+          }
+        }
+      }
+      setMaterial(m, thisgeom, geom_matid, rgba, vopt->flags);
 
       // set texcoord
       if (m->geom_type[i] == mjGEOM_MESH &&
@@ -1474,17 +1527,21 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
   if (vopt->flags[mjVIS_TENDON] && (category & catmask)) {
     for (int i=0; i < m->ntendon; i++) {
       if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) {
-        // stiff tendon has a deadband spring
+        // tendon has a deadband spring
         int limitedspring =
           m->tendon_stiffness[i] > 0            &&    // positive stiffness
           m->tendon_lengthspring[2*i] == 0      &&    // range lower-bound is 0
           m->tendon_lengthspring[2*i+1] > 0;          // range upper-bound is positive
 
-        // non-stiff tendon has a length constraint
+        // tendon has a simple length constraint, but is currently not limited
+        mjtNum ten_length = d->ten_length[i];
+        mjtNum lower = m->tendon_range[2*i];
+        mjtNum upper = m->tendon_range[2*i + 1];
         int limitedconstraint =
           m->tendon_stiffness[i] == 0           &&    // zero stiffness
           m->tendon_limited[i] == 1             &&    // limited length range
-          m->tendon_range[2*i] == 0;                  // range lower-bound is 0
+          lower == 0                            &&    // range lower-bound is 0
+          ten_length < upper;                         // current length is smaller than upper bound
 
         // conditions for drawing a catenary
         int draw_catenary =
@@ -1511,8 +1568,33 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
               // construct geom
               mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], d->wrap_xpos+3*j, d->wrap_xpos+3*j+3);
 
-              // set material if given
+              // set material properties, override if visualizing islands
-              setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags);
+              float* rgba = m->tendon_rgba+4*i;
+              float rgba_island[4] = {.5, .5, .5, 1};
+              int tendon_matid = m->tendon_matid[i];
+              if (vopt->flags[mjVIS_ISLAND] && d->nisland) {
+                tendon_matid = -1;
+                rgba = rgba_island;
+                int frictional = m->tendon_frictionloss[i] > 0;
+                int limited = m->tendon_limited[i] && (ten_length <= lower || ten_length >= upper);
+                if (frictional || limited) {
+                  // search for tendon's island
+                  int island = -1;
+                  for (int k=0; k < d->nefc; k++) {
+                    int istendon = d->efc_type[k] == mjCNSTR_FRICTION_TENDON ||
+                                   d->efc_type[k] == mjCNSTR_LIMIT_TENDON;
+                    if (istendon && d->efc_id[k] == i) {
+                      island = d->efc_island[k];
+                      break;
+                    }
+                  }
+                  if (island > -1) {
+                    // set color using island's first dof
+                    islandColor(rgba_island, d->island_dofadr[island]);
+                  }
+                }
+              }
+              setMaterial(m, thisgeom, tendon_matid, rgba, vopt->flags);
 
               // vopt->label: only the first segment
               if (vopt->label == mjLABEL_TENDON && j == d->ten_wrapadr[i]) {
@@ -1546,7 +1628,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
           for (int j=0; j < npoints-1; j++) {
             START
 
-              sz[0] = m->tendon_width[i];
+            sz[0] = m->tendon_width[i];
 
             // construct geom
             mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], catenary+3*j, catenary+3*j+3);

src/user/user_model.cc

@@ -1618,6 +1618,26 @@ void mjCModel::CopyTree(mjModel* m) {
     throw mjCError(0, "unexpected number of DOFs");
   }
 
+  // count kinematic trees under world body, compute dof_treeid
+  int ntree = 0;
+  for (int i=0; i < nv; i++) {
+    if (m->dof_parentid[i] == -1) {
+      ntree++;
+    }
+    m->dof_treeid[i] = ntree - 1;
+  }
+  m->ntree = ntree;
+
+  // compute body_treeid
+  for (int i=0; i < nbody; i++) {
+    int weldid = m->body_weldid[i];
+    if (m->body_dofnum[weldid]) {
+      m->body_treeid[i] = m->dof_treeid[m->body_dofadr[weldid]];
+    } else {
+      m->body_treeid[i] = -1;
+    }
+  }
+
   // compute nM and dof_Madr
   nM = 0;
   for (int i=0; i<nv; i++) {

src/xml/xml_native_reader.cc

@@ -102,10 +102,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
         "integrator", "collision", "cone", "jacobian",
         "solver", "iterations", "noslip_iterations", "mpr_iterations"},
     {"<"},
-        {"flag", "?", "18", "constraint", "equality", "frictionloss", "limit", "contact",
+        {"flag", "?", "19", "constraint", "equality", "frictionloss", "limit", "contact",
             "passive", "gravity", "clampctrl", "warmstart",
             "filterparent", "actuation", "refsafe", "sensor",
-            "override", "energy", "fwdinv", "sensornoise", "multiccd"},
+            "override", "energy", "fwdinv", "sensornoise", "multiccd", "island"},
     {">"},
 
     {"size", "*", "14", "memory", "njmax", "nconmax", "nstack", "nuserdata", "nkey",
@@ -999,6 +999,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
     READENBL("fwdinv",      mjENBL_FWDINV)
     READENBL("sensornoise", mjENBL_SENSORNOISE)
     READENBL("multiccd",    mjENBL_MULTICCD)
+    READENBL("island",      mjENBL_ISLAND)
 #undef READENBL
   }
 }

src/xml/xml_native_writer.cc

@@ -839,6 +839,7 @@ void mjXWriter::Option(XMLElement* root) {
     WRITEENBL("fwdinv",         mjENBL_FWDINV)
     WRITEENBL("sensornoise",    mjENBL_SENSORNOISE)
     WRITEENBL("multiccd",       mjENBL_MULTICCD)
+    WRITEENBL("island",         mjENBL_ISLAND)
 #undef WRITEENBL
   }
 

test/engine/CMakeLists.txt

@@ -30,6 +30,9 @@ target_link_libraries(engine_derivative_test fixture gmock)
 mujoco_test(engine_forward_test)
 target_link_libraries(engine_forward_test fixture gmock)
 
+mujoco_test(engine_island_test)
+target_link_libraries(engine_island_test fixture gmock)
+
 mujoco_test(engine_io_test)
 target_link_libraries(
   engine_io_test

test/engine/engine_core_constraint_test.cc

@@ -17,6 +17,7 @@
 #include <array>
 #include <cstddef>
 #include <string>
+#include <vector>
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
@@ -24,15 +25,12 @@
 #include <mujoco/mujoco.h>
 #include "src/engine/engine_core_constraint.h"
 #include "src/engine/engine_support.h"
-#include "src/engine/engine_util_sparse.h"
 #include "test/fixture.h"
 
 namespace mujoco {
 namespace {
 
 using ::testing::DoubleNear;
-using ::testing::ElementsAre;
-using ::testing::ElementsAreArray;
 using ::testing::Pointwise;
 using CoreConstraintTest = MujocoTest;
 
@@ -272,233 +270,5 @@ TEST_F(CoreConstraintTest, CombineSparseCount) {
   }
 }
 
-TEST_F(CoreConstraintTest, EdgeToSparse4) {
-  // unsorted edges, with duplication
-  constexpr int ne = 6;
-  constexpr int nr = 5;
-  int edge[2*ne] = {
-    1, 1,
-    0, 0,
-    0, 1,
-    3, 2,
-    1, 1,
-    0, 0
-  };
-
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[ne];
-
-  int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
-
-  constexpr int expected_nnz = 4;
-
-  EXPECT_EQ(nnz, expected_nnz);
-
-  EXPECT_THAT(rownnz, ElementsAre(2, 1, 0, 1, 0));
-  EXPECT_THAT(rowadr, ElementsAre(0, 2, 3, 3, 4));
-
-  int expected_colind[expected_nnz] = {0, 1, 1, 2};
-  EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
-}
-
-TEST_F(CoreConstraintTest, EdgeToSparse2) {
-  // unsorted edges, with duplication
-  constexpr int ne = 4;
-  constexpr int nr = 5;
-  int edge[2*ne] = {
-    3, 4,
-    1, 1,
-    3, 4,
-    1, 1
-  };
-
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[ne];
-
-  int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
-
-  constexpr int expected_nnz = 2;
-
-  EXPECT_EQ(nnz, expected_nnz);
-
-  EXPECT_THAT(rownnz, ElementsAre(0, 1, 0, 1, 0));
-  EXPECT_THAT(rowadr, ElementsAre(0, 0, 1, 1, 2));
-
-  int expected_colind[expected_nnz] = {1, 4};
-  EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
-}
-
-TEST_F(CoreConstraintTest, EdgeToSparse3) {
-  // unsorted edges, with duplication
-  constexpr int ne = 3;
-  constexpr int nr = 1;
-  int edge[2*ne] = {
-    0, 0,
-    0, 0,
-    0, 0
-  };
-
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[ne];
-
-  int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
-
-  constexpr int expected_nnz = 1;
-
-  EXPECT_EQ(nnz, expected_nnz);
-
-  EXPECT_THAT(rownnz, ElementsAre(1));
-  EXPECT_THAT(rowadr, ElementsAre(0));
-
-  int expected_colind[expected_nnz] = {0};
-  EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
-}
-
-
-TEST_F(CoreConstraintTest, FloodFillSingleton) {
-  // adjacency matrix for the graph  0   1   2
-  //                                 U       U
-  // (3 singletons, 0 and 2 have self-edges)
-  mjtNum mat[9] = {
-    1, 0, 0,
-    0, 0, 0,
-    0, 0, 1
-  };
-  constexpr int nr = 3;
-  constexpr int nnz = 2;
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[nnz];
-  mjtNum res[nnz];  // unused
-  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
-
-  // outputs / scratch
-  int island[nr];
-  int scratch[2*nr];
-
-  // flood fill
-  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, scratch);
-
-  EXPECT_EQ(nisland, 2);
-  EXPECT_THAT(island, ElementsAre(0, -1, 1));
-}
-
-TEST_F(CoreConstraintTest, FloodFill1) {
-  // adjacency matrix for the graph  0 - 1 - 2
-  mjtNum mat[9] = {
-    0, 1, 0,
-    1, 0, 1,
-    0, 1, 0
-  };
-  constexpr int nr = 3;
-  constexpr int nnz = 4;
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[nnz];
-  mjtNum res[nnz];  // unused
-  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
-
-  // outputs / stack
-  int island[nr];
-  int stack[nnz];
-
-  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
-
-  EXPECT_EQ(nisland, 1);
-  EXPECT_THAT(island, ElementsAre(0, 0, 0));
-}
-
-TEST_F(CoreConstraintTest, FloodFill2) {
-  //  adjacency matrix for the graph   6 – 1 – 4   0 – 3 – 5 – 2
-  mjtNum mat[49] = {
-    0, 0, 0, 1, 0, 0, 0,
-    0, 0, 0, 0, 1, 0, 1,
-    0, 0, 0, 0, 0, 1, 0,
-    1, 0, 0, 0, 0, 1, 0,
-    0, 1, 0, 0, 0, 0, 0,
-    0, 0, 1, 1, 0, 0, 0,
-    0, 1, 0, 0, 0, 0, 0,
-  };
-  constexpr int nr = 7;
-  constexpr int nnz = 10;
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[nnz];
-  mjtNum res[nnz];  // unused
-  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
-
-  // outputs / stack
-  int island[nr];
-  int stack[nnz];
-
-  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
-
-  EXPECT_EQ(nisland, 2);
-  EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
-}
-
-TEST_F(CoreConstraintTest, FloodFill3a) {
-  //  adjacency matrix for the graph   0   2   1 – 3
-  //                                       U
-  mjtNum mat[16] = {
-    0, 0, 0, 0,
-    0, 0, 0, 1,
-    0, 0, 1, 0,
-    0, 1, 0, 0,
-  };
-  constexpr int nr = 4;
-  constexpr int nnz = 3;
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[nnz];
-  mjtNum res[nnz];  // unused
-  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
-
-  // outputs / stack
-  int island[nr];
-  int stack[nnz];
-
-  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
-
-  EXPECT_EQ(nisland, 2);
-  EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
-}
-
-TEST_F(CoreConstraintTest, FloodFill3b) {
-  /*
-    adjacency matrix for the graph   1 – 2   3   4 – 5
-                                     U           | \ |
-                                                 0 – 6
-  */
-  mjtNum mat[49] = {
-    0, 0, 0, 0, 1, 0, 1,
-    0, 1, 1, 0, 0, 0, 0,
-    0, 1, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0,
-    1, 0, 0, 0, 0, 1, 1,
-    0, 0, 0, 0, 1, 0, 1,
-    1, 0, 0, 0, 1, 1, 0,
-  };
-  constexpr int nr = 7;
-  constexpr int nnz = 13;
-  int rownnz[nr];
-  int rowadr[nr];
-  int colind[nnz];
-  mjtNum res[nnz];  // unused
-  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
-
-  // outputs / stack
-  int island[nr];
-  int stack[nnz];
-
-  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
-
-  EXPECT_EQ(nisland, 2);
-  EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
-}
-
 }  // namespace
 }  // namespace mujoco

test/engine/engine_island_test.cc

@@ -0,0 +1,381 @@
+// Copyright 2023 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.
+
+// Tests for engine/engine_island.c.
+
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mujoco.h>
+#include "src/engine/engine_island.h"
+#include "src/engine/engine_util_sparse.h"
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::ElementsAre;
+using ::testing::ElementsAreArray;
+using IslandTest = MujocoTest;
+
+std::vector<int> AsVector(const int* array, int n) {
+  return std::vector<int>(array, array + n);
+}
+
+TEST_F(IslandTest, EdgeToSparse2) {
+  // unsorted edges, with duplication
+  constexpr int ne = 4;
+  constexpr int nr = 5;
+  int edge[2*ne] = {
+    3, 4,
+    1, 1,
+    3, 4,
+    1, 1
+  };
+
+  int rownnz[nr];
+  int rowadr[nr];
+  int colind[ne];
+
+  int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
+
+  constexpr int expected_nnz = 2;
+
+  EXPECT_EQ(nnz, expected_nnz);
+
+  EXPECT_THAT(rownnz, ElementsAre(0, 1, 0, 1, 0));
+  EXPECT_THAT(rowadr, ElementsAre(0, 0, 1, 1, 2));
+
+  int expected_colind[expected_nnz] = {1, 4};
+  EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
+}
+
+TEST_F(IslandTest, EdgeToSparse3) {
+  // unsorted edges, with duplication
+  constexpr int ne = 3;
+  constexpr int nr = 1;
+  int edge[2*ne] = {
+    0, 0,
+    0, 0,
+    0, 0
+  };
+
+  int rownnz[nr];
+  int rowadr[nr];
+  int colind[ne];
+
+  int nnz = mj_edge2Sparse(rownnz, rowadr, colind, edge, ne, nr);
+
+  constexpr int expected_nnz = 1;
+
+  EXPECT_EQ(nnz, expected_nnz);
+
+  EXPECT_THAT(rownnz, ElementsAre(1));
+  EXPECT_THAT(rowadr, ElementsAre(0));
+
+  int expected_colind[expected_nnz] = {0};
+  EXPECT_THAT(expected_colind, ElementsAreArray(colind, expected_nnz));
+}
+
+
+TEST_F(IslandTest, FloodFillSingleton) {
+  // adjacency matrix for the graph  0   1   2
+  //                                 U       U
+  // (3 singletons, 0 and 2 have self-edges)
+  mjtNum mat[9] = {
+    1, 0, 0,
+    0, 0, 0,
+    0, 0, 1
+  };
+  constexpr int nr = 3;
+  constexpr int nnz = 2;
+  int rownnz[nr];
+  int rowadr[nr];
+  int colind[nnz];
+  mjtNum res[nnz];  // unused
+  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
+
+  // outputs / scratch
+  int island[nr];
+  int scratch[2*nr];
+
+  // flood fill
+  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, scratch);
+
+  EXPECT_EQ(nisland, 2);
+  EXPECT_THAT(island, ElementsAre(0, -1, 1));
+}
+
+
+TEST_F(IslandTest, FloodFill1) {
+  // adjacency matrix for the graph  0 - 1 - 2
+  mjtNum mat[9] = {
+    0, 1, 0,
+    1, 0, 1,
+    0, 1, 0
+  };
+  constexpr int nr = 3;
+  constexpr int nnz = 4;
+  int rownnz[nr];
+  int rowadr[nr];
+  int colind[nnz];
+  mjtNum res[nnz];  // unused
+  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
+
+  // outputs / stack
+  int island[nr];
+  int stack[nnz];
+
+  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
+
+  EXPECT_EQ(nisland, 1);
+  EXPECT_THAT(island, ElementsAre(0, 0, 0));
+}
+
+
+TEST_F(IslandTest, FloodFill2) {
+  //  adjacency matrix for the graph   6 – 1 – 4   0 – 3 – 5 – 2
+  mjtNum mat[49] = {
+    0, 0, 0, 1, 0, 0, 0,
+    0, 0, 0, 0, 1, 0, 1,
+    0, 0, 0, 0, 0, 1, 0,
+    1, 0, 0, 0, 0, 1, 0,
+    0, 1, 0, 0, 0, 0, 0,
+    0, 0, 1, 1, 0, 0, 0,
+    0, 1, 0, 0, 0, 0, 0,
+  };
+  constexpr int nr = 7;
+  constexpr int nnz = 10;
+  int rownnz[nr];
+  int rowadr[nr];
+  int colind[nnz];
+  mjtNum res[nnz];  // unused
+  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
+
+  // outputs / stack
+  int island[nr];
+  int stack[nnz];
+
+  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
+
+  EXPECT_EQ(nisland, 2);
+  EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
+}
+
+
+TEST_F(IslandTest, FloodFill3a) {
+  //  adjacency matrix for the graph   0   2   1 – 3
+  //                                       U
+  mjtNum mat[16] = {
+    0, 0, 0, 0,
+    0, 0, 0, 1,
+    0, 0, 1, 0,
+    0, 1, 0, 0,
+  };
+  constexpr int nr = 4;
+  constexpr int nnz = 3;
+  int rownnz[nr];
+  int rowadr[nr];
+  int colind[nnz];
+  mjtNum res[nnz];  // unused
+  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
+
+  // outputs / stack
+  int island[nr];
+  int stack[nnz];
+
+  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
+
+  EXPECT_EQ(nisland, 2);
+  EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
+}
+
+
+TEST_F(IslandTest, FloodFill3b) {
+  /*
+    adjacency matrix for the graph   1 – 2   3   4 – 5
+                                     U           | \ |
+                                                 0 – 6
+  */
+  mjtNum mat[49] = {
+    0, 0, 0, 0, 1, 0, 1,
+    0, 1, 1, 0, 0, 0, 0,
+    0, 1, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0,
+    1, 0, 0, 0, 0, 1, 1,
+    0, 0, 0, 0, 1, 0, 1,
+    1, 0, 0, 0, 1, 1, 0,
+  };
+  constexpr int nr = 7;
+  constexpr int nnz = 13;
+  int rownnz[nr];
+  int rowadr[nr];
+  int colind[nnz];
+  mjtNum res[nnz];  // unused
+  mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind);
+
+  // outputs / stack
+  int island[nr];
+  int stack[nnz];
+
+  int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, stack);
+
+  EXPECT_EQ(nisland, 2);
+  EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
+}
+
+static const char* const kAbacusPath =
+    "engine/testdata/island/abacus.xml";
+
+TEST_F(IslandTest, Abacus) {
+  const std::string xml_path = GetTestDataFilePath(kAbacusPath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+
+  // disable gravity
+  model->opt.disableflags |= mjDSBL_GRAVITY;
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  // no islands at qpos0
+  EXPECT_EQ(data->nisland, 0);
+
+  // push bead 0 to the left and bead 2 to the right until there are 3 contacts
+  data->qfrc_applied[0] = -1;
+  data->qfrc_applied[2] = 1;
+  while (data->ncon != 3) {
+    mj_step(model, data);
+  }
+
+  // sizes
+  int nv      = model->nv;
+  int nefc    = data->nefc;
+  int nisland = data->nisland;
+
+  // 4 dofs, 12 constraints, 2 islands
+  EXPECT_EQ(nv, 4);
+  EXPECT_EQ(nefc, 12);  // 3 pyramidal contacts
+  EXPECT_EQ(nisland, 2);
+
+  // the islands begin at dofs 0 and 2
+  EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 2));
+
+  // dof 0 in    island 0
+  // dof 1 in no island
+  // dofs 2,3 in island 1
+  EXPECT_THAT(AsVector(data->dof_island, nv), ElementsAre(0, -1, 1, 1));
+
+  // dof 0 is last dof of island 0
+  // dof 1 in no island
+  // next dof after 2 is 3
+  // dof 3 is last dof of island 1
+  EXPECT_THAT(AsVector(data->dof_islandnext, nv), ElementsAre(-1, -1, 3, -1));
+
+  // island 0 starts at constraint 0
+  // island 1 starts at constraint 4
+  EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4));
+
+  // first contact (4 constraints) is in island 0
+  // second contact (8 constraints) is in island 1
+  EXPECT_THAT(AsVector(data->efc_island, nefc),
+              ElementsAre(0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1));
+
+  // linked list for island 0
+  // linked list for island 1
+  EXPECT_THAT(AsVector(data->efc_islandnext, nefc),
+              ElementsAre(1, 2, 3, -1, 5, 6, 7, 8, 9, 10, 11, -1));
+
+  // reset, push 0 to the left, 3 to the right, 1,2 to the middle
+  mj_resetData(model, data);
+  data->qfrc_applied[0] = -1;
+  data->qfrc_applied[1] = 1;
+  data->qfrc_applied[2] = -1;
+  data->qfrc_applied[3] = 1;
+
+  // simulate until there are 3 contacts
+  while (data->ncon != 3) {
+    mj_step(model, data);
+  }
+
+  // local variables
+  nefc           = data->nefc;
+  nisland        = data->nisland;
+
+  EXPECT_EQ(nisland, 3);
+  EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 1, 3));
+  EXPECT_THAT(AsVector(data->dof_island, nv), ElementsAre(0, 1, 1, 2));
+  EXPECT_THAT(AsVector(data->dof_islandnext, nv), ElementsAre(-1, 2, -1, -1));
+  EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4, 8));
+  EXPECT_THAT(AsVector(data->efc_island, nefc),
+              ElementsAre(0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2));
+  EXPECT_THAT(AsVector(data->efc_islandnext, nefc),
+              ElementsAre(1, 2, 3, -1, 5, 6, 7, -1, 9, 10, 11, -1));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+static const char* const kTendonWrapPath =
+    "engine/testdata/island/tendon_wrap.xml";
+
+TEST_F(IslandTest, DenseSparse) {
+  const std::string xml_path = GetTestDataFilePath(kTendonWrapPath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+  mjData* data1 = mj_makeData(model);
+  mjData* data2 = mj_makeData(model);
+
+  // dense
+  model->opt.jacobian = mjJAC_DENSE;
+  while (!data1->nefc) {
+    mj_step(model, data1);
+  }
+
+  // sparse
+  model->opt.jacobian = mjJAC_SPARSE;
+  while (!data2->nefc) {
+    mj_step(model, data2);
+  }
+
+  // sizes
+  int nv      = model->nv;
+  int nefc    = data1->nefc;
+  int nisland = data1->nisland;
+
+  // expect sparse and dense to be identical
+  EXPECT_EQ(data1->nefc, data2->nefc);
+  EXPECT_EQ(data1->nisland, data2->nisland);
+  EXPECT_EQ(data1->nefc, data2->nefc);
+  EXPECT_EQ(AsVector(data1->island_dofadr, nisland),
+            AsVector(data2->island_dofadr, nisland));
+  EXPECT_EQ(AsVector(data1->dof_island, nv),
+            AsVector(data2->dof_island, nv));
+  EXPECT_EQ(AsVector(data1->dof_islandnext, nv),
+            AsVector(data2->dof_islandnext, nv));
+  EXPECT_EQ(AsVector(data1->island_efcadr, nisland),
+            AsVector(data2->island_efcadr, nisland));
+  EXPECT_EQ(AsVector(data1->efc_island, nefc),
+            AsVector(data2->efc_island, nefc));
+  EXPECT_EQ(AsVector(data1->efc_islandnext, nefc),
+            AsVector(data2->efc_islandnext, nefc));
+
+  mj_deleteData(data2);
+  mj_deleteData(data1);
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/testdata/island/abacus.xml

@@ -0,0 +1,35 @@
+<mujoco>
+  <option gravity="1 0 0">
+    <flag island="enable"/>
+  </option>
+
+  <default>
+    <joint type="slide" axis="1 0 0" damping="10"/>
+    <geom size=".1"/>
+  </default>
+
+  <worldbody>
+    <body>
+      <geom type="box" size=".05 .1 .1"/>
+    </body>
+    <body pos=".25 0 0">
+      <joint/>
+      <geom/>
+    </body>
+    <body pos=".5 0 0">
+      <joint/>
+      <geom/>
+    </body>
+    <body pos=".75 0 0">
+      <joint/>
+      <geom/>
+    </body>
+    <body pos="1 0 0">
+      <joint/>
+      <geom/>
+    </body>
+    <body pos="1.25 0 0">
+      <geom type="box" size=".05 .1 .1"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/island/tendon_wrap.xml

@@ -0,0 +1,56 @@
+<mujoco>
+  <option>
+    <flag island="enable"/>
+  </option>
+
+  <default>
+    <joint type="slide" axis="1 0 0" damping="3" frictionloss=".1"/>
+    <geom type="cylinder" size=".07 .1" zaxis="0 1 0"/>
+    <site size=".015" rgba="1 0 0 1"/>
+  </default>
+
+  <worldbody>
+    <site name="anchor" pos="0 0 1"/>
+
+    <body pos="0 0 .75">
+      <joint stiffness="10"/>
+      <geom name="0"/>
+    </body>
+    <site name="side0" pos=".15 0 .75" rgba="0 0 1 1"/>
+
+    <site name="01" pos="0 0 .625"/>
+
+    <body pos="0 0 .5">
+      <joint stiffness="30"/>
+      <geom name="1"/>
+    </body>
+    <site name="side1" pos="-.15 0 .5" rgba="0 0 1 1"/>
+
+    <site name="12" pos="0 0 .375"/>
+
+    <body pos="0 0 .25">
+      <joint stiffness="100"/>
+      <geom name="2"/>
+    </body>
+    <site name="side2" pos=".15 0 .25" rgba="0 0 1 1"/>
+
+    <body>
+      <freejoint/>
+      <site name="box" pos="0 0 .1"/>
+      <geom type="box" size=".1 .1 .1"/>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial limited="true" range="0 1.05" rgba="0 1 0 1" width=".005">
+      <site site="anchor"/>
+      <geom geom="0" sidesite="side0"/>
+      <site site="01"/>
+      <geom geom="1" sidesite="side1"/>
+      <site site="12"/>
+      <geom geom="2" sidesite="side2"/>
+      <site site="box"/>
+    </spatial>
+  </tendon>
+</mujoco>
+

test/testdata/model.xml

@@ -1,6 +1,6 @@
 <mujoco>
   <option jacobian="dense" density="1.225" viscosity="1.8e-5" wind="0 0 1">
-    <flag fwdinv="enable" energy="enable"/>
+    <flag fwdinv="enable" energy="enable" island="enable"/>
   </option>
 
   <asset>

unity/Runtime/Bindings/MjBindings.cs

@@ -159,7 +159,8 @@ public enum mjtEnableBit : int{
   mjENBL_FWDINV = 4,
   mjENBL_SENSORNOISE = 8,
   mjENBL_MULTICCD = 16,
-  mjNENABLE = 5,
+  mjENBL_ISLAND = 32,
+  mjNENABLE = 6,
 }
 public enum mjtJoint : int{
   mjJNT_FREE = 0,
@@ -455,7 +456,8 @@ public enum mjtLabel : int{
   mjLABEL_SELPNT = 12,
   mjLABEL_CONTACTPOINT = 13,
   mjLABEL_CONTACTFORCE = 14,
-  mjNLABEL = 15,
+  mjLABEL_ISLAND = 15,
+  mjNLABEL = 16,
 }
 public enum mjtFrame : int{
   mjFRAME_NONE = 0,
@@ -484,17 +486,18 @@ public enum mjtVisFlag : int{
   mjVIS_PERTFORCE = 12,
   mjVIS_PERTOBJ = 13,
   mjVIS_CONTACTPOINT = 14,
-  mjVIS_CONTACTFORCE = 15,
+  mjVIS_ISLAND = 15,
-  mjVIS_CONTACTSPLIT = 16,
+  mjVIS_CONTACTFORCE = 16,
-  mjVIS_TRANSPARENT = 17,
+  mjVIS_CONTACTSPLIT = 17,
-  mjVIS_AUTOCONNECT = 18,
+  mjVIS_TRANSPARENT = 18,
-  mjVIS_COM = 19,
+  mjVIS_AUTOCONNECT = 19,
-  mjVIS_SELECT = 20,
+  mjVIS_COM = 20,
-  mjVIS_STATIC = 21,
+  mjVIS_SELECT = 21,
-  mjVIS_SKIN = 22,
+  mjVIS_STATIC = 22,
-  mjVIS_MIDPHASE = 23,
+  mjVIS_SKIN = 23,
-  mjVIS_MESHBVH = 24,
+  mjVIS_MIDPHASE = 24,
-  mjNVISFLAG = 25,
+  mjVIS_MESHBVH = 25,
+  mjNVISFLAG = 26,
 }
 public enum mjtRndFlag : int{
   mjRND_SHADOW = 0,
@@ -1604,6 +1607,7 @@ public unsafe struct mjData_ {
   public int nefc;
   public int nnzJ;
   public int ncon;
+  public int nisland;
   public double time;
   public fixed double energy[2];
   public void* buffer;
@@ -1706,6 +1710,12 @@ public unsafe struct mjData_ {
   public double* efc_KBIP;
   public double* efc_D;
   public double* efc_R;
+  public int* island_dofadr;
+  public int* island_efcadr;
+  public int* dof_island;
+  public int* dof_islandnext;
+  public int* efc_island;
+  public int* efc_islandnext;
   public int* efc_AR_rownnz;
   public int* efc_AR_rowadr;
   public int* efc_AR_colind;
@@ -1940,6 +1950,7 @@ public unsafe struct mjModel_ {
   public int nM;
   public int nD;
   public int nB;
+  public int ntree;
   public int nemax;
   public int njmax;
   public int nconmax;
@@ -1962,6 +1973,7 @@ public unsafe struct mjModel_ {
   public int* body_jntadr;
   public int* body_dofnum;
   public int* body_dofadr;
+  public int* body_treeid;
   public int* body_geomnum;
   public int* body_geomadr;
   public byte* body_simple;
@@ -2002,6 +2014,7 @@ public unsafe struct mjModel_ {
   public int* dof_bodyid;
   public int* dof_jntid;
   public int* dof_parentid;
+  public int* dof_treeid;
   public int* dof_Madr;
   public int* dof_simplenum;
   public double* dof_solref;
@@ -2588,7 +2601,7 @@ public unsafe struct mjvOption_ {
   public fixed byte tendongroup[6];
   public fixed byte actuatorgroup[6];
   public fixed byte skingroup[6];
-  public fixed byte flags[25];
+  public fixed byte flags[26];
   public int bvh_depth;
 }
 
@@ -2911,6 +2924,7 @@ public unsafe struct data {
   public mjWarningStat_ warning7;
   public int nefc;
   public int ncon;
+  public int nisland;
   public double time;
   public double* act;
   public double* ctrl;
@@ -2937,6 +2951,9 @@ public unsafe struct data {
   public int* wrap_obj;
   public double* wrap_xpos;
   public byte* bvh_active;
+  public int* island_dofadr;
+  public int* dof_island;
+  public int* efc_island;
   public mjContact_* contact;
   public double* efc_force;
 }
@@ -3187,6 +3204,9 @@ public static unsafe extern void mj_collision(mjModel_* m, mjData_* d);
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_makeConstraint(mjModel_* m, mjData_* d);
 
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern void mj_island(mjModel_* m, mjData_* d);
+
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_projectConstraint(mjModel_* m, mjData_* d);