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

Rework sparse addM function to support compressed sparse matrices.

Browse Source 
Note:

- M is still a nv x nv uncompressed sparse matrix.
- NNZ precounting still needs to be implemented using the combineSparseCount helper function.

PiperOrigin-RevId: 554452710
Change-Id: I14adf58fc3acd4ed82d864d7c2e9e96ee6b0377d
This commit is contained in:
Kyle Bayes
2023-08-07 06:06:33 -07:00
committed by Copybara-Service
parent 6d7b49ccf5
commit 2d7d5319f7
4 changed files with 93 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_solver.c
+10 -1
View File
@@ -1364,7 +1364,15 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
// sparse
if (mj_isSparse(m)) {
// create sparse inertia matrix M (uncompressed)
mjtNum* M = mj_stackAlloc(d, nv*nv);
int* M_rownnz = (int*) mj_stackAlloc(d, nv);
int* M_rowadr = (int*) mj_stackAlloc(d, nv);
int* M_colind = (int*) mj_stackAlloc(d, nv*nv);
mj_createMSparse(m, d, M, M_rownnz, M_rowadr, M_colind);
// compute H = J'*D*J
// TODO(b/266802572): remove uncompressed layout
mju_sqrMatTDUncompressedInit(ctx->rowadr, nv);
mju_sqrMatTDSparse(ctx->H, d->efc_J, d->efc_JT, D, nefc, nv,
@@ -1375,7 +1383,8 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
d->efc_JT_colind, d->efc_JT_rowsuper, d);
// compute H = M + J'*D*J
mj_addMSparse(m, d, ctx->H, ctx->rownnz, ctx->rowadr, ctx->colind);
mj_addMSparse(m, d, ctx->H, ctx->rownnz, ctx->rowadr, ctx->colind,
M, M_rownnz, M_rowadr, M_colind);
// factorize H, uncompressed layout
int rank = mju_cholFactorSparse(ctx->H, nv, mjMINVAL,