diff --git a/doc/includes/references.h b/doc/includes/references.h index f02d7410..4bc95ddc 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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; diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 204222b9..4a47d8af 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -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) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index f406aa84..ff1a745c 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -588,6 +588,7 @@ struct mjvSceneState_ { mjtByte* bvh_active; int* island_dofadr; + int* island_dofind; int* dof_island; int* efc_island; int* tendon_efcadr; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index c6a222bb..f13bddae 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 diff --git a/introspect/structs.py b/introspect/structs.py index ea99631e..58fd605d 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -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( diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index ca78a119..a83e09be 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -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; diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 6a41d8de..e6ae3c6f 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -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"); } diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 444769b6..36409a17 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -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); } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index a83850f3..19749a95 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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); diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index 20d235e7..02f09beb 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -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); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 88514ba1..79dc3be8 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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;