From 96bf8aea8134dc6afa7503c4cb130d23f7dc163b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 30 May 2026 03:07:31 -0700 Subject: [PATCH] Move island-specific sparse matrices from arena to stack. PiperOrigin-RevId: 923850345 Change-Id: I9683d7554b15b7cd8c45a8dce7814640aa266452 --- doc/changelog.rst | 6 +- doc/includes/references.h | 11 --- include/mujoco/mjdata.h | 11 --- include/mujoco/mjxmacro.h | 11 --- python/mujoco/introspect/structs.py | 88 ------------------ src/engine/engine_island.c | 35 ------- src/engine/engine_print.c | 47 +++++----- src/engine/engine_solver.c | 127 ++++++++++++++++++------- test/engine/engine_island_test.cc | 133 --------------------------- unity/Runtime/Bindings/MjBindings.cs | 11 --- wasm/codegen/generated/bindings.cc | 44 --------- 11 files changed, 122 insertions(+), 402 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index c5b5377d..5862cf42 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -19,7 +19,11 @@ General - The header file ``mjthread.h`` was removed along with the engine threading API. - **Migration:** Use :ref:`mju_threadpool` to set number of worker threads for the engine. + **Migration:** Use :ref:`mju_threadpool` to set number of worker threads for the engine. + + - Moved island sparse matrix construction from :ref:`mj_island` (single threaded) into :ref:`mj_fwdConstraint` + (multi-threaded). The island-specific matrices ``iM, iLD, iefc_J`` were removed from the arena and are now + allocated on the stack. Bug fixes ^^^^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index 1ce969d7..03f478c5 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -341,12 +341,6 @@ struct mjData_ { // computed by mj_island (dofs sorted by island) mjtNum* ifrc_smooth; // net unconstrained force (nidof x 1) mjtNum* iacc_smooth; // unconstrained acceleration (nidof x 1) - int* iM_rownnz; // inertia: non-zeros in each row (nidof x 1) - int* iM_rowadr; // inertia: address of each row in iM_colind (nidof x 1) - int* iM_colind; // inertia: column indices of non-zeros (nC x 1) - mjtNum* iM; // total inertia (sparse) (nC x 1) - mjtNum* iLD; // L'*D*L factorization of M (sparse) (nC x 1) - mjtNum* iLDiagInv; // 1/diag(D) (nidof x 1) mjtNum* iacc; // acceleration (nidof x 1) // computed by mj_island (island constraint structure) @@ -361,11 +355,6 @@ struct mjData_ { // computed by mj_island (constraints sorted by island) int* iefc_type; // constraint type (mjtConstraint) (nefc x 1) int* iefc_id; // id of object of specified type (nefc x 1) - int* iefc_J_rownnz; // number of non-zeros in constraint Jacobian row (nefc x 1) - int* iefc_J_rowadr; // row start address in colind array (nefc x 1) - int* iefc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1) - int* iefc_J_colind; // column indices in constraint Jacobian (nJ x 1) - mjtNum* iefc_J; // constraint Jacobian (nJ x 1) mjtNum* iefc_frictionloss; // frictionloss (friction) (nefc x 1) mjtNum* iefc_D; // constraint mass (nefc x 1) mjtNum* iefc_R; // inverse constraint mass (nefc x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 2ee67f95..aab0709a 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -364,12 +364,6 @@ struct mjData_ { // computed by mj_island (dofs sorted by island) mjtNum* ifrc_smooth; // net unconstrained force (nidof x 1) mjtNum* iacc_smooth; // unconstrained acceleration (nidof x 1) - int* iM_rownnz; // inertia: non-zeros in each row (nidof x 1) - int* iM_rowadr; // inertia: address of each row in iM_colind (nidof x 1) - int* iM_colind; // inertia: column indices of non-zeros (nC x 1) - mjtNum* iM; // total inertia (sparse) (nC x 1) - mjtNum* iLD; // L'*D*L factorization of M (sparse) (nC x 1) - mjtNum* iLDiagInv; // 1/diag(D) (nidof x 1) mjtNum* iacc; // acceleration (nidof x 1) // computed by mj_island (island constraint structure) @@ -384,11 +378,6 @@ struct mjData_ { // computed by mj_island (constraints sorted by island) int* iefc_type; // constraint type (mjtConstraint) (nefc x 1) int* iefc_id; // id of object of specified type (nefc x 1) - int* iefc_J_rownnz; // number of non-zeros in constraint Jacobian row (nefc x 1) - int* iefc_J_rowadr; // row start address in colind array (nefc x 1) - int* iefc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1) - int* iefc_J_colind; // column indices in constraint Jacobian (nJ x 1) - mjtNum* iefc_J; // constraint Jacobian (nJ x 1) mjtNum* iefc_frictionloss; // frictionloss (friction) (nefc x 1) mjtNum* iefc_D; // constraint mass (nefc x 1) mjtNum* iefc_R; // inverse constraint mass (nefc x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 04539f5e..ec3e0da3 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -969,12 +969,6 @@ X ( int, map_idof2dof, MJ_M(nv), 1 ) \ X ( mjtNum, ifrc_smooth, MJ_D(nidof), 1 ) \ X ( mjtNum, iacc_smooth, MJ_D(nidof), 1 ) \ - XNV( int, iM_rownnz, MJ_D(nidof), 1 ) \ - XNV( int, iM_rowadr, MJ_D(nidof), 1 ) \ - XNV( int, iM_colind, MJ_M(nC), 1 ) \ - XNV( mjtNum, iM, MJ_M(nC), 1 ) \ - XNV( mjtNum, iLD, MJ_M(nC), 1 ) \ - X ( mjtNum, iLDiagInv, MJ_D(nidof), 1 ) \ X ( mjtNum, iacc, MJ_D(nidof), 1 ) \ X ( int, efc_island, MJ_D(nefc), 1 ) \ X ( int, island_ne, MJ_D(nisland), 1 ) \ @@ -985,11 +979,6 @@ X ( int, map_iefc2efc, MJ_D(nefc), 1 ) \ X ( int, iefc_type, MJ_D(nefc), 1 ) \ X ( int, iefc_id, MJ_D(nefc), 1 ) \ - XNV( int, iefc_J_rownnz, MJ_D(nefc), 1 ) \ - XNV( int, iefc_J_rowadr, MJ_D(nefc), 1 ) \ - XNV( int, iefc_J_rowsuper, MJ_D(nefc), 1 ) \ - XNV( int, iefc_J_colind, MJ_D(nJ), 1 ) \ - XNV( mjtNum, iefc_J, MJ_D(nJ), 1 ) \ X ( mjtNum, iefc_frictionloss, MJ_D(nefc), 1 ) \ X ( mjtNum, iefc_D, MJ_D(nefc), 1 ) \ X ( mjtNum, iefc_R, MJ_D(nefc), 1 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 8fa54694..78f97fd0 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -6541,54 +6541,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='unconstrained acceleration', array_extent=('nidof',), ), - StructFieldDecl( - name='iM_rownnz', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='inertia: non-zeros in each row', - array_extent=('nidof',), - ), - StructFieldDecl( - name='iM_rowadr', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='inertia: address of each row in iM_colind', - array_extent=('nidof',), - ), - StructFieldDecl( - name='iM_colind', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='inertia: column indices of non-zeros', - array_extent=('nC',), - ), - StructFieldDecl( - name='iM', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='total inertia (sparse)', - array_extent=('nC',), - ), - StructFieldDecl( - name='iLD', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc="L'*D*L factorization of M (sparse)", - array_extent=('nC',), - ), - StructFieldDecl( - name='iLDiagInv', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='1/diag(D)', - array_extent=('nidof',), - ), StructFieldDecl( name='iacc', type=PointerType( @@ -6669,46 +6621,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='id of object of specified type', array_extent=('nefc',), ), - StructFieldDecl( - name='iefc_J_rownnz', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='number of non-zeros in constraint Jacobian row', - array_extent=('nefc',), - ), - StructFieldDecl( - name='iefc_J_rowadr', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='row start address in colind array', - array_extent=('nefc',), - ), - StructFieldDecl( - name='iefc_J_rowsuper', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='number of subsequent rows in supernode', - array_extent=('nefc',), - ), - StructFieldDecl( - name='iefc_J_colind', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='column indices in constraint Jacobian', - array_extent=('nJ',), - ), - StructFieldDecl( - name='iefc_J', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='constraint Jacobian', - array_extent=('nJ',), - ), StructFieldDecl( name='iefc_frictionloss', type=PointerType( diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index a8aff1ce..88ee12e9 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -526,15 +526,6 @@ void mj_island(const mjModel* m, mjData* d) { d->island_dofadr[i] = d->map_idof2dof[d->island_idofadr[i]]; } - // inertia: block-diagonalize both iLD <- qLD and iM <- M - mju_blockDiagSparse(d->iLD, d->iM_rownnz, d->iM_rowadr, d->iM_colind, - d->qLD, m->M_rownnz, m->M_rowadr, m->M_colind, - nidof, nisland, - d->map_idof2dof, d->map_dof2idof, - d->island_idofadr, d->island_idofadr, - d->iM, d->M); - mju_gather(d->iLDiagInv, d->qLDiagInv, d->map_idof2dof, nidof); - // ------------------------------------- constraints --------------------------------------------- @@ -578,32 +569,6 @@ void mj_island(const mjModel* m, mjData* d) { // SHOULD NOT OCCUR if (!mju_compare(island_nefc2, d->island_nefc, nisland)) mjERROR("island_nefc miscount"); - // dense: block-diagonalize Jacobian - if (!mj_isSparse(m)) { - mju_blockDiag(d->iefc_J, d->efc_J, - nv, nidof, nisland, - d->map_iefc2efc, d->map_idof2dof, - d->island_nefc, d->island_nv, - d->island_iefcadr, d->island_idofadr); - } - - // sparse - else { - // block-diagonalize Jacobian - mju_blockDiagSparse(d->iefc_J, d->iefc_J_rownnz, d->iefc_J_rowadr, d->iefc_J_colind, - d->efc_J, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, - nefc, nisland, - d->map_iefc2efc, d->map_dof2idof, - d->island_iefcadr, d->island_idofadr, NULL, NULL); - - // recompute rowsuper per island - for (int island=0; island < nisland; island++) { - int adr = d->island_iefcadr[island]; - mju_superSparse(d->island_nefc[island], d->iefc_J_rowsuper + adr, - d->iefc_J_rownnz + adr, d->iefc_J_rowadr + adr, d->iefc_J_colind); - } - } - // copy position-dependent efc vectors required by solver mju_gatherInt(d->iefc_type, d->efc_type, d->map_iefc2efc, nefc); mju_gatherInt(d->iefc_id, d->efc_id, d->map_iefc2efc, nefc); diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 776d5281..f6ca332b 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -190,11 +190,12 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, // print block-diagonal dense matrix, embedded in a larger matrix -static void printBlockArray(const char* str, const mjtNum* data, int nr, int nc, +static void printBlockArray(const char* str, const mjtNum* data, int nc, int nisland, const int* island_nr, const int* island_nc, const int* island_r, const int* island_c, + const int* map_r, const int* map_c, FILE* fp, const char* float_format) { - if (!data || !nr || !nc) { + if (!data || !nisland) { return; } @@ -209,7 +210,6 @@ static void printBlockArray(const char* str, const mjtNum* data, int nr, int nc, int bnc = island_nc[b]; int r_start = island_r[b]; int c_start = island_c[b]; - const mjtNum* data_ptr = data + r_start * nc; // print rows for this block for (int r_block = 0; r_block < bnr; r_block++) { @@ -220,10 +220,13 @@ static void printBlockArray(const char* str, const mjtNum* data, int nr, int nc, fprintf(fp, " "); } + int row = map_r[r_start + r_block]; + // block data for (int c = 0; c < bnc; c++) { + int col = map_c[c_start + c]; fprintf(fp, " "); - fprintf(fp, float_format, *data_ptr++); + fprintf(fp, float_format, data[row * nc + col]); } // trailing dots @@ -318,6 +321,7 @@ void mj_printBlockSparsity(const char* str, int nr, int nc, int nisland, const int* island_col_offset, const int* entity_island, const int* map_row_to_entity, + const int* map_col_to_entity, const int* rownnz, const int* rowadr, const int* colind, const int* rowsuper, FILE* fp) { // if no rows / columns, or too many columns to be visually useful, return @@ -343,27 +347,25 @@ void mj_printBlockSparsity(const char* str, int nr, int nc, int nisland, int c_start = island_col_offset[island]; int bnc = island_block_ncols[island]; - int current_nnz = 0; - int adr = rowadr[r]; + int adr = rowadr[entity_r]; + int nnz = rownnz[entity_r]; char nz_char = (island < 10) ? ('0' + island) : 'x'; - for (int c = 0; c < nc; c++) { // c is the global column index + for (int c = 0; c < nc; c++) { // c is the block-space column index bool nonzero = false; if (c >= c_start && c < c_start + bnc) { - int c_block = c - c_start; // c_block is the island-local column index - - // search for c_block in colind for the current row r - while (current_nnz < rownnz[r] && colind[adr + current_nnz] < c_block) { - current_nnz++; - } - if (current_nnz < rownnz[r] && colind[adr + current_nnz] == c_block) { - nonzero = true; + int target_col = map_col_to_entity[c]; + for (int i = 0; i < nnz; i++) { + if (colind[adr + i] == target_col) { + nonzero = true; + break; + } } } fprintf(fp, "%c", nonzero ? nz_char : ' '); } fprintf(fp, " |"); - if (rowsuper && rowsuper[r] > 0) fprintf(fp, " %d", rowsuper[r]); + if (rowsuper && rowsuper[entity_r] > 0) fprintf(fp, " %d", rowsuper[entity_r]); fprintf(fp, "\n"); } for (int c = 0; c < nc + 2; c++) fprintf(fp, "-"); @@ -1477,8 +1479,8 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena mj_printBlockSparsity("iM: block-diagonal inertia (nnzs are island ids)", d->nidof, d->nidof, d->nisland, d->island_nv, d->island_idofadr, - d->dof_island, d->map_idof2dof, - d->iM_rownnz, d->iM_rowadr, d->iM_colind, NULL, fp); + d->dof_island, d->map_idof2dof, d->map_idof2dof, + m->M_rownnz, m->M_rowadr, m->M_colind, NULL, fp); } if (!mju_isZero(d->qHDiagInv, m->nv)) { @@ -1555,9 +1557,10 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena if (!mj_isSparse(m)) { printArray2d("EFC_J", d->nefc, m->nv, d->efc_J, fp, float_format); if (d->nisland) { - printBlockArray("IEFC_J", d->iefc_J, d->nefc, d->nidof, + printBlockArray("IEFC_J", d->efc_J, m->nv, d->nisland, d->island_nefc, d->island_nv, d->island_iefcadr, d->island_idofadr, + d->map_iefc2efc, d->map_idof2dof, fp, float_format); } printArray2d("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format); @@ -1577,9 +1580,9 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena mj_printBlockSparsity("IEFC_J: block-diagonalized constraint Jacobian (nnzs are island ids)", d->nefc, d->nidof, d->nisland, d->island_nv, d->island_idofadr, - d->efc_island, d->map_iefc2efc, - d->iefc_J_rownnz, d->iefc_J_rowadr, d->iefc_J_colind, - d->iefc_J_rowsuper, fp); + d->efc_island, d->map_iefc2efc, d->map_idof2dof, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, + d->efc_J_rowsuper, fp); } if (mj_isDual(m)) { diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index b9c112a1..5c7fa0ea 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -877,12 +877,12 @@ typedef struct { mjtNum* qacc; // inertia - const int* M_rownnz; - const int* M_rowadr; - const int* M_colind; - const mjtNum* M; - const mjtNum* qLD; - const mjtNum* qLDiagInv; + int* M_rownnz; + int* M_rowadr; + int* M_colind; + mjtNum* M; + mjtNum* qLD; + mjtNum* qLDiagInv; // efc arrays const mjtNum* efc_D; @@ -895,11 +895,11 @@ typedef struct { int* efc_state; // Jacobians - const int* J_rownnz; - const int* J_rowadr; - const int* J_rowsuper; - const int* J_colind; - const mjtNum* J; + int* J_rownnz; + int* J_rowadr; + int* J_rowsuper; + int* J_colind; + mjtNum* J; int* JT_rownnz; int* JT_rowadr; int* JT_rowsuper; @@ -1031,14 +1031,6 @@ static void PrimalPointers(const mjModel* m, const mjData* d, mjPrimalContext* c ctx->qacc_smooth = d->iacc_smooth + idofadr; ctx->qacc = d->iacc + idofadr; - // inertia - ctx->M_rownnz = d->iM_rownnz + idofadr; - ctx->M_rowadr = d->iM_rowadr + idofadr; - ctx->M_colind = d->iM_colind; - ctx->M = d->iM; - ctx->qLD = d->iLD; - ctx->qLDiagInv = d->iLDiagInv + idofadr; - // efc arrays int iefcadr = d->island_iefcadr[island]; ctx->efc_D = d->iefc_D + iefcadr; @@ -1049,32 +1041,44 @@ static void PrimalPointers(const mjModel* m, const mjData* d, mjPrimalContext* c ctx->efc_type = d->iefc_type + iefcadr; ctx->efc_force = d->iefc_force + iefcadr; ctx->efc_state = d->iefc_state + iefcadr; - - // Jacobians - if (!ctx->is_sparse) { - ctx->J = d->iefc_J + d->nidof * iefcadr; - } else { - ctx->J_rownnz = d->iefc_J_rownnz + iefcadr; - ctx->J_rowadr = d->iefc_J_rowadr + iefcadr; - ctx->J_rowsuper = d->iefc_J_rowsuper + iefcadr; - ctx->J_colind = d->iefc_J_colind; - ctx->J = d->iefc_J; - ctx->nJ = ctx->J_rowadr[ctx->nefc-1] + ctx->J_rownnz[ctx->nefc-1] - - ctx->J_rowadr[0]; - } } } // allocate fixed-size arrays in mjPrimalContext // mj_{mark/free}Stack in calling function! -static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { +static void PrimalAllocate(const mjModel* m, mjData* d, mjPrimalContext* ctx, int flg_Newton) { // local sizes and flags int nv = ctx->nv; int nefc = ctx->nefc; - int nJ = ctx->is_sparse ? ctx->nJ : 0; int is_sparse = ctx->is_sparse; int is_elliptic = ctx->is_elliptic; + int nJ = is_sparse ? d->nJ : 0; + + // compute island matrix sizes if needed + int nC = 0; + if (ctx->island >= 0) { + // count nC: number of nonzeros in M block of island (always sparse) + int island = ctx->island; + int idofadr = d->island_idofadr[island]; + for (int i = 0; i < nv; i++) { + int dof = d->map_idof2dof[idofadr + i]; + nC += m->M_rownnz[dof]; + } + + // count nJ: number of nonzeros in J block of island (sparse or dense) + if (is_sparse) { + nJ = 0; + int iefcadr = d->island_iefcadr[island]; + for (int i = 0; i < nefc; i++) { + int efc = d->map_iefc2efc[iefcadr + i]; + nJ += d->efc_J_rownnz[efc]; + } + } else { + nJ = nefc * nv; + } + ctx->nJ = nJ; + } // compute mjtNum block size size_t nNum = 5*nefc + 5*nv; // common arrays @@ -1090,6 +1094,11 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { nNum += 3*nv; // CG arrays } + // add island matrix sizes + if (ctx->island >= 0) { + nNum += 2 * nC + nv + nJ; // iM, iLD, iLDiagInv, iefc_J + } + // compute int block size size_t nInt = nefc; // oldstate if (is_sparse) { @@ -1097,10 +1106,58 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { if (flg_Newton) nInt += 8*nv; // Newton sparse } + // add island matrix sizes + if (ctx->island >= 0) { + nInt += 2 * nv + nC; // iM_{rownnz, rowadr, colind} + if (is_sparse) { + nInt += 3 * nefc + nJ; // iefc_J_{rownnz, rowadr, rowsuper, colind} + } + } + // allocate mjtNum and int blocks mjtNum* numblock = mjSTACKALLOC(d, nNum, mjtNum); int* intblock = mjSTACKALLOC(d, nInt, int); + // populate island matrices if needed + if (ctx->island >= 0) { + int island = ctx->island; + + int idofadr = d->island_idofadr[island]; + int iefcadr = d->island_iefcadr[island]; + + ctx->M_rownnz = intblock; intblock += nv; + ctx->M_rowadr = intblock; intblock += nv; + ctx->M_colind = intblock; intblock += nC; + + ctx->M = numblock; numblock += nC; + ctx->qLD = numblock; numblock += nC; + ctx->qLDiagInv = numblock; numblock += nv; + + mju_blockSparse(ctx->qLD, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, + d->qLD, m->M_rownnz, m->M_rowadr, m->M_colind, + nv, d->map_idof2dof + idofadr, d->map_dof2idof, + d->island_idofadr[island], 0, ctx->M, d->M); + mju_gather(ctx->qLDiagInv, d->qLDiagInv, d->map_idof2dof + idofadr, nv); + + ctx->J = numblock; numblock += nJ; + if (!is_sparse) { + mju_block(ctx->J, d->efc_J, m->nv, nv, nefc, + d->map_iefc2efc + iefcadr, d->map_idof2dof + idofadr); + } else { + ctx->J_rownnz = intblock; intblock += nefc; + ctx->J_rowadr = intblock; intblock += nefc; + ctx->J_rowsuper = intblock; intblock += nefc; + ctx->J_colind = intblock; intblock += nJ; + + mju_blockSparse(ctx->J, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, + d->efc_J, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, + nefc, d->map_iefc2efc + iefcadr, d->map_dof2idof, + d->island_idofadr[island], 0, NULL, NULL); + + mju_superSparse(nefc, ctx->J_rowsuper, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind); + } + } + // carve mjtNum block ctx->Jaref = numblock; numblock += nefc; ctx->Jv = numblock; numblock += nefc; @@ -1962,7 +2019,7 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i // make context PrimalPointers(m, d, &ctx, island); - PrimalAllocate(d, &ctx, flg_Newton); + PrimalAllocate(m, d, &ctx, flg_Newton); // local copies int nv = ctx.nv; diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index e6592b2b..227b350d 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -400,139 +400,6 @@ TEST_F(IslandTest, IslandFlex) { mj_deleteModel(model); } -static const char* const k2H100Path = "engine/testdata/island/2humanoid100.xml"; - -TEST_F(IslandTest, IslandJacobian) { - for (const char* local_path : {kIlslandEfcPath, k2H100Path}) { - const std::string xml_path = GetTestDataFilePath(local_path); - char error[1024]; - mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - int jac0 = m->opt.jacobian; - mjData* d = mj_makeData(m); - - for (mjtNum t_stop : {0.0, 0.2, 2.0}) { - while (d->time < t_stop) { - mj_step(m, d); - } - - for (mjtJacobian jac : {mjJAC_DENSE, mjJAC_SPARSE}) { - m->opt.jacobian = jac; - mj_forward(m, d); - - int nv = m->nv; - int nefc = d->nefc; - int nisland = d->nisland; - int nidof = d->nidof; - - mjtNum* J = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc * nv); - mjtNum* iJ = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc * nidof); - - // get local dense Jacobian - if (jac == mjJAC_DENSE) { - mju_copy(J, d->efc_J, nefc * nv); - mju_copy(iJ, d->iefc_J, nefc * nidof); - } else { - mju_sparse2dense(J, d->efc_J, nefc, nv, d->efc_J_rownnz, - d->efc_J_rowadr, d->efc_J_colind); - } - - // compare random access in efc_J to contiguous memory in iefc_J - for (int island=0; island < nisland; island++) { - int idof = d->island_idofadr[island]; - int iefc = d->island_iefcadr[island]; - int nefc_island = d->island_nefc[island]; - int nv_island = d->island_nv[island]; - - // === test J - - // get pointer to J_island, dense (nefc_island x nv_island) submatrix - mjtNum* J_island; - if (jac == mjJAC_DENSE) { - // point to starting address of island in efc_J - J_island = iJ + iefc * nidof; - } else { - // dense copy of island in iJ (here used as scratch) - mju_sparse2dense(iJ, d->iefc_J, nefc_island, nv_island, - d->iefc_J_rownnz + iefc, - d->iefc_J_rowadr + iefc, - d->iefc_J_colind); - J_island = iJ; - } - - // sequential memory in J_island equals random access memory in J - for (int i=0; i < nefc_island; i++) { - for (int j=0; j < nv_island; j++) { - int efc = d->map_iefc2efc[iefc + i]; - int dof = d->map_idof2dof[idof + j]; - EXPECT_EQ(J_island[i * nv_island + j], J[efc * nv + dof]); - } - } - } - - mju_free(iJ); - mju_free(J); - } - - // reset opt.jacobian to initial value - m->opt.jacobian = jac0; - } - - mj_deleteData(d); - mj_deleteModel(m); - } -} - -TEST_F(IslandTest, IslandInertia) { - for (const char* local_path : {kIlslandEfcPath, k2H100Path}) { - const std::string xml_path = GetTestDataFilePath(local_path); - char error[1024]; - mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - int nv = m->nv; - mjData* d = mj_makeData(m); - mjtNum* M = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv * nv); - - for (mjtNum t_stop : {0.0, 0.2, 2.0}) { - while (d->time < t_stop) { - mj_step(m, d); - } - mj_forward(m, d); - - int nisland = d->nisland; - - // get dense inertia (lower only) - mj_fullM(m, M, d->qM); - - // compare iM sub-matrix to full M - for (int island=0; island < nisland; island++) { - int nvi = d->island_nv[island]; - mjtNum* Mi = (mjtNum*)mju_malloc(sizeof(mjtNum) * nvi * nvi); - - int adr = d->island_idofadr[island]; - mju_sparse2dense(Mi, d->iM, nvi, nvi, - d->iM_rownnz + adr, - d->iM_rowadr + adr, - d->iM_colind); - - // compare Mi to M (lower triangle only) - for (int i=0; i < nvi; i++) { - for (int j=0; j <= i; j++) { - int dofi = d->map_idof2dof[adr + j]; - int dofj = d->map_idof2dof[adr + i]; - EXPECT_EQ(Mi[i * nvi + j], M[dofi * nv + dofj]); - } - } - mju_free(Mi); - } - } - - mju_free(M); - mj_deleteData(d); - mj_deleteModel(m); - } -} - TEST_F(IslandTest, IslandEfcElliptic) { const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); char error[1024]; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e25c0986..e9baa2ea 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5752,12 +5752,6 @@ public unsafe struct mjData_ { public int* map_idof2dof; public double* ifrc_smooth; public double* iacc_smooth; - public int* iM_rownnz; - public int* iM_rowadr; - public int* iM_colind; - public double* iM; - public double* iLD; - public double* iLDiagInv; public double* iacc; public int* efc_island; public int* island_ne; @@ -5768,11 +5762,6 @@ public unsafe struct mjData_ { public int* map_iefc2efc; public int* iefc_type; public int* iefc_id; - public int* iefc_J_rownnz; - public int* iefc_J_rowadr; - public int* iefc_J_rowsuper; - public int* iefc_J_colind; - public double* iefc_J; public double* iefc_frictionloss; public double* iefc_D; public double* iefc_R; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 735c4b1e..eae8ae44 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -6981,24 +6981,6 @@ struct MjData { emscripten::val iacc_smooth() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nidof, ptr_->iacc_smooth)); } - emscripten::val iM_rownnz() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nidof, ptr_->iM_rownnz)); - } - emscripten::val iM_rowadr() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nidof, ptr_->iM_rowadr)); - } - emscripten::val iM_colind() const { - return emscripten::val(emscripten::typed_memory_view(model->nC, ptr_->iM_colind)); - } - emscripten::val iM() const { - return emscripten::val(emscripten::typed_memory_view(model->nC, ptr_->iM)); - } - emscripten::val iLD() const { - return emscripten::val(emscripten::typed_memory_view(model->nC, ptr_->iLD)); - } - emscripten::val iLDiagInv() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nidof, ptr_->iLDiagInv)); - } emscripten::val iacc() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nidof, ptr_->iacc)); } @@ -7029,21 +7011,6 @@ struct MjData { emscripten::val iefc_id() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->iefc_id)); } - emscripten::val iefc_J_rownnz() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->iefc_J_rownnz)); - } - emscripten::val iefc_J_rowadr() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->iefc_J_rowadr)); - } - emscripten::val iefc_J_rowsuper() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->iefc_J_rowsuper)); - } - emscripten::val iefc_J_colind() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nJ, ptr_->iefc_J_colind)); - } - emscripten::val iefc_J() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nJ, ptr_->iefc_J)); - } emscripten::val iefc_frictionloss() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->iefc_frictionloss)); } @@ -11694,20 +11661,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("geom_xmat", &MjData::geom_xmat) .property("geom_xpos", &MjData::geom_xpos) .property("history", &MjData::history) - .property("iLD", &MjData::iLD) - .property("iLDiagInv", &MjData::iLDiagInv) - .property("iM", &MjData::iM) - .property("iM_colind", &MjData::iM_colind) - .property("iM_rowadr", &MjData::iM_rowadr) - .property("iM_rownnz", &MjData::iM_rownnz) .property("iacc", &MjData::iacc) .property("iacc_smooth", &MjData::iacc_smooth) .property("iefc_D", &MjData::iefc_D) - .property("iefc_J", &MjData::iefc_J) - .property("iefc_J_colind", &MjData::iefc_J_colind) - .property("iefc_J_rowadr", &MjData::iefc_J_rowadr) - .property("iefc_J_rownnz", &MjData::iefc_J_rownnz) - .property("iefc_J_rowsuper", &MjData::iefc_J_rowsuper) .property("iefc_R", &MjData::iefc_R) .property("iefc_aref", &MjData::iefc_aref) .property("iefc_force", &MjData::iefc_force)