chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
test/benchmark/factorI_benchmark_test.cc
+3 -3
View File
@@ -45,8 +45,8 @@ 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->nM);
mju_gather(M, d->qM, d->mapM2M, m->nM);
mjtNum* M = mj_stackAllocNum(d, m->nC);
mju_gather(M, d->qM, d->mapM2C, m->nC);
// LDlegacy: legacy LD matrix (size nM)
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
@@ -59,7 +59,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->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
}
}
test/benchmark/inertia_benchmark_test.cc
+4 -4
View File
@@ -47,8 +47,8 @@ static void BM_solve(benchmark::State& state, SolveType type) {
mj_markStack(d);
// M: mass matrix in CSR format
mjtNum* M = mj_stackAllocNum(d, m->nM);
mju_gather(M, d->qM, d->mapM2M, m->nM);
mjtNum* M = mj_stackAllocNum(d, m->nC);
mju_gather(M, d->qM, d->mapM2C, m->nC);
// LDlegacy: legacy LD matrix (size nM)
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
@@ -73,9 +73,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->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
mj_solveLD(res, d->qLD, d->qLDiagInv, m->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);
}
}
}
test/benchmark/solveLD_benchmark_test.cc
+3 -2
View File
@@ -53,7 +53,8 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
// scatter into legacy matrix
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
mju_scatter(LDlegacy, d->qLD, d->mapM2M, m->nM);
mju_zero(LDlegacy, m->nM);
mju_scatter(LDlegacy, d->qLD, d->mapM2C, m->nC);
// benchmark
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
@@ -63,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->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
}
}
}