chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
test/benchmark/factorI_benchmark_test.cc
+4 -4
View File
@@ -44,9 +44,9 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
mj_markStack(d);
// M: mass matrix in CSR format
mjtNum* M = mj_stackAllocNum(d, m->nC);
for (int i=0; i < m->nC; i++) {
M[i] = d->qM[d->mapM2C[i]];
mjtNum* M = mj_stackAllocNum(d, m->nM);
for (int i=0; i < m->nM; i++) {
M[i] = d->qM[d->mapM2M[i]];
}
// LDlegacy: legacy LD matrix (size nM)
@@ -60,7 +60,7 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
} else {
mju_copy(d->qLD, M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
}
}
}
test/benchmark/inertia_benchmark_test.cc
+5 -5
View File
@@ -46,9 +46,9 @@ static void BM_solve(benchmark::State& state, SolveType type) {
mj_markStack(d);
// M: mass matrix in CSR format
mjtNum* M = mj_stackAllocNum(d, m->nC);
for (int i=0; i < m->nC; i++) {
M[i] = d->qM[d->mapM2C[i]];
mjtNum* M = mj_stackAllocNum(d, m->nM);
for (int i=0; i < m->nM; i++) {
M[i] = d->qM[d->mapM2M[i]];
}
// LDlegacy: legacy LD matrix (size nM)
@@ -74,9 +74,9 @@ static void BM_solve(benchmark::State& state, SolveType type) {
case SolveType::kCsr:
mju_copy(d->qLD, M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
mj_solveLD(res, d->qLD, d->qLDiagInv, m->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);
}
}
}
test/benchmark/solveLD_benchmark_test.cc
+3 -4
View File
@@ -52,9 +52,8 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
// make legacy matrix
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
mju_zero(LDlegacy, m->nM);
for (int i=0; i < m->nC; i++) {
LDlegacy[d->mapM2C[i]] = d->qLD[i];
for (int i=0; i < m->nM; i++) {
LDlegacy[d->mapM2M[i]] = d->qLD[i];
}
// benchmark
@@ -65,7 +64,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv);
} else {
mj_solveLD(res, d->qLD, d->qLDiagInv, m->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);
}
}
}