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

Rename C sparse structure to M in mjdata, improve docstrings.

Browse Source 
PiperOrigin-RevId: 758636638
Change-Id: If78acc423601d2911f514929b27f7b6d0af9ef58
This commit is contained in:
Yuval Tassa
2025-05-14 05:25:11 -07:00
committed by Copybara-Service
parent 4ba04586ae
commit dd28b887d4
21 changed files with 128 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+6 -6
View File
@@ -2039,7 +2039,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
// inverse square root of D from inertia LDL decomposition
mjtNum* sqrtInvD = mjSTACKALLOC(d, nv, mjtNum);
for (int i=0; i < nv; i++) {
int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1;
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
@@ -2076,10 +2076,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
}
// traverse row j of C, marking new unique nonzeros
int nnzC = d->C_rownnz[j];
int adrC = d->C_rowadr[j];
int nnzC = d->M_rownnz[j];
int adrC = d->M_rowadr[j];
for (int k=0; k < nnzC; k++) {
int c = d->C_colind[adrC + k];
int c = d->M_colind[adrC + k];
if (marker[c] != r) {
marker[c] = r;
nnz++;
@@ -2159,10 +2159,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
continue;
}
int j = B_colind[i];
int adrC = d->C_rowadr[j];
int adrC = d->M_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrC,
nnzB, B_colind + adrB,
d->C_rownnz[j]-1, d->C_colind + adrC, -b);
d->M_rownnz[j]-1, d->M_colind + adrC, -b);
}
// B(r,:) <- sqrt(inv(D)) * B(r,:)
src/engine/engine_core_smooth.c
+6 -6
View File
@@ -1579,7 +1579,7 @@ void mj_makeM(const mjModel* m, mjData* d) {
TM_START;
mj_crb(m, d);
mj_tendonArmature(m, d);
mju_gather(d->M, d->qM, d->mapM2C, m->nC);
mju_gather(d->M, d->qM, d->mapM2M, m->nC);
TM_END(mjTIMER_POS_INERTIA);
}
@@ -1653,7 +1653,7 @@ void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD
void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
mju_copy(d->qLD, d->M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
TM_ADD(mjTIMER_POS_INERTIA);
}
@@ -1895,7 +1895,7 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
mju_copy(x, y, n*m->nv);
}
mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
}
@@ -1906,9 +1906,9 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y,
int nv = m->nv;
// local copies of key variables
const int* rownnz = d->C_rownnz;
const int* rowadr = d->C_rowadr;
const int* colind = d->C_colind;
const int* rownnz = d->M_rownnz;
const int* rowadr = d->M_rowadr;
const int* colind = d->M_colind;
const int* diagnum = m->dof_simplenum;
const mjtNum* qLD = d->qLD;
src/engine/engine_forward.c
+6 -6
View File
@@ -869,18 +869,18 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// qH = M + h*diag(B)
mju_copy(d->qH, d->M, nC);
for (int i=0; i < nv; i++) {
d->qH[d->C_rowadr[i] + d->C_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
d->qH[d->M_rowadr[i] + d->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
}
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
}
// solve
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
mju_copy(qacc, qfrc, m->nv);
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
}
// advance state and time
@@ -1053,16 +1053,16 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mju_addScl(MhB, d->qM, MhB, -m->opt.timestep, nM);
// gather qH <- MhB (legacy to CSR)
mju_gather(d->qH, MhB, d->mapM2C, nC);
mju_gather(d->qH, MhB, d->mapM2M, nC);
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
}
// solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_copy(qacc, qfrc, nv);
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
} else {
mjERROR("integrator must be implicit or implicitfast");
src/engine/engine_io.c
+6 -6
View File
@@ -1139,8 +1139,8 @@ static void copyM2Sparse(const mjModel* m, mjData* d, int* dst, const int* src,
const int* rownnz;
const int* rowadr;
if (reduced && !upper) {
rownnz = d->C_rownnz;
rowadr = d->C_rowadr;
rownnz = d->M_rownnz;
rowadr = d->M_rowadr;
} else if (!reduced && upper) {
rownnz = d->D_rownnz;
rowadr = d->D_rowadr;
@@ -1248,12 +1248,12 @@ static void makeDofDofmaps(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, /*upper=*/0);
for (int i=0; i < nC; i++) d->mapM2M[i] = -1;
copyM2Sparse(m, d, d->mapM2M, M, /*reduced=*/1, /*upper=*/0);
// check that all indices are filled in
for (int i=0; i < nC; i++) {
if (d->mapM2C[i] < 0) {
if (d->mapM2M[i] < 0) {
mjERROR("unassigned index in mapM2C");
}
}
@@ -1976,7 +1976,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
checkDBSparse(m, d);
// make C
makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, NULL, d->C_colind, /*reduced=*/1, /*upper=*/0);
makeDofDofSparse(m, d, d->M_rownnz, d->M_rowadr, NULL, d->M_colind, /*reduced=*/1, /*upper=*/0);
// make index mappings: mapM2D, mapD2M, mapM2C, mapM2M
makeDofDofmaps(m, d);
src/engine/engine_island.c
+1 -1
View File
@@ -537,7 +537,7 @@ void mj_island(const mjModel* m, mjData* d) {
// inertia: block-diagonalize both iLD <- qLD and iM <- qM
mju_blockDiagSparse(d->iLD, d->iM_rownnz, d->iM_rowadr, d->iM_colind,
d->qLD, d->C_rownnz, d->C_rowadr, d->C_colind,
d->qLD, d->M_rownnz, d->M_rowadr, d->M_colind,
nidof, nisland,
d->map_idof2dof, d->map_dof2idof,
d->island_idofadr, d->island_idofadr,
src/engine/engine_print.c
+17 -17
View File
@@ -1124,14 +1124,14 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
d->moment_rowadr, d->moment_colind, fp, float_format);
printArray("CRB", m->nbody, 10, d->crb, fp, float_format);
printInertia("QM", d->qM, m, fp, float_format);
printSparse("M", d->M, m->nv, d->C_rownnz,
d->C_rowadr, d->C_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, d->C_rownnz,
d->C_rowadr, d->C_colind, fp, float_format);
printSparse("M", d->M, m->nv, d->M_rownnz,
d->M_rowadr, d->M_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, d->M_rownnz,
d->M_rowadr, d->M_colind, fp, float_format);
printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
if (!mju_isZero(d->qHDiagInv, m->nv)) {
printSparse("QH", d->qH, m->nv, d->C_rownnz, d->C_rowadr, d->C_colind, fp, float_format);
printSparse("QH", d->qH, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind, fp, float_format);
printArray("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format);
}
@@ -1160,34 +1160,34 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
}
fprintf(fp, "\n\n");
// C sparse structure
mj_printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, NULL, d->C_rownnz,
NULL, d->C_colind, fp);
// M sparse structure
mj_printSparsity("M: reduced inertia matrix", m->nv, m->nv, d->M_rowadr, NULL, d->M_rownnz,
NULL, d->M_colind, fp);
fprintf(fp, NAME_FORMAT, "C_rownnz");
fprintf(fp, NAME_FORMAT, "M_rownnz");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->C_rownnz[i]);
fprintf(fp, " %d", d->M_rownnz[i]);
}
fprintf(fp, "\n\n");
// C_rowadr
fprintf(fp, NAME_FORMAT, "C_rowadr");
fprintf(fp, NAME_FORMAT, "M_rowadr");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->C_rowadr[i]);
fprintf(fp, " %d", d->M_rowadr[i]);
}
fprintf(fp, "\n\n");
// C_colind
fprintf(fp, NAME_FORMAT, "C_colind");
fprintf(fp, NAME_FORMAT, "M_colind");
for (int i = 0; i < m->nC; i++) {
fprintf(fp, " %d", d->C_colind[i]);
fprintf(fp, " %d", d->M_colind[i]);
}
fprintf(fp, "\n\n");
// mapM2C
fprintf(fp, NAME_FORMAT, "mapM2C");
// mapM2M
fprintf(fp, NAME_FORMAT, "mapM2M");
for (int i = 0; i < m->nC; i++) {
fprintf(fp, " %d", d->mapM2C[i]);
fprintf(fp, " %d", d->mapM2M[i]);
}
fprintf(fp, "\n\n");
src/engine/engine_solver.c
+3 -3
View File
@@ -886,10 +886,10 @@ static void CGpointers(const mjModel* m, const mjData* d, mjCGContext* ctx, int
ctx->qacc = d->qacc;
// inertia
ctx->M_rownnz = d->C_rownnz;
ctx->M_rowadr = d->C_rowadr;
ctx->M_rownnz = d->M_rownnz;
ctx->M_rowadr = d->M_rowadr;
ctx->M_diagnum = m->dof_simplenum;
ctx->M_colind = d->C_colind;
ctx->M_colind = d->M_colind;
ctx->M = d->M;
ctx->qLD = d->qLD;
ctx->qLDiagInv = d->qLDiagInv;
src/engine/engine_support.c
+5 -5
View File
@@ -1047,14 +1047,14 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
// non-simple: add off-diagonals
if (!m->dof_simplenum[i]) {
int adr = d->C_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, d->C_rownnz[i] - 1, d->C_colind+adr);
int adr = d->M_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, d->M_rownnz[i] - 1, d->M_colind+adr);
}
}
// res *= sqrt(D)
for (int i=0; i < nv; i++) {
int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1;
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
res[i] *= mju_sqrt(qLD[diag]);
}
}
@@ -1072,10 +1072,10 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
// gather C <- qM (legacy to CSR)
mjtNum* C = mjSTACKALLOC(d, nC, mjtNum);
mju_gather(C, d->qM, d->mapM2C, nC);
mju_gather(C, d->qM, d->mapM2M, nC);
// add to dst
mj_addMSparse(m, d, dst, rownnz, rowadr, colind, C, d->C_rownnz, d->C_rowadr, d->C_colind);
mj_addMSparse(m, d, dst, rownnz, rowadr, colind, C, d->M_rownnz, d->M_rowadr, d->M_colind);
mj_freeStack(d);
}