chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add constraint island discovery

Browse Source 
PiperOrigin-RevId: 557067599
Change-Id: Ic41e1d0efef02b7a79142518afe49cf9d4e74725
This commit is contained in:
Yuval Tassa
2023-08-15 02:29:26 -07:00
committed by Copybara-Service
parent e4dddea42a
commit 3e034e38b2
40 changed files with 1557 additions and 468 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+9
View File
@@ -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
+12 -5
View File
@@ -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 <https://youtu.be/Vc1tq0fFvQA>`__.
In a future release, the constraint solver will exploit the disjoint nature of constraint islands.
.. _body:
doc/XMLschema.rst
+1 -1
View File
@@ -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
+13 -3
View File
@@ -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.
(`#870 <https://github.com/deepmind/mujoco/issues/870>`_)
4. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
camera name used the default camera.
Version 2.3.7 (July 20, 2023)
-----------------------------
doc/includes/references.h
+23 -3
View File
@@ -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
+9
View File
@@ -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
+7 -3
View File
@@ -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
+6
View File
@@ -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
+43 -29
View File
@@ -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 )
include/mujoco/mujoco.h
+3
View File
@@ -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
+16 -13
View File
@@ -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',
introspect/enums_test.py
+2 -1
View File
@@ -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
+20
View File
@@ -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
+95 -3
View File
@@ -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
+1 -1
View File
@@ -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
+1
View File
@@ -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
+9 -2
View File
@@ -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
+8 -1
View File
@@ -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
+2
View File
@@ -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
+9 -130
View File
@@ -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
+1 -6
View File
@@ -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
+4
View File
@@ -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
+1
View File
@@ -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
+521
View File
@@ -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
+46
View File
@@ -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
+42 -2
View File
@@ -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
+2 -1
View File
@@ -66,7 +66,8 @@ const char* mjENABLESTRING[mjNENABLE] = {
"Energy",
"Fwdinv",
"Sensornoise",
"MultiCCD"
"MultiCCD",
"Island"
};
src/engine/engine_vis_init.c
+3 -1
View File
@@ -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
+16
View File
@@ -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
+97 -15
View File
@@ -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);
src/user/user_model.cc
+20
View File
@@ -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
+3 -2
View File
@@ -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
+1
View File
@@ -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
+3
View File
@@ -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
+1 -231
View File
@@ -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
+381
View File
@@ -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
Vendored
+35
View File
@@ -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
Vendored
+56
View File
@@ -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
Vendored
+1 -1
View File
@@ -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
+34 -14
View File
@@ -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);