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

Reuse sparse Dof-Dof matrix to make compressed sparse M in mj_addM.

Browse Source 
PiperOrigin-RevId: 556823353
Change-Id: I650ac83767d60c69859aae6e38aa331f43814378
This commit is contained in:
Kyle Bayes
2023-08-14 09:48:35 -07:00
committed by Copybara-Service
parent 7e1c65b10e
commit f34cb22bd3
4 changed files with 115 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_solver.c
+7 -7
View File
@@ -1364,12 +1364,12 @@ 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);
// create sparse inertia matrix M
int nnz = m->nD; // use sparse dof-dof matrix
int* M_rownnz = (int*) mj_stackAlloc(d, nv); // actual nnz count
int* M_colind = (int*) mj_stackAlloc(d, nnz);
mjtNum* M = mj_stackAlloc(d, nnz);
mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
// compute H = J'*D*J
@@ -1384,7 +1384,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
// compute H = M + J'*D*J
mj_addMSparse(m, d, ctx->H, ctx->rownnz, ctx->rowadr, ctx->colind,
M, M_rownnz, M_rowadr, M_colind);
M, M_rownnz, NULL, M_colind);
// factorize H, uncompressed layout
int rank = mju_cholFactorSparse(ctx->H, nv, mjMINVAL,