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

Remove mjData.qLDiagSqrtInv, add corresponding argument to mj_solveM2.

Browse Source 
- `qLDiagSqrtInv` is only required for the dual solvers. It is now computed as-needed rather than unconditionally.
- `mj_solveM2` now requires a new input array `sqrtInvD` which contains the square root of the inverse diagonal D (formerly saved in `qLDiagSqrtInv`).

PiperOrigin-RevId: 710805133
Change-Id: I0622d6a8da3882916824e9c10bad9223c122c321
This commit is contained in:
Yuval Tassa
2024-12-30 15:23:41 -08:00
committed by Copybara-Service
parent ec322641b7
commit 69c9ac074a
19 changed files with 53 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_interact.c
+5 -1
View File
@@ -541,6 +541,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
mjtNum* jac = mjSTACKALLOC(d, 3*nv, mjtNum);
mjtNum* jacM2 = mjSTACKALLOC(d, 3*nv, mjtNum);
mjtNum* sqrtInvD = mjSTACKALLOC(d, nv, mjtNum);
// invalid selected body: return
if (sel <= 0 || sel >= m->nbody) {
@@ -554,8 +555,11 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
mju_addTo3(selpos, d->xpos+3*sel);
// compute average spatial inertia at selection point
for (int i=0; i < nv; i++) {
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[m->dof_Madr[i]]);
}
mj_jac(m, d, jac, NULL, selpos, sel);
mj_solveM2(m, d, jacM2, jac, 3);
mj_solveM2(m, d, jacM2, jac, sqrtInvD, 3);
mjtNum invmass = mju_dot(jacM2+0*nv, jacM2+0*nv, nv) +
mju_dot(jacM2+1*nv, jacM2+1*nv, nv) +
mju_dot(jacM2+2*nv, jacM2+2*nv, nv);