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

Roll back recent change to mjData.qLD until some issues are resolved.

Browse Source 
PiperOrigin-RevId: 724389161
Change-Id: I1b30ab0950ab5c5e611b1c6fc3bf3f42f964939c
This commit is contained in:
Yuval Tassa
2025-02-07 10:20:41 -08:00
committed by Copybara-Service
parent 06758e35ab
commit 5ee0654830
22 changed files with 153 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+9 -3
View File
@@ -2131,8 +2131,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
// inverse square root of D from inertia LDL decomposition
mjtNum* sqrtInvD = mjSTACKALLOC(d, nv, mjtNum);
for (int i=0; i < nv; i++) {
int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[m->dof_Madr[i]]);
}
// sparse
@@ -2239,6 +2238,13 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
// === in-place sparse back-substitution: B <- B * M^-1/2
// make qLD
int nC = m->nC;
mjtNum* qLD = mjSTACKALLOC(d, nC, mjtNum);
for (int i=0; i < nC; i++) {
qLD[i] = d->qLD[d->mapM2C[i]];
}
// sparse backsubM2 (half of LD back-substitution)
for (int r=0; r < nefc; r++) {
int nnzB = B_rownnz[r];
@@ -2252,7 +2258,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
}
int j = B_colind[i];
int adrC = d->C_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrC,
mju_addToSclSparseInc(B + adrB, qLD + adrC,
nnzB, B_colind + adrB,
d->C_rownnz[j]-1, d->C_colind + adrC, -b);
}