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

Add reduced sparse dof-dof matrix structures to mjData.

Browse Source 
PiperOrigin-RevId: 670919263
Change-Id: I3592de802ae84648d97230115c2e9c3437424968
This commit is contained in:
Yuval Tassa
2024-09-04 04:22:43 -07:00
committed by Copybara-Service
parent 42eb669e4e
commit 3437361bfb
14 changed files with 265 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+12 -7
View File
@@ -306,14 +306,18 @@ struct mjData_ {
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
// computed by mj_resetData
int* D_rownnz; // non-zeros in each row (nv x 1)
int* D_rowadr; // address of each row in D_colind (nv x 1)
int* D_colind; // column indices of non-zeros (nD x 1)
int* B_rownnz; // body-dof: non-zeros in each row (nbody x 1)
int* B_rowadr; // body-dof: address of each row in B_colind (nbody x 1)
int* B_colind; // body-dof: column indices of non-zeros (nB x 1)
int* C_rownnz; // reduced dof-dof: non-zeros in each row (nv x 1)
int* C_rowadr; // reduced dof-dof: address of each row in C_colind (nv x 1)
int* C_colind; // reduced dof-dof: column indices of non-zeros (nC x 1)
int* mapM2C; // index mapping from M to C (nC x 1)
int* D_rownnz; // dof-dof: non-zeros in each row (nv x 1)
int* D_rowadr; // dof-dof: address of each row in D_colind (nv x 1)
int* D_colind; // dof-dof: column indices of non-zeros (nD x 1)
int* mapM2D; // index mapping from M to D (nD x 1)
int* mapD2M; // index mapping from D to M (nM x 1)
int* B_rownnz; // non-zeros in each row (nbody x 1)
int* B_rowadr; // address of each row in B_colind (nbody x 1)
int* B_colind; // column indices of non-zeros (nB x 1)
// computed by mj_implicit/mj_derivative
mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1)
@@ -945,8 +949,9 @@ struct mjModel_ {
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse dof-dof matrix
int nB; // number of non-zeros in sparse body-dof matrix
int nC; // number of non-zeros in sparse reduced dof-dof matrix
int nD; // number of non-zeros in sparse dof-dof matrix
int ntree; // number of kinematic trees under world body
int ngravcomp; // number of bodies with nonzero gravcomp
int nemax; // number of potential equality-constraint rows
include/mujoco/mjdata.h
+10 -6
View File
@@ -334,14 +334,18 @@ struct mjData_ {
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
// computed by mj_resetData
int* D_rownnz; // non-zeros in each row (nv x 1)
int* D_rowadr; // address of each row in D_colind (nv x 1)
int* D_colind; // column indices of non-zeros (nD x 1)
int* B_rownnz; // body-dof: non-zeros in each row (nbody x 1)
int* B_rowadr; // body-dof: address of each row in B_colind (nbody x 1)
int* B_colind; // body-dof: column indices of non-zeros (nB x 1)
int* C_rownnz; // reduced dof-dof: non-zeros in each row (nv x 1)
int* C_rowadr; // reduced dof-dof: address of each row in C_colind (nv x 1)
int* C_colind; // reduced dof-dof: column indices of non-zeros (nC x 1)
int* mapM2C; // index mapping from M to C (nC x 1)
int* D_rownnz; // dof-dof: non-zeros in each row (nv x 1)
int* D_rowadr; // dof-dof: address of each row in D_colind (nv x 1)
int* D_colind; // dof-dof: column indices of non-zeros (nD x 1)
int* mapM2D; // index mapping from M to D (nD x 1)
int* mapD2M; // index mapping from D to M (nM x 1)
int* B_rownnz; // non-zeros in each row (nbody x 1)
int* B_rowadr; // address of each row in B_colind (nbody x 1)
int* B_colind; // column indices of non-zeros (nB x 1)
// computed by mj_implicit/mj_derivative
mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1)
include/mujoco/mjmodel.h
+2 -1
View File
@@ -659,8 +659,9 @@ struct mjModel_ {
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse dof-dof matrix
int nB; // number of non-zeros in sparse body-dof matrix
int nC; // number of non-zeros in sparse reduced dof-dof matrix
int nD; // number of non-zeros in sparse dof-dof matrix
int ntree; // number of kinematic trees under world body
int ngravcomp; // number of bodies with nonzero gravcomp
int nemax; // number of potential equality-constraint rows
include/mujoco/mjxmacro.h
+9 -4
View File
@@ -133,8 +133,9 @@
XMJV( npaths ) \
X ( nnames_map ) \
X ( nM ) \
X ( nD ) \
X ( nB ) \
X ( nC ) \
X ( nD ) \
X ( nemax ) \
X ( njmax ) \
X ( nconmax ) \
@@ -640,14 +641,18 @@
X ( mjtNum, subtree_angmom, nbody, 3 ) \
X ( mjtNum, qH, nM, 1 ) \
X ( mjtNum, qHDiagInv, nv, 1 ) \
X ( int, B_rownnz, nbody, 1 ) \
X ( int, B_rowadr, nbody, 1 ) \
X ( int, B_colind, nB, 1 ) \
X ( int, C_rownnz, nv, 1 ) \
X ( int, C_rowadr, nv, 1 ) \
X ( int, C_colind, nC, 1 ) \
X ( int, mapM2C, nC, 1 ) \
X ( int, D_rownnz, nv, 1 ) \
X ( int, D_rowadr, nv, 1 ) \
X ( int, D_colind, nD, 1 ) \
X ( int, mapM2D, nD, 1 ) \
X ( int, mapD2M, nM, 1 ) \
X ( int, B_rownnz, nbody, 1 ) \
X ( int, B_rowadr, nbody, 1 ) \
X ( int, B_colind, nB, 1 ) \
X ( mjtNum, qDeriv, nD, 1 ) \
X ( mjtNum, qLU, nD, 1 ) \
X ( mjtNum, actuator_force, nu, 1 ) \
introspect/structs.py
+62 -29
View File
@@ -1158,16 +1158,21 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='number of non-zeros in sparse inertia matrix',
),
StructFieldDecl(
name='nD',
type=ValueType(name='int'),
doc='number of non-zeros in sparse dof-dof matrix',
),
StructFieldDecl(
name='nB',
type=ValueType(name='int'),
doc='number of non-zeros in sparse body-dof matrix',
),
StructFieldDecl(
name='nC',
type=ValueType(name='int'),
doc='number of non-zeros in sparse reduced dof-dof matrix',
),
StructFieldDecl(
name='nD',
type=ValueType(name='int'),
doc='number of non-zeros in sparse dof-dof matrix',
),
StructFieldDecl(
name='ntree',
type=ValueType(name='int'),
@@ -4845,26 +4850,75 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='1/diag(D) of modified M (nv x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='B_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='body-dof: non-zeros in each row (nbody x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='B_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='body-dof: address of each row in B_colind (nbody x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='B_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='body-dof: column indices of non-zeros (nB x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='C_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='reduced dof-dof: non-zeros in each row (nv x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='C_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='reduced dof-dof: address of each row in C_colind (nv x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='C_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='reduced dof-dof: column indices of non-zeros (nC x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='mapM2C',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='index mapping from M to C (nC x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='D_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='non-zeros in each row (nv x 1)', # pylint: disable=line-too-long
doc='dof-dof: non-zeros in each row (nv x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='D_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='address of each row in D_colind (nv x 1)', # pylint: disable=line-too-long
doc='dof-dof: address of each row in D_colind (nv x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='D_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='column indices of non-zeros (nD x 1)', # pylint: disable=line-too-long
doc='dof-dof: column indices of non-zeros (nD x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='mapM2D',
@@ -4880,27 +4934,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='index mapping from D to M (nM x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='B_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='non-zeros in each row (nbody x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='B_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='address of each row in B_colind (nbody x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='B_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='column indices of non-zeros (nB x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='qDeriv',
type=PointerType(
src/engine/engine_io.c
+76 -36
View File
@@ -886,11 +886,14 @@ int mj_sizeModel(const mjModel* m) {
//-------------------------- sparse system matrix construction -------------------------------------
// construct sparse representation of dof-dof matrix
static void makeDSparse(const mjModel* m, mjData* d) {
static void makeDofDofSparse(const mjModel* m, mjData* d,
int* rownnz, int* rowadr, int* colind, int reduced) {
int nv = m->nv;
int* rownnz = d->D_rownnz;
int* rowadr = d->D_rowadr;
int* colind = d->D_colind;
// no dofs, nothing to do
if (!nv) {
return;
}
mj_markStack(d);
int* remaining = mj_stackAllocInt(d, nv);
@@ -902,10 +905,12 @@ static void makeDSparse(const mjModel* m, mjData* d) {
int j = i;
rownnz[i]++;
// process below diagonal
while ((j = m->dof_parentid[j]) >= 0) {
rownnz[i]++;
rownnz[j]++;
// process below diagonal unless reduced and dof is simple
if (!reduced || !m->dof_simplenum[i]) {
while ((j = m->dof_parentid[j]) >= 0) {
rownnz[i]++;
rownnz[j]++;
}
}
}
@@ -922,29 +927,34 @@ static void makeDSparse(const mjModel* m, mjData* d) {
remaining[i]--;
colind[rowadr[i] + remaining[i]] = i;
// process below diagonal
int j = i;
while ((j = m->dof_parentid[j]) >= 0) {
remaining[i]--;
colind[rowadr[i] + remaining[i]] = j;
// process below diagonal unless reduced and dof is simple
if (!reduced || !m->dof_simplenum[i]) {
int j = i;
while ((j = m->dof_parentid[j]) >= 0) {
remaining[i]--;
colind[rowadr[i] + remaining[i]] = j;
remaining[j]--;
colind[rowadr[j] + remaining[j]] = i;
remaining[j]--;
colind[rowadr[j] + remaining[j]] = i;
}
}
}
// sanity check; SHOULD NOT OCCUR
// check for remaining; SHOULD NOT OCCUR
for (int i = 0; i < nv; i++) {
if (remaining[i] != 0) {
mjERROR("unexpected remaining");
}
}
// check total nnz; SHOULD NOT OCCUR
if (rowadr[nv - 1] + rownnz[nv - 1] != (reduced ? m->nC : m->nD)) {
mjERROR("sum of rownnz different from expected");
}
mj_freeStack(d);
}
// construct sparse representation of body-dof matrix
static void makeBSparse(const mjModel* m, mjData* d) {
int nv = m->nv, nbody = m->nbody;
@@ -959,7 +969,7 @@ static void makeBSparse(const mjModel* m, mjData* d) {
rownnz[m->body_parentid[i]] += rownnz[i];
}
// sanity check; SHOULD NOT OCCUR
// check if rownnz[0] != nv; SHOULD NOT OCCUR
if (rownnz[0] != nv) {
mjERROR("rownnz[0] different from nv");
}
@@ -979,7 +989,7 @@ static void makeBSparse(const mjModel* m, mjData* d) {
rowadr[i] = rowadr[i - 1] + rownnz[i - 1];
}
// sanity check; SHOULD NOT OCCUR
// check if total nnz != nB; SHOULD NOT OCCUR
if (m->nB != rowadr[nbody - 1] + rownnz[nbody - 1]) {
mjERROR("sum of rownnz different from nB");
}
@@ -1059,33 +1069,46 @@ static void checkDBSparse(const mjModel* m, mjData* d) {
// integer valued dst[D] = src[M], handle different sparsity representations
static void copyM2DSparse(const mjModel* m, mjData* d, int* dst, const int* src) {
// integer valued dst[D or C] = src[M], handle different sparsity representations
static void copyM2Sparse(const mjModel* m, mjData* d, int* dst, const int* src,
int reduced) {
int nv = m->nv;
const int* rownnz;
const int* rowadr;
if (reduced) {
rownnz = d->C_rownnz;
rowadr = d->C_rowadr;
} else {
rownnz = d->D_rownnz;
rowadr = d->D_rowadr;
}
mj_markStack(d);
// init remaining
int* remaining = mj_stackAllocInt(d, nv);
mju_copyInt(remaining, d->D_rownnz, nv);
mju_copyInt(remaining, rownnz, nv);
// copy data
for (int i = nv - 1; i >= 0; i--) {
// init at diagonal
int adr = m->dof_Madr[i];
remaining[i]--;
dst[d->D_rowadr[i] + remaining[i]] = src[adr];
dst[rowadr[i] + remaining[i]] = src[adr];
adr++;
// process below diagonal
int j = i;
while ((j = m->dof_parentid[j]) >= 0) {
remaining[i]--;
dst[d->D_rowadr[i] + remaining[i]] = src[adr];
// process below diagonal unless reduced and dof is simple
if (!reduced || !m->dof_simplenum[i]) {
int j = i;
while ((j = m->dof_parentid[j]) >= 0) {
remaining[i]--;
dst[rowadr[i] + remaining[i]] = src[adr];
remaining[j]--;
dst[d->D_rowadr[j] + remaining[j]] = src[adr];
remaining[j]--;
dst[rowadr[j] + remaining[j]] = src[adr];
adr++;
adr++;
}
}
}
@@ -1125,16 +1148,16 @@ static void copyD2MSparse(const mjModel* m, const mjData* d, int* dst, const int
// construct index mappings between D <-> M
// construct index mappings between M <-> D and M -> C
static void makeDmap(const mjModel* m, mjData* d) {
int nM = m->nM, nD = m->nD;
int nM = m->nM, nC = m->nC, nD = m->nD;
mj_markStack(d);
// make mapM2D
int* M = mj_stackAllocInt(d, nM);
for (int i=0; i < nM; i++) M[i] = i;
for (int i=0; i < nD; i++) d->mapM2D[i] = -1;
copyM2DSparse(m, d, d->mapM2D, M);
copyM2Sparse(m, d, d->mapM2D, M, /*reduced=*/0);
// check that all indices are filled in
for (int i=0; i < nD; i++) {
@@ -1156,6 +1179,17 @@ static void makeDmap(const mjModel* m, mjData* d) {
}
}
// make mapM2C
for (int i=0; i < nC; i++) d->mapM2C[i] = -1;
copyM2Sparse(m, d, d->mapM2C, M, /*reduced=*/1);
// check that all indices are filled in
for (int i=0; i < nC; i++) {
if (d->mapM2C[i] < 0) {
mjERROR("unassigned index in mapM2C");
}
}
mj_freeStack(d);
}
@@ -1805,9 +1839,15 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
// construct sparse matrix representations
if (m->body_dofadr) {
makeDSparse(m, d);
// make D
makeDofDofSparse(m, d, d->D_rownnz, d->D_rowadr, d->D_colind, /*reduced=*/0);
// make B, check D and B
makeBSparse(m, d);
checkDBSparse(m, d);
// make C
makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, d->C_colind, /*reduced=*/1);
makeDmap(m, d);
}
src/engine/engine_print.c
+49 -21
View File
@@ -979,6 +979,55 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
printArray("QLDIAGSQRTINV", m->nv, 1, d->qLDiagSqrtInv, fp, float_format);
// B_rownnz
fprintf(fp, NAME_FORMAT, "B_rownnz");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rownnz[i]);
}
fprintf(fp, "\n\n");
// B_rowadr
fprintf(fp, NAME_FORMAT, "B_rowadr");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rowadr[i]);
}
fprintf(fp, "\n\n");
// B_colind
fprintf(fp, NAME_FORMAT, "B_colind");
for (int i = 0; i < m->nB; i++) {
fprintf(fp, " %d", d->B_colind[i]);
}
fprintf(fp, "\n\n");
// C_rownnz
fprintf(fp, NAME_FORMAT, "C_rownnz");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->C_rownnz[i]);
}
fprintf(fp, "\n\n");
// C_rowadr
fprintf(fp, NAME_FORMAT, "C_rowadr");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->C_rowadr[i]);
}
fprintf(fp, "\n\n");
// C_colind
fprintf(fp, NAME_FORMAT, "C_colind");
for (int i = 0; i < m->nC; i++) {
fprintf(fp, " %d", d->C_colind[i]);
}
fprintf(fp, "\n\n");
// mapM2C
fprintf(fp, NAME_FORMAT, "mapM2C");
for (int i = 0; i < m->nC; i++) {
fprintf(fp, " %d", d->mapM2C[i]);
}
fprintf(fp, "\n\n");
// D_rownnz
fprintf(fp, NAME_FORMAT, "D_rownnz");
for (int i = 0; i < m->nv; i++) {
@@ -1014,27 +1063,6 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
}
fprintf(fp, "\n\n");
// B_rownnz
fprintf(fp, NAME_FORMAT, "B_rownnz");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rownnz[i]);
}
fprintf(fp, "\n\n");
// B_rowadr
fprintf(fp, NAME_FORMAT, "B_rowadr");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rowadr[i]);
}
fprintf(fp, "\n\n");
// B_colind
fprintf(fp, NAME_FORMAT, "B_colind");
for (int i = 0; i < m->nB; i++) {
fprintf(fp, " %d", d->B_colind[i]);
}
fprintf(fp, "\n\n");
// print qDeriv
mju_sparse2dense(M, d->qDeriv, m->nv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind);
printArray("QDERIV", m->nv, m->nv, M, fp, float_format);
src/engine/engine_solver.c
+7 -7
View File
@@ -1442,12 +1442,12 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
// sparse
if (mj_isSparse(m)) {
// create sparse inertia matrix M
int nnz = m->nD; // use sparse dof-dof matrix
int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count
int* M_colind = mj_stackAllocInt(d, nnz);
mjtNum* M = mj_stackAllocNum(d, nnz);
mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
// fill-in reduced sparse inertia matrix C (no off-diagonals for simple dofs)
int nC = m->nC;
mjtNum* C = mj_stackAllocNum(d, nC);
for (int i=0; i < nC; i++) {
C[i] = d->qM[d->mapM2C[i]];
}
// compute H = J'*D*J
@@ -1462,7 +1462,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
// compute H = M + J'*D*J
mj_addMSparse(m, d, ctx->H, ctx->rownnz, ctx->rowadr, ctx->colind,
M, M_rownnz, NULL, M_colind);
C, d->C_rownnz, d->C_rowadr, d->C_colind);
// factorize H, uncompressed layout
int rank = mju_cholFactorSparse(ctx->H, nv, mjMINVAL,
src/engine/engine_support.c
+9 -79
View File
@@ -1145,17 +1145,17 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
int* rownnz, int* rowadr, int* colind) {
// sparse
if (rownnz && rowadr && colind) {
int nv = m->nv;
int nC = m->nC;
mj_markStack(d);
// create sparse inertia matrix M
int nnz = m->nD; // use sparse dof-dof matrix
int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count
int* M_colind = mj_stackAllocInt(d, nnz);
mjtNum* M = mj_stackAllocNum(d, nnz);
mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M,
M_rownnz, NULL, M_colind);
// create reduced sparse inertia matrix C
mjtNum* C = mj_stackAllocNum(d, nC);
for (int i=0; i < nC; i++) {
C[i] = d->qM[d->mapM2C[i]];
}
mj_addMSparse(m, d, dst, rownnz, rowadr, colind, C,
d->C_rownnz, d->C_rowadr, d->C_colind);
mj_freeStack(d);
}
@@ -1167,81 +1167,11 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
// make inertia matrix M
void mj_makeMSparse(const mjModel* m, mjData* d, mjtNum* M,
int* M_rownnz, int* M_rowadr, int* M_colind) {
int nv = m->nv;
// currently the sparse dof-dof matrix D row addresses are used, since D has
// the same predetermined sparsity structure as M, however with simple bodies
// M has less non-zeros and can be precounted for further memory reduction
if (M_rowadr == NULL) {
M_rowadr = d->D_rowadr;
}
// build M into sparse format, lower triangle
for (int i = 0; i < nv; i++) {
int Madr = m->dof_Madr[i];
// simple, fill diagonal only
if (m->dof_simplenum[i]) {
M_rownnz[i] = 1;
M[M_rowadr[i]] = d->qM[Madr];
M_colind[M_rowadr[i]] = i;
continue;
}
// backward pass over dofs: construct M_row(i) in reverse order
int col = M_rowadr[i]; // current column in row i
for (int j = i; j >= 0; j = m->dof_parentid[j]) {
M[col] = d->qM[Madr++];
M_colind[col++] = j;
}
// track nnz of lower triangle for row i
int nnz = M_rownnz[i] = col - M_rowadr[i];
// reverse order
int end = nnz >> 1;
for (int j = 0; j < end; j++) {
int a1 = M_rowadr[i] + j; // address 1
int a2 = (M_rowadr[i] + nnz - 1) - j; // address 2
// swap M data on row i
mjtNum val = M[a1];
M[a1] = M[a2];
M[a2] = val;
// swap M column indices on row i
int ind = M_colind[a1];
M_colind[a1] = M_colind[a2];
M_colind[a2] = ind;
}
}
// fill upper triangle
for (int i = 1; i < nv; i++) {
int end = M_rowadr[i] + M_rownnz[i] - 1;
for (int j = M_rowadr[i]; j < end; j++) {
int a = M_rowadr[M_colind[j]] + M_rownnz[M_colind[j]]++;
M[a] = M[j];
M_colind[a] = i;
}
}
}
// add inertia matrix to sparse destination matrix
void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst,
int* rownnz, int* rowadr, int* colind, mjtNum* M,
int* M_rownnz, int* M_rowadr, int* M_colind) {
int nv = m->nv;
// currently the sparse dof-dof matrix D row addresses are used, since D has
// the same predetermined sparsity structure as M, however with simple bodies
// M has less non-zeros and can be precounted for further memory reduction
if (M_rowadr == NULL) {
M_rowadr = d->D_rowadr;
}
mj_markStack(d);
int* buf_ind = mj_stackAllocInt(d, nv);
src/engine/engine_support.h
-4
View File
@@ -135,10 +135,6 @@ MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum
MJAPI void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
int* rownnz, int* rowadr, int* colind);
// make inertia matrix M
MJAPI void mj_makeMSparse(const mjModel* m, mjData* d, mjtNum* M,
int* M_rownnz, int* M_rowadr, int* M_colind);
// add inertia matrix to sparse destination matrix
MJAPI void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst,
int* rownnz, int* rowadr, int* colind, mjtNum* M,
src/user/user_model.cc
+15 -1
View File
@@ -2362,6 +2362,20 @@ void mjCModel::CopyTree(mjModel* m) {
}
m->dof_simplenum[i] = count;
}
// compute nC
int nOD = 0; // number of off-diagonal (non-simple) parent dofs
for (int i=0; i<nv; i++) {
// count ancestor (off-diagonal) dofs
if (!m->dof_simplenum[i]) {
int j = i;
while (j >= 0) {
if (j != i) nOD++;
j = m->dof_parentid[j];
}
}
}
m->nC = nC = 2 * nOD + nv;
}
@@ -4100,7 +4114,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
nmat != m->nmat || ntex != m->ntex || npair!=m->npair || nexclude!=m->nexclude ||
neq!=m->neq || ntendon!=m->ntendon || nwrap!=m->nwrap || nsensor!=m->nsensor ||
nnumeric!=m->nnumeric || nnumericdata!=m->nnumericdata || ntext!=m->ntext ||
ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nD!=m->nD ||
ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nD!=m->nD || nC!=m->nC ||
nB!=m->nB || nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax ||
npaths!=m->npaths) {
errInfo = mjCError(0, "incompatible models in CopyBack");
src/user/user_model.h
+2 -1
View File
@@ -114,8 +114,9 @@ class mjCModel_ : public mjsElement {
int nnames; // number of chars in all names
int npaths; // number of chars in all paths
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse dof-dof matrix
int nB; // number of non-zeros in sparse body-dof matrix
int nC; // number of non-zeros in reduced sparse dof-dof matrix
int nD; // number of non-zeros in sparse dof-dof matrix
// statistics, as computed by mj_setConst
double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst
test/pipeline_test.cc
+3 -1
View File
@@ -41,7 +41,9 @@ using PipelineTest = MujocoTest;
// Joint and actuator damping should integrate identically under implicit
TEST_F(PipelineTest, SparseDenseEquivalent) {
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_NE(model, nullptr) << error;
mjData* data = mj_makeData(model);
// set dense jacobian, call mj_forward, save accelerations
unity/Runtime/Bindings/MjBindings.cs
+9 -4
View File
@@ -4921,14 +4921,18 @@ public unsafe struct mjData_ {
public double* subtree_angmom;
public double* qH;
public double* qHDiagInv;
public int* B_rownnz;
public int* B_rowadr;
public int* B_colind;
public int* C_rownnz;
public int* C_rowadr;
public int* C_colind;
public int* mapM2C;
public int* D_rownnz;
public int* D_rowadr;
public int* D_colind;
public int* mapM2D;
public int* mapD2M;
public int* B_rownnz;
public int* B_rowadr;
public int* B_colind;
public double* qDeriv;
public double* qLU;
public double* actuator_force;
@@ -5222,8 +5226,9 @@ public unsafe struct mjModel_ {
public int nnames_map;
public int npaths;
public int nM;
public int nD;
public int nB;
public int nC;
public int nD;
public int ntree;
public int ngravcomp;
public int nemax;