chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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_core_smooth.c
+7 -11
View File
@@ -774,7 +774,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
L[i] += (mju_dist3(wpnt, wpnt+3) + wlen + mju_dist3(wpnt+6, wpnt+9))/divisor;
}
// accumulate moments if consequtive points are in different bodies
// accumulate moments if consecutive points are in different bodies
for (int k=0; k < (wlen < 0 ? 1 : 3); k++) {
if (wbody[k] != wbody[k+1]) {
// get 3D position difference, normalize
@@ -1387,8 +1387,7 @@ void mj_crb(const mjModel* m, mjData* d) {
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd
void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv,
mjtNum* qLDiagSqrtInv) {
void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv) {
int cnt;
int Madr_kk, Madr_ki;
mjtNum tmp;
@@ -1445,9 +1444,6 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
for (int i=0; i < nv; i++) {
mjtNum qLDi = qLD[dof_Madr[i]];
qLDiagInv[i] = 1.0/qLDi;
if (qLDiagSqrtInv) {
qLDiagSqrtInv[i] = 1.0/mju_sqrt(qLDi);
}
}
}
@@ -1456,7 +1452,7 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
// sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv, d->qLDiagSqrtInv);
mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv);
TM_ADD(mjTIMER_POS_INERTIA);
}
@@ -1685,10 +1681,10 @@ void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int
// half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y
void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y,
const mjtNum* sqrtInvD, int n) {
// local copies of key variables
mjtNum* qLD = d->qLD;
mjtNum* qLDiagSqrtInv = d->qLDiagSqrtInv;
int* dof_Madr = m->dof_Madr;
int* dof_parentid = m->dof_parentid;
int nv = m->nv;
@@ -1720,7 +1716,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n)
// x <- sqrt(inv(D)) * x
for (int i=0; i < nv; i++) {
x[i+offset] *= qLDiagSqrtInv[i]; // x(i) /= sqrt(L(i,i))
x[i+offset] *= sqrtInvD[i]; // x(i) /= sqrt(L(i,i))
}
}
}
@@ -1781,7 +1777,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
default:
// in principle we should use the new velocity to compute cdofdot,
// but it makes no difference becase crossMotion(cdof, cdof) = 0,
// but it makes no difference because crossMotion(cdof, cdof) = 0,
// and using the old velocity may be more accurate numerically
mju_crossMotion(cdofdot+6*j, cvel, d->cdof+6*(bda+j));