From 3e034e38b2f923ebdaf89bc16529f2c2b10347a9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 15 Aug 2023 02:29:26 -0700 Subject: [PATCH] Add constraint island discovery PiperOrigin-RevId: 557067599 Change-Id: Ic41e1d0efef02b7a79142518afe49cf9d4e74725 --- doc/APIreference/functions.rst | 9 + doc/XMLreference.rst | 17 +- doc/XMLschema.rst | 2 +- doc/changelog.rst | 16 +- doc/includes/references.h | 26 +- include/mujoco/mjdata.h | 9 + include/mujoco/mjmodel.h | 10 +- include/mujoco/mjvisualize.h | 6 + include/mujoco/mjxmacro.h | 72 +-- include/mujoco/mujoco.h | 3 + introspect/enums.py | 29 +- introspect/enums_test.py | 3 +- introspect/functions.py | 20 + introspect/structs.py | 98 +++- python/mujoco/bindings_test.py | 2 +- python/mujoco/functions.cc | 1 + python/mujoco/structs.cc | 11 +- simulate/simulate.cc | 9 +- src/engine/CMakeLists.txt | 2 + src/engine/engine_core_constraint.c | 139 +----- src/engine/engine_core_constraint.h | 7 +- src/engine/engine_forward.c | 4 + src/engine/engine_io.c | 1 + src/engine/engine_island.c | 521 ++++++++++++++++++++ src/engine/engine_island.h | 46 ++ src/engine/engine_print.c | 44 +- src/engine/engine_support.c | 3 +- src/engine/engine_vis_init.c | 4 +- src/engine/engine_vis_state.c | 16 + src/engine/engine_vis_visualize.c | 112 ++++- src/user/user_model.cc | 20 + src/xml/xml_native_reader.cc | 5 +- src/xml/xml_native_writer.cc | 1 + test/engine/CMakeLists.txt | 3 + test/engine/engine_core_constraint_test.cc | 232 +-------- test/engine/engine_island_test.cc | 381 ++++++++++++++ test/engine/testdata/island/abacus.xml | 35 ++ test/engine/testdata/island/tendon_wrap.xml | 56 +++ test/testdata/model.xml | 2 +- unity/Runtime/Bindings/MjBindings.cs | 48 +- 40 files changed, 1557 insertions(+), 468 deletions(-) create mode 100644 src/engine/engine_island.c create mode 100644 src/engine/engine_island.h create mode 100644 test/engine/engine_island_test.cc create mode 100644 test/engine/testdata/island/abacus.xml create mode 100644 test/engine/testdata/island/tendon_wrap.xml diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index caa62507..82ad9370 100644 --- a/doc/APIreference/functions.rst +++ b/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 diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index d37c1dc8..ee9fba7a 100644 --- a/doc/XMLreference.rst +++ b/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: - -: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-island: +: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 `__. + In a future release, the constraint solver will exploit the disjoint nature of constraint islands. .. _body: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index bb6463cf..e681670f 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -227,7 +227,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`sensor` | :ref:`override` | :ref:`energy` | :ref:`fwdinv` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`sensornoise` | :ref:`multiccd` | | | | +| | | | :ref:`sensornoise` | :ref:`multiccd` | :ref:`island` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 20086c60..45631d08 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,7 +8,16 @@ Upcoming version (not yet released) General ^^^^^^^ -1. Added a new :ref:`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` + 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`, ``filterexact``, which updates first-order filter states with the exact formula rather than with Euler integration. #. Added an actuator attribute, :ref:`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. - (`#870 `_) +4. Fixed `#870 `__ where calling ``update_scene`` with an invalid + camera name used the default camera. + Version 2.3.7 (July 20, 2023) ----------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index ac60b0f6..6c70afe9 100644 --- a/doc/includes/references.h +++ b/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, diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 012ec7e7..9c6781d5 100644 --- a/include/mujoco/mjdata.h +++ b/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) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 731a617a..6298edf6 100644 --- a/include/mujoco/mjmodel.h +++ b/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) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index 53c124c3..e4e7f5c8 100644 --- a/include/mujoco/mjvisualize.h +++ b/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; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 5322e7a2..4d622ada 100644 --- a/include/mujoco/mjxmacro.h +++ b/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 \ - X(int, efc_type, 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_rowadr, 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_JT_rownnz, 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_colind, MJ_D(nnzJ), 1) \ - X(mjtNum, efc_J, MJ_D(nnzJ), 1) \ - X(mjtNum, efc_JT, MJ_D(nnzJ), 1) \ - X(mjtNum, efc_pos, MJ_D(nefc), 1) \ - X(mjtNum, efc_margin, MJ_D(nefc), 1) \ - X(mjtNum, efc_frictionloss, MJ_D(nefc), 1) \ - X(mjtNum, efc_diagApprox, MJ_D(nefc), 1) \ - X(mjtNum, efc_KBIP, MJ_D(nefc), 4) \ - X(mjtNum, efc_D, MJ_D(nefc), 1) \ - X(mjtNum, efc_R, MJ_D(nefc), 1) \ - X(mjtNum, efc_vel, MJ_D(nefc), 1) \ - X(mjtNum, efc_aref, MJ_D(nefc), 1) \ - X(mjtNum, efc_b, MJ_D(nefc), 1) \ - X(mjtNum, efc_force, MJ_D(nefc), 1) \ - X(int, efc_state, MJ_D(nefc), 1) +#define MJDATA_ARENA_POINTERS_PRIMAL \ + X( int, efc_type, 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_rowadr, 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_JT_rownnz, 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_colind, MJ_D(nnzJ), 1) \ + X( mjtNum, efc_J, MJ_D(nnzJ), 1) \ + X( mjtNum, efc_JT, MJ_D(nnzJ), 1) \ + X( mjtNum, efc_pos, MJ_D(nefc), 1) \ + X( mjtNum, efc_margin, MJ_D(nefc), 1) \ + X( mjtNum, efc_frictionloss, MJ_D(nefc), 1) \ + X( mjtNum, efc_diagApprox, MJ_D(nefc), 1) \ + X( mjtNum, efc_KBIP, MJ_D(nefc), 4) \ + X( mjtNum, efc_D, MJ_D(nefc), 1) \ + X( mjtNum, efc_R, MJ_D(nefc), 1) \ + X( mjtNum, efc_vel, MJ_D(nefc), 1) \ + X( mjtNum, efc_aref, MJ_D(nefc), 1) \ + X( mjtNum, efc_b, MJ_D(nefc), 1) \ + X( mjtNum, efc_force, 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 ) diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index d062b6bd..1ef8bb63 100644 --- a/include/mujoco/mujoco.h +++ b/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); diff --git a/introspect/enums.py b/introspect/enums.py index b0449c6e..f2d99d5d 100644 --- a/introspect/enums.py +++ b/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_CONTACTSPLIT', 16), - ('mjVIS_TRANSPARENT', 17), - ('mjVIS_AUTOCONNECT', 18), - ('mjVIS_COM', 19), - ('mjVIS_SELECT', 20), - ('mjVIS_STATIC', 21), - ('mjVIS_SKIN', 22), - ('mjVIS_MIDPHASE', 23), - ('mjVIS_MESHBVH', 24), - ('mjNVISFLAG', 25), + ('mjVIS_ISLAND', 15), + ('mjVIS_CONTACTFORCE', 16), + ('mjVIS_CONTACTSPLIT', 17), + ('mjVIS_TRANSPARENT', 18), + ('mjVIS_AUTOCONNECT', 19), + ('mjVIS_COM', 20), + ('mjVIS_SELECT', 21), + ('mjVIS_STATIC', 22), + ('mjVIS_SKIN', 23), + ('mjVIS_MIDPHASE', 24), + ('mjVIS_MESHBVH', 25), + ('mjNVISFLAG', 26), ]), )), ('mjtRndFlag', diff --git a/introspect/enums_test.py b/introspect/enums_test.py index 2f017281..02cac977 100644 --- a/introspect/enums_test.py +++ b/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 diff --git a/introspect/functions.py b/introspect/functions.py index 91feff67..15f02142 100644 --- a/introspect/functions.py +++ b/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', diff --git a/introspect/structs.py b/introspect/structs.py index d3e0c1e2..60ad5f1c 100644 --- a/introspect/structs.py +++ b/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( diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index c079c814..629face1 100644 --- a/python/mujoco/bindings_test.py +++ b/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) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 3b52a937..edd4d4d8 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -253,6 +253,7 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def( diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 767fdd87..ea6f8dda 100644 --- a/python/mujoco/structs.cc +++ b/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) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index a5a3595d..776729d9 100644 --- a/simulate/simulate.cc +++ b/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, diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index 3dfa1468..029b6d23 100644 --- a/src/engine/CMakeLists.txt +++ b/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 diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 0bf6a4a3..66b08f06 100644 --- a/src/engine/engine_core_constraint.c +++ b/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; -} diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 0d357bbc..5ef90e67 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -18,6 +18,7 @@ #include #include #include +#include #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 diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 67b8cbe1..ba3b5bbe 100644 --- a/src/engine/engine_forward.c +++ b/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); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 2a08b779..a5673dbf 100644 --- a/src/engine/engine_io.c +++ b/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; diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c new file mode 100644 index 00000000..dccf809c --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include +#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 +#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; +} diff --git a/src/engine/engine_island.h b/src/engine/engine_island.h new file mode 100644 index 00000000..20c0b303 --- /dev/null +++ b/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 +#include +#include + +#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_ diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 564559f6..1df38560 100644 --- a/src/engine/engine_print.c +++ b/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); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 5951d67d..4af5504c 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -66,7 +66,8 @@ const char* mjENABLESTRING[mjNENABLE] = { "Energy", "Fwdinv", "Sensornoise", - "MultiCCD" + "MultiCCD", + "Island" }; diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index eebb2768..e0d4cdd9 100644 --- a/src/engine/engine_vis_init.c +++ b/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"}, diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index d10e1a66..678ffe4e 100644 --- a/src/engine/engine_vis_state.c +++ b/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); + } + } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index d1a36964..ef45a723 100644 --- a/src/engine/engine_vis_visualize.c +++ b/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 - if (d->contact[i].efc_address >= 0) { - f2f(thisgeom->rgba, m->vis.rgba.contactpoint, 4); - } else { - f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4); + int efc_adr = d->contact[i].efc_address; + + // override standard colors if visualizing islands + if (vopt->flags[mjVIS_ISLAND] && d->nisland && efc_adr >= 0) { + // 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 - setMaterial(m, thisgeom, m->geom_matid[i], m->geom_rgba+4*i, vopt->flags); + // set material properties, override if visualizing islands + 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 - setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags); + // set material properties, override if visualizing islands + 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); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 18d0c33e..269e1dc3 100644 --- a/src/user/user_model.cc +++ b/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"}, {"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 } } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 5101d370..46d95280 100644 --- a/src/xml/xml_native_writer.cc +++ b/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 } diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 32e1746e..8678d4a1 100644 --- a/test/engine/CMakeLists.txt +++ b/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 diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index f28d6dff..7d47545b 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -24,15 +25,12 @@ #include #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 diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc new file mode 100644 index 00000000..72dacece --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include +#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 AsVector(const int* array, int n) { + return std::vector(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 diff --git a/test/engine/testdata/island/abacus.xml b/test/engine/testdata/island/abacus.xml new file mode 100644 index 00000000..9c451df0 --- /dev/null +++ b/test/engine/testdata/island/abacus.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/island/tendon_wrap.xml b/test/engine/testdata/island/tendon_wrap.xml new file mode 100644 index 00000000..7608fd8f --- /dev/null +++ b/test/engine/testdata/island/tendon_wrap.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/testdata/model.xml b/test/testdata/model.xml index 8ae54f4d..ba071dc6 100644 --- a/test/testdata/model.xml +++ b/test/testdata/model.xml @@ -1,6 +1,6 @@ diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 42dcb2b7..6e0e8066 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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_CONTACTSPLIT = 16, - mjVIS_TRANSPARENT = 17, - mjVIS_AUTOCONNECT = 18, - mjVIS_COM = 19, - mjVIS_SELECT = 20, - mjVIS_STATIC = 21, - mjVIS_SKIN = 22, - mjVIS_MIDPHASE = 23, - mjVIS_MESHBVH = 24, - mjNVISFLAG = 25, + mjVIS_ISLAND = 15, + mjVIS_CONTACTFORCE = 16, + mjVIS_CONTACTSPLIT = 17, + mjVIS_TRANSPARENT = 18, + mjVIS_AUTOCONNECT = 19, + mjVIS_COM = 20, + mjVIS_SELECT = 21, + mjVIS_STATIC = 22, + mjVIS_SKIN = 23, + mjVIS_MIDPHASE = 24, + 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);