chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 6 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
doc/includes/references.h
+7 -4
View File
@@ -345,12 +345,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)
@@ -2164,6 +2166,7 @@ struct mjvSceneState_ {
mjtByte* bvh_active;
int* island_dofadr;
int* island_dofind;
int* dof_island;
int* efc_island;
int* tendon_efcadr;
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
introspect/structs.py
+39 -18
View File
@@ -4339,20 +4339,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='first efc address involving tendon; -1: none (ntendon 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(
@@ -4361,11 +4347,25 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='island id of this dof; -1: none (nv x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='dof_islandnext',
name='island_dofnum',
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
doc='number of dofs in island (nisland x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='island_dofadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='start address in island_dofind (nisland x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='island_dofind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='island dof indices; -1: none (nv x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='efc_island',
@@ -4375,11 +4375,25 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='island id of this constraint (nefc x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='efc_islandnext',
name='island_efcnum',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='address of next constraint in island; -1: last (nefc x 1)', # pylint: disable=line-too-long
doc='number of constraints in island (nisland x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='island_efcadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='start address in island_efcind (nisland x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='island_efcind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='island constraint indices (nefc x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='efc_AR_rownnz',
@@ -6536,6 +6550,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='',
),
StructFieldDecl(
name='island_dofind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='',
),
StructFieldDecl(
name='dof_island',
type=PointerType(
src/engine/engine_island.c
+79 -81
View File
@@ -106,6 +106,36 @@ static void clearIsland(mjData* d, size_t parena) {
// allocate island arrays on arena, return 1 on success, 0 on failure
static int arenaAllocIsland(const mjModel* m, mjData* d) {
#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); \
return 0; \
}
MJDATA_ARENA_POINTERS_ISLAND
#undef X
#undef MJ_M
#define MJ_M(n) n
#undef MJ_D
#define MJ_D(n) n
return 1;
}
// return upper bound on number of tree-tree edges
static int countMaxEdge(const mjModel* m, const mjData* d) {
int nedge_max = 0;
@@ -362,112 +392,80 @@ void mj_island(const mjModel* m, mjData* d) {
int* stack = mj_stackAllocInt(d, nedge);
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; \
// allocate island arrays on arena
if (!arenaAllocIsland(m, d)) {
mjFREESTACK;
return;
}
MJDATA_ARENA_POINTERS_ISLAND
int nisland = d->nisland; // local copy
#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;
// compute dof_island, island_dofnum
int num_dof_unc = 0; // number of unconstrained dofs
memset(d->island_dofnum, 0, nisland*sizeof(int));
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;
// island_dofnum
if (island >= 0) {
d->island_dofnum[island]++;
} 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;
num_dof_unc++;
}
}
// compute island_dofadr
if (nisland) d->island_dofadr[0] = 0;
for (int i=1; i < nisland; i++) {
d->island_dofadr[i] = d->island_dofadr[i-1] + d->island_dofnum[i-1];
}
// reset island_dofnum
memset(d->island_dofnum, 0, nisland*sizeof(int));
// compute dof_islandind
int num_dof_island = 0;
for (int i=0; i < nv; i++) {
int island = d->dof_island[i];
if (island >= 0) {
d->island_dofind[d->island_dofadr[island] + (d->island_dofnum[island]++)] = i;
num_dof_island++;
}
}
// sanity check, SHOULD NOT OCCUR
if (nisland_found != d->nisland) {
if (num_dof_island + num_dof_unc != nv) {
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;
// finalize dof_islandind: set remaning indices to -1
for (int i=num_dof_island; i < nv; i++) {
d->island_dofind[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;
// compute efc_island, island_efcnum
memset(d->island_efcnum, 0, nisland*sizeof(int));
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;
}
d->island_efcnum[island]++;
}
// sanity check, SHOULD NOT OCCUR
if (nisland_found != d->nisland) {
mjERROR("not all islands assigned to constraints");
// compute island_efcadr
if (nisland) d->island_efcadr[0] = 0;
for (int i=1; i < nisland; i++) {
d->island_efcadr[i] = d->island_efcadr[i-1] + d->island_efcnum[i-1];
}
// 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;
// reset island_efcnum
memset(d->island_efcnum, 0, nisland*sizeof(int));
// compute efc_islandind
for (int i=0; i < nefc; i++) {
int island = d->efc_island[i];
d->island_efcind[d->island_efcadr[island] + (d->island_efcnum[island]++)] = i;
}
mjFREESTACK;
src/engine/engine_print.c
+28 -16
View File
@@ -1054,27 +1054,27 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
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");
fprintf(fp, NAME_FORMAT, "ISLAND_DOFNUM");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_dofnum[i]);
}
fprintf(fp, "\n\n");
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_DOFIND");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->dof_islandnext[i]);
fprintf(fp, " %d", d->island_dofind[i]);
}
fprintf(fp, "\n\n");
@@ -1084,9 +1084,21 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "EFC_ISLANDNEXT");
fprintf(fp, NAME_FORMAT, "ISLAND_EFCNUM");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_efcnum[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, "ISLAND_EFCIND");
for (int i = 0; i < d->nefc; i++) {
fprintf(fp, " %d", d->efc_islandnext[i]);
fprintf(fp, " %d", d->island_efcind[i]);
}
fprintf(fp, "\n\n");
}
src/engine/engine_vis_state.c
+6
View File
@@ -93,6 +93,7 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta
// buffer space required for islands
scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->island_dofadr) * m->ntree);
scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->island_dofind) * m->nv);
scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->dof_island) * m->nv);
scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->efc_island) * maxgeom * condimmax);
scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->tendon_efcadr) * m->ntendon);
@@ -126,6 +127,9 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta
scnstate->data.island_dofadr = (int*)ptr;
ptr += roundUpToCacheLine(sizeof(*scnstate->data.island_dofadr) * scnstate->model.ntree);
scnstate->data.island_dofind = (int*)ptr;
ptr += roundUpToCacheLine(sizeof(*scnstate->data.island_dofind) * scnstate->model.nv);
scnstate->data.dof_island = (int*)ptr;
ptr += roundUpToCacheLine(sizeof(*scnstate->data.dof_island) * scnstate->model.nv);
@@ -204,6 +208,7 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData*
d->contact = scnstate->data.contact;
d->efc_force = scnstate->data.efc_force;
d->island_dofadr = scnstate->data.island_dofadr;
d->island_dofind = scnstate->data.island_dofind;
d->dof_island = scnstate->data.dof_island;
d->efc_island = scnstate->data.efc_island;
d->tendon_efcadr = scnstate->data.tendon_efcadr;
@@ -358,6 +363,7 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
scnstate->data.nisland = d->nisland;
if (d->nisland) {
memcpy(scnstate->data.island_dofadr, d->island_dofadr, sizeof(*d->island_dofadr) * d->nisland);
memcpy(scnstate->data.island_dofind, d->island_dofind, sizeof(*d->island_dofind) * m->nv);
memcpy(scnstate->data.dof_island, d->dof_island, sizeof(*d->dof_island) * m->nv);
memcpy(scnstate->data.tendon_efcadr, d->tendon_efcadr, sizeof(*d->tendon_efcadr) * m->ntendon);
}
src/engine/engine_vis_visualize.c
+4 -3
View File
@@ -133,7 +133,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// 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]]);
islandColor(thisgeom->rgba, d->island_dofind[d->island_dofadr[d->efc_island[efc_adr]]]);
}
// otherwise regular colors (different for included and excluded contacts)
@@ -1182,7 +1182,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
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]);
islandColor(rgba_island, d->island_dofind[d->island_dofadr[island]]);
}
}
}
@@ -1583,7 +1583,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba = rgba_island;
if (d->tendon_efcadr[i] != -1) {
// set color using island's first dof
islandColor(rgba_island, d->island_dofadr[d->efc_island[d->tendon_efcadr[i]]]);
int island = d->efc_island[d->tendon_efcadr[i]];
islandColor(rgba_island, d->island_dofind[d->island_dofadr[island]]);
}
}
setMaterial(m, thisgeom, tendon_matid, rgba, vopt->flags);
test/engine/engine_island_test.cc
+27 -16
View File
@@ -214,24 +214,29 @@ TEST_F(IslandTest, Abacus) {
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));
// the islands begin at dofs 0 and 1
EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 1));
// number of dofs in the 2 islands
EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(1, 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));
// dof 0 constitutes first island
// dofs 2, 3 are the second island
// last index is unassigned since dof 1 is unconstrained
EXPECT_THAT(AsVector(data->island_dofind, nv), ElementsAre(0, 2, 3, -1));
// island 0 starts at constraint 0
// island 1 starts at constraint 4
EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4));
// number of constraints in the 2 islands
EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(4, 8));
// first contact (4 constraints) is in island 0
// second contact (8 constraints) is in island 1
EXPECT_THAT(AsVector(data->efc_island, nefc),
@@ -239,8 +244,8 @@ TEST_F(IslandTest, Abacus) {
// 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));
EXPECT_THAT(AsVector(data->island_efcind, nefc),
ElementsAre(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11));
// reset, push 0 to the left, 3 to the right, 1,2 to the middle
mj_resetData(model, data);
@@ -260,13 +265,15 @@ TEST_F(IslandTest, Abacus) {
EXPECT_EQ(nisland, 3);
EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 1, 3));
EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(1, 2, 1));
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_dofind, nv), ElementsAre(0, 1, 2, 3));
EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 4, 8));
EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(4, 4, 4));
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));
EXPECT_THAT(AsVector(data->island_efcind, nefc),
ElementsAre(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11));
mj_deleteData(data);
mj_deleteModel(model);
@@ -304,16 +311,20 @@ TEST_F(IslandTest, DenseSparse) {
EXPECT_EQ(data1->nefc, data2->nefc);
EXPECT_EQ(AsVector(data1->island_dofadr, nisland),
AsVector(data2->island_dofadr, nisland));
EXPECT_EQ(AsVector(data1->island_dofnum, nisland),
AsVector(data2->island_dofnum, 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_dofind, nv),
AsVector(data2->island_dofind, nv));
EXPECT_EQ(AsVector(data1->island_efcadr, nisland),
AsVector(data2->island_efcadr, nisland));
EXPECT_EQ(AsVector(data1->island_efcnum, nisland),
AsVector(data2->island_efcnum, 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));
EXPECT_EQ(AsVector(data1->island_efcind, nefc),
AsVector(data2->island_efcind, nefc));
mj_deleteData(data2);
mj_deleteData(data1);
unity/Runtime/Bindings/MjBindings.cs
+7 -4
View File
@@ -1715,12 +1715,14 @@ public unsafe struct mjData_ {
public double* efc_D;
public double* efc_R;
public int* tendon_efcadr;
public int* island_dofadr;
public int* island_efcadr;
public int* dof_island;
public int* dof_islandnext;
public int* island_dofnum;
public int* island_dofadr;
public int* island_dofind;
public int* efc_island;
public int* efc_islandnext;
public int* island_efcnum;
public int* island_efcadr;
public int* island_efcind;
public int* efc_AR_rownnz;
public int* efc_AR_rowadr;
public int* efc_AR_colind;
@@ -2969,6 +2971,7 @@ public unsafe struct data {
public double* wrap_xpos;
public byte* bvh_active;
public int* island_dofadr;
public int* island_dofind;
public int* dof_island;
public int* efc_island;
public int* tendon_efcadr;