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

Switch mjData.{qH,qLD} from reduced ("C") to full ("M") inertia matrix structure. No performance impact of extra zeros because of existing "simple dof" skipping mechanism.

Browse Source 
PiperOrigin-RevId: 733523931
Change-Id: Ic8d8a152dda5532331c239cb6b4ce7d8d09b7fff
This commit is contained in:
Yuval Tassa
2025-03-04 17:33:48 -08:00
committed by Copybara-Service
parent 51f6aa8b43
commit 0f563ecf31
20 changed files with 95 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+11 -11
View File
@@ -770,7 +770,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, nC = m->nC;
int nv = m->nv, nM = m->nM;
mj_markStack(d);
mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* qacc = mjSTACKALLOC(d, nv, mjtNum);
@@ -795,22 +795,22 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
else {
if (!skipfactor) {
// qH = M + h*diag(B)
for (int i=0; i < nC; i++) {
d->qH[i] = d->qM[d->mapM2C[i]];
for (int i=0; i < nM; i++) {
d->qH[i] = d->qM[d->mapM2M[i]];
}
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
@@ -939,7 +939,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, nC = m->nC;
int nv = m->nv, nM = m->nM, nD = m->nD;
mj_markStack(d);
mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum);
@@ -987,18 +987,18 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mju_addScl(MhB, d->qM, MhB, -m->opt.timestep, nM);
// copy into qH
for (int i=0; i < nC; i++) {
d->qH[i] = MhB[d->mapM2C[i]];
for (int i=0; i < nM; i++) {
d->qH[i] = MhB[d->mapM2M[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 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");