chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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
src/engine/engine_core_constraint.c
+9 -9
View File
@@ -2131,7 +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;
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
@@ -2167,11 +2167,11 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
continue;
}
// traverse row j of C, marking new unique nonzeros
int nnzC = d->C_rownnz[j];
int adrC = d->C_rowadr[j];
for (int k=0; k < nnzC; k++) {
int c = d->C_colind[adrC + k];
// traverse row j of M, marking new unique nonzeros
int nnzM = d->M_rownnz[j];
int adrM = d->M_rowadr[j];
for (int k=0; k < nnzM; k++) {
int c = d->M_colind[adrM + k];
if (marker[c] != r) {
marker[c] = r;
nnz++;
@@ -2251,10 +2251,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
continue;
}
int j = B_colind[i];
int adrC = d->C_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrC,
int adrM = d->M_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrM,
nnzB, B_colind + adrB,
d->C_rownnz[j]-1, d->C_colind + adrC, -b);
d->M_rownnz[j]-1, d->M_colind + adrM, -b);
}
// B(r,:) <- sqrt(inv(D)) * B(r,:)