chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 6 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
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);