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

Move island-specific sparse matrices from arena to stack.

Browse Source 
PiperOrigin-RevId: 923850345
Change-Id: I9683d7554b15b7cd8c45a8dce7814640aa266452
This commit is contained in:
Yuval Tassa
2026-05-30 03:07:31 -07:00
committed by Copybara-Service
parent 5d782a2bb8
commit 96bf8aea81
11 changed files with 122 additions and 402 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+5 -1
View File
@@ -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
^^^^^^^^^
doc/includes/references.h
-11
View File
@@ -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)
include/mujoco/mjdata.h
-11
View File
@@ -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)
include/mujoco/mjxmacro.h
-11
View File
@@ -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 ) \
python/mujoco/introspect/structs.py
-88
View File
@@ -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(
src/engine/engine_island.c
-35
View File
@@ -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);
src/engine/engine_print.c
+25 -22
View File
@@ -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)) {
src/engine/engine_solver.c
+92 -35
View File
@@ -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;
test/engine/engine_island_test.cc
-133
View File
@@ -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];
unity/Runtime/Bindings/MjBindings.cs
-11
View File
@@ -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;
wasm/codegen/generated/bindings.cc
-44
View File
@@ -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)