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

Refactor constraint island representation.

Browse Source 
Required for constraint solver islanding.

Replace linked lists with `island_{dof,efc}_{num,adr,ind}`, corresponding to the standard `{rownnz,rowadr,colind}` sparse matrix representation. By effectively defining two sparse matrix structures of dimensions `nisland x nv` and `nisland x nefc`, respectively, this representation is more conducive to reuse of existing sparse matrix utility functions, while being cache-friendlier by making sequential indices adjacent in memory.

PiperOrigin-RevId: 559704957
Change-Id: I919362cbef0d5fe5acc4aa2bb4ffb17eee8223a2
This commit is contained in:
Yuval Tassa
2023-08-24 03:48:10 -07:00
committed by Copybara-Service
parent b92a9d599c
commit 1dad5993f0
11 changed files with 248 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/mujoco/mjdata.h
+6 -4
View File
@@ -372,12 +372,14 @@ struct mjData_ {
int* tendon_efcadr; // first efc address involving tendon; -1: none (ntendon 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* island_dofnum; // number of dofs in island (nisland x 1)
int* island_dofadr; // start address in island_dofind (nisland x 1)
int* island_dofind; // island dof indices; -1: 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)
int* island_efcnum; // number of constraints in island (nisland x 1)
int* island_efcadr; // start address in island_efcind (nisland x 1)
int* island_efcind; // island constraint indices (nefc x 1)
// computed by mj_projectConstraint (dual solver)
int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1)
include/mujoco/mjvisualize.h
+1
View File
@@ -588,6 +588,7 @@ struct mjvSceneState_ {
mjtByte* bvh_active;
int* island_dofadr;
int* island_dofind;
int* dof_island;
int* efc_island;
int* tendon_efcadr;
include/mujoco/mjxmacro.h
+44 -42
View File
@@ -565,54 +565,56 @@
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( int, tendon_efcadr, MJ_M(ntendon), 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( int, tendon_efcadr, MJ_M(ntendon), 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 \
X( int, efc_AR_rownnz, MJ_D(nefc), 1 ) \
X( int, efc_AR_rowadr, MJ_D(nefc), 1 ) \
X( int, efc_AR_colind, MJ_D(nefc), MJ_D(nefc) ) \
X( mjtNum, efc_AR, MJ_D(nefc), MJ_D(nefc) )
#define MJDATA_ARENA_POINTERS_DUAL \
X( int, efc_AR_rownnz, MJ_D(nefc), 1 ) \
X( int, efc_AR_rowadr, MJ_D(nefc), 1 ) \
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 )
#define MJDATA_ARENA_POINTERS_ISLAND \
X( int, dof_island, MJ_M(nv), 1 ) \
X( int, island_dofnum, MJ_D(nisland), 1 ) \
X( int, island_dofadr, MJ_D(nisland), 1 ) \
X( int, island_dofind, MJ_M(nv), 1 ) \
X( int, efc_island, MJ_D(nefc), 1 ) \
X( int, island_efcnum, MJ_D(nisland), 1 ) \
X( int, island_efcadr, MJ_D(nisland), 1 ) \
X( int, island_efcind, 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 \
#define MJDATA_ARENA_POINTERS \
MJDATA_ARENA_POINTERS_CONTACT \
MJDATA_ARENA_POINTERS_PRIMAL \
MJDATA_ARENA_POINTERS_DUAL \
MJDATA_ARENA_POINTERS_ISLAND