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

Switch mjData.{qH,qLD} from full ("M") to reduced ("C") inertia matrix structure.

Browse Source 
PiperOrigin-RevId: 758273074
Change-Id: If1a2e663ea70044694af985e0119afd6d58115ac
This commit is contained in:
Yuval Tassa
2025-05-13 10:19:22 -07:00
committed by Copybara-Service
parent 436b5a8e1f
commit 627fffdef9
18 changed files with 99 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+9 -9
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->M_rowadr[i] + d->M_rownnz[i] - 1;
int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
@@ -2075,11 +2075,11 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
continue;
}
// traverse row j of M, marking new unique nonzeros
int nnzM = d->M_rownnz[j];
int adrM = d->M_rowadr[j];
for (int k=0; k < nnzM; k++) {
int c = d->M_colind[adrM + k];
// traverse row j of C, marking new unique nonzeros
int nnzC = d->C_rownnz[j];
int adrC = d->C_rowadr[j];
for (int k=0; k < nnzC; k++) {
int c = d->C_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 adrM = d->M_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrM,
int adrC = d->C_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrC,
nnzB, B_colind + adrB,
d->M_rownnz[j]-1, d->M_colind + adrM, -b);
d->C_rownnz[j]-1, d->C_colind + adrC, -b);
}
// B(r,:) <- sqrt(inv(D)) * B(r,:)
src/engine/engine_core_smooth.c
+6 -7
View File
@@ -1653,9 +1653,8 @@ void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
// gather LD <- M (legacy to CSR) and factorize in-place
mju_gather(d->qLD, d->qM, d->mapM2M, m->nM);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
mju_gather(d->qLD, d->qM, d->mapM2C, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
TM_ADD(mjTIMER_POS_INERTIA);
}
@@ -1897,7 +1896,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->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
@@ -1908,9 +1907,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->M_rownnz;
const int* rowadr = d->M_rowadr;
const int* colind = d->M_colind;
const int* rownnz = d->C_rownnz;
const int* rowadr = d->C_rowadr;
const int* colind = d->C_colind;
const int* diagnum = m->dof_simplenum;
const mjtNum* qLD = d->qLD;
src/engine/engine_forward.c
+9 -9
View File
@@ -836,7 +836,7 @@ static void mj_advance(const mjModel* m, mjData* d,
// Euler integrator, semi-implicit in velocity, possibly skipping factorisation
void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
TM_START;
int nv = m->nv, nM = m->nM;
int nv = m->nv, nC = m->nC;
mj_markStack(d);
mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* qacc = mjSTACKALLOC(d, nv, mjtNum);
@@ -861,20 +861,20 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
else {
if (!skipfactor) {
// qH = M + h*diag(B)
mju_gather(d->qH, d->qM, d->mapM2M, nM);
mju_gather(d->qH, d->qM, d->mapM2C, nC);
for (int i=0; i < nv; i++) {
d->qH[d->M_rowadr[i] + d->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
d->qH[d->C_rowadr[i] + d->C_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
}
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_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->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
// advance state and time
@@ -1003,7 +1003,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
// fully implicit in velocity, possibly skipping factorization
void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
TM_START;
int nv = m->nv, nM = m->nM, nD = m->nD;
int nv = m->nv, nM = m->nM, nD = m->nD, nC = m->nC;
mj_markStack(d);
mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum);
@@ -1047,16 +1047,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->mapM2M, nM);
mju_gather(d->qH, MhB, d->mapM2C, nC);
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
// solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_copy(qacc, qfrc, nv);
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
} else {
mjERROR("integrator must be implicit or implicitfast");
src/engine/engine_island.c
+3 -3
View File
@@ -536,12 +536,12 @@ void mj_island(const mjModel* m, mjData* d) {
}
// local CSR copy of qM
mjtNum* qM = mjSTACKALLOC(d, m->nM, mjtNum);
mju_gather(qM, d->qM, d->mapM2M, m->nM);
mjtNum* qM = mjSTACKALLOC(d, m->nC, mjtNum);
mju_gather(qM, d->qM, d->mapM2C, m->nC);
// 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->M_rownnz, d->M_rowadr, d->M_colind,
d->qLD, d->C_rownnz, d->C_rowadr, d->C_colind,
nidof, nisland,
d->map_idof2dof, d->map_dof2idof,
d->island_idofadr, d->island_idofadr,
src/engine/engine_print.c
+3 -3
View File
@@ -1126,12 +1126,12 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
printInertia("QM", d->qM, m, fp, float_format);
printSparse("QLD", d->qLD, m->nv, d->M_rownnz,
d->M_rowadr, d->M_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, d->C_rownnz,
d->C_rowadr, d->C_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->M_rownnz, d->M_rowadr, d->M_colind, fp, float_format);
printSparse("QH", d->qH, m->nv, d->C_rownnz, d->C_rowadr, d->C_colind, fp, float_format);
printArray("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format);
}
src/engine/engine_solver.c
+3 -3
View File
@@ -881,10 +881,10 @@ static void CGpointers(const mjModel* m, const mjData* d, mjCGContext* ctx, int
ctx->qacc = d->qacc;
// inertia
ctx->M_rownnz = d->M_rownnz;
ctx->M_rowadr = d->M_rowadr;
ctx->M_rownnz = d->C_rownnz;
ctx->M_rowadr = d->C_rowadr;
ctx->M_diagnum = m->dof_simplenum;
ctx->M_colind = d->M_colind;
ctx->M_colind = d->C_colind;
ctx->dof_Madr = m->dof_Madr;
ctx->dof_parentid = m->dof_parentid;
ctx->qM = d->qM;
src/engine/engine_support.c
+3 -3
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->M_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, d->M_rownnz[i] - 1, d->M_colind+adr);
int adr = d->C_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, d->C_rownnz[i] - 1, d->C_colind+adr);
}
}
// res *= sqrt(D)
for (int i=0; i < nv; i++) {
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1;
res[i] *= mju_sqrt(qLD[diag]);
}
}