diff --git a/doc/includes/references.h b/doc/includes/references.h index 402043aa..a2b820e5 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -272,7 +272,7 @@ struct mjData_ { mjtNum* qM; // total inertia (sparse) (nM x 1) // computed by mj_fwdPosition/mj_factorM - mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1) + mjtNum* qLD; // L'*D*L factorization of M (sparse) (nC x 1) mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) // computed by mj_collisionTree @@ -305,7 +305,7 @@ struct mjData_ { mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3) // computed by mj_Euler or mj_implicit - mjtNum* qH; // L'*D*L factorization of modified M (nM x 1) + mjtNum* qH; // L'*D*L factorization of modified M (nC x 1) mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1) // computed by mj_resetData @@ -396,9 +396,9 @@ struct mjData_ { int* iM_rownnz; // inertia: non-zeros in each row (nidof x 1) int* iM_rowadr; // inertia: address of each row in iM_colind (nidof x 1) int* iM_diagnum; // inertia: num of consecutive diagonal elements (nidof x 1) - int* iM_colind; // inertia: column indices of non-zeros (nM x 1) - mjtNum* iM; // total inertia (sparse) (nM x 1) - mjtNum* iLD; // L'*D*L factorization of M (sparse) (nM x 1) + int* iM_colind; // inertia: column indices of non-zeros (nC x 1) + mjtNum* iM; // total inertia (sparse) (nC x 1) + mjtNum* iLD; // L'*D*L factorization of M (sparse) (nC x 1) mjtNum* iLDiagInv; // 1/diag(D) (nidof x 1) mjtNum* iacc; // acceleration (nidof x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 963630cc..9ac187dc 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -300,7 +300,7 @@ struct mjData_ { mjtNum* qM; // total inertia (sparse) (nM x 1) // computed by mj_fwdPosition/mj_factorM - mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1) + mjtNum* qLD; // L'*D*L factorization of M (sparse) (nC x 1) mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) // computed by mj_collisionTree @@ -333,7 +333,7 @@ struct mjData_ { mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3) // computed by mj_Euler or mj_implicit - mjtNum* qH; // L'*D*L factorization of modified M (nM x 1) + mjtNum* qH; // L'*D*L factorization of modified M (nC x 1) mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1) // computed by mj_resetData @@ -424,9 +424,9 @@ struct mjData_ { int* iM_rownnz; // inertia: non-zeros in each row (nidof x 1) int* iM_rowadr; // inertia: address of each row in iM_colind (nidof x 1) int* iM_diagnum; // inertia: num of consecutive diagonal elements (nidof x 1) - int* iM_colind; // inertia: column indices of non-zeros (nM x 1) - mjtNum* iM; // total inertia (sparse) (nM x 1) - mjtNum* iLD; // L'*D*L factorization of M (sparse) (nM x 1) + int* iM_colind; // inertia: column indices of non-zeros (nC x 1) + mjtNum* iM; // total inertia (sparse) (nC x 1) + mjtNum* iLD; // L'*D*L factorization of M (sparse) (nC x 1) mjtNum* iLDiagInv; // 1/diag(D) (nidof x 1) mjtNum* iacc; // acceleration (nidof x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 88e6c0f7..1f96199e 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -653,7 +653,7 @@ X ( mjtNum, actuator_moment, nJmom, 1 ) \ X ( mjtNum, crb, nbody, 10 ) \ X ( mjtNum, qM, nM, 1 ) \ - X ( mjtNum, qLD, nM, 1 ) \ + X ( mjtNum, qLD, nC, 1 ) \ X ( mjtNum, qLDiagInv, nv, 1 ) \ XMJV( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ XMJV( mjtByte, bvh_active, nbvh, 1 ) \ @@ -670,7 +670,7 @@ X ( mjtNum, qfrc_passive, nv, 1 ) \ X ( mjtNum, subtree_linvel, nbody, 3 ) \ X ( mjtNum, subtree_angmom, nbody, 3 ) \ - X ( mjtNum, qH, nM, 1 ) \ + X ( mjtNum, qH, nC, 1 ) \ X ( mjtNum, qHDiagInv, nv, 1 ) \ X ( int, B_rownnz, nbody, 1 ) \ X ( int, B_rowadr, nbody, 1 ) \ @@ -758,9 +758,9 @@ X( int, iM_rownnz, MJ_D(nidof), 1 ) \ X( int, iM_rowadr, MJ_D(nidof), 1 ) \ X( int, iM_diagnum, MJ_D(nidof), 1 ) \ - X( int, iM_colind, MJ_M(nM), 1 ) \ - X( mjtNum, iM, MJ_M(nM), 1 ) \ - X( mjtNum, iLD, MJ_M(nM), 1 ) \ + X( int, iM_colind, MJ_M(nC), 1 ) \ + X( mjtNum, iM, MJ_M(nC), 1 ) \ + X( mjtNum, iLD, MJ_M(nC), 1 ) \ X( mjtNum, iLDiagInv, MJ_D(nidof), 1 ) \ X( mjtNum, iacc, MJ_D(nidof), 1 ) \ X( int, efc_island, MJ_D(nefc), 1 ) \ diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 1d2ad234..8fc28d83 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -509,7 +509,7 @@ def _make_data_jax( 'actuator_moment': (m.nu, m.nv, float_), 'crb': (m.nbody, 10, float_), 'qM': (m.nM, float_) if support.is_sparse(m) else (m.nv, m.nv, float_), - 'qLD': (m.nM, float_) if support.is_sparse(m) else (m.nv, m.nv, float_), + 'qLD': (m.nC, float_) if support.is_sparse(m) else (m.nv, m.nv, float_), 'qLDiagInv': (m.nv, float_) if support.is_sparse(m) else (0, float_), 'ten_velocity': (m.ntendon, float_), 'actuator_velocity': (m.nu, float_), @@ -618,8 +618,8 @@ def _make_data_c( 'flexedge_velocity': (nflexedge, float_), 'crb': (m.nbody, 10, float_), 'qM': (m.nM, float_), - 'qLD': (m.nM, float_), - 'qH': (m.nM, float_), + 'qLD': (m.nC, float_), + 'qH': (m.nC, float_), 'qHDiagInv': (m.nv, float_), 'qLDiagInv': (m.nv, float_), 'ten_velocity': (m.ntendon, float_), diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 4f182a75..9e23fb68 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -91,8 +91,8 @@ class SmoothTest(absltest.TestCase): # factor_m dx = jax.jit(mjx.factor_m)(mx, mjx.put_data(m, d)) qLDLegacy = np.zeros(mx.nM) # pylint:disable=invalid-name - for i in range(m.nM): - qLDLegacy[d.mapM2M[i]] = d.qLD[i] + for i in range(m.nC): + qLDLegacy[d.mapM2C[i]] = d.qLD[i] _assert_eq(qLDLegacy, dx._impl.qLD, 'qLD') _assert_attr_eq(d, dx._impl, 'qLDiagInv') # com_vel diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index b4e48f39..500b2d40 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -5398,7 +5398,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc="L'*D*L factorization of M (sparse)", - array_extent=('nM',), + array_extent=('nC',), ), StructFieldDecl( name='qLDiagInv', @@ -5534,7 +5534,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc="L'*D*L factorization of modified M", - array_extent=('nM',), + array_extent=('nC',), ), StructFieldDecl( name='qHDiagInv', @@ -6030,7 +6030,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='inertia: column indices of non-zeros', - array_extent=('nM',), + array_extent=('nC',), ), StructFieldDecl( name='iM', @@ -6038,7 +6038,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='total inertia (sparse)', - array_extent=('nM',), + array_extent=('nC',), ), StructFieldDecl( name='iLD', @@ -6046,7 +6046,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc="L'*D*L factorization of M (sparse)", - array_extent=('nM',), + array_extent=('nC',), ), StructFieldDecl( name='iLDiagInv', diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index dba314d9..aa7267ed 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2039,7 +2039,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->M_rowadr[i] + d->M_rownnz[i] - 1; + int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1; sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]); } @@ -2075,11 +2075,11 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { continue; } - // 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]; + // 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]; if (marker[c] != r) { marker[c] = r; nnz++; @@ -2159,10 +2159,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { continue; } int j = B_colind[i]; - int adrM = d->M_rowadr[j]; - mju_addToSclSparseInc(B + adrB, d->qLD + adrM, + int adrC = d->C_rowadr[j]; + mju_addToSclSparseInc(B + adrB, d->qLD + adrC, nnzB, B_colind + adrB, - d->M_rownnz[j]-1, d->M_colind + adrM, -b); + d->C_rownnz[j]-1, d->C_colind + adrC, -b); } // B(r,:) <- sqrt(inv(D)) * B(r,:) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 112f62bd..3b5601a2 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1653,9 +1653,8 @@ void mj_factorM(const mjModel* m, mjData* d) { TM_START; // gather LD <- M (legacy to CSR) and factorize in-place - mju_gather(d->qLD, d->qM, d->mapM2M, m->nM); - mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); - + mju_gather(d->qLD, d->qM, d->mapM2C, m->nC); + mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); TM_ADD(mjTIMER_POS_INERTIA); } @@ -1897,7 +1896,7 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { mju_copy(x, y, n*m->nv); } mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } @@ -1908,9 +1907,9 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int nv = m->nv; // local copies of key variables - const int* rownnz = d->M_rownnz; - const int* rowadr = d->M_rowadr; - const int* colind = d->M_colind; + const int* rownnz = d->C_rownnz; + const int* rowadr = d->C_rowadr; + const int* colind = d->C_colind; const int* diagnum = m->dof_simplenum; const mjtNum* qLD = d->qLD; diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b48b5804..0707efee 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -836,7 +836,7 @@ static void mj_advance(const mjModel* m, mjData* d, // Euler integrator, semi-implicit in velocity, possibly skipping factorisation void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { TM_START; - int nv = m->nv, nM = m->nM; + int nv = m->nv, nC = m->nC; mj_markStack(d); mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum); mjtNum* qacc = mjSTACKALLOC(d, nv, mjtNum); @@ -861,20 +861,20 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { else { if (!skipfactor) { // qH = M + h*diag(B) - mju_gather(d->qH, d->qM, d->mapM2M, nM); + mju_gather(d->qH, d->qM, d->mapM2C, nC); for (int i=0; i < nv; i++) { - d->qH[d->M_rowadr[i] + d->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i]; + d->qH[d->C_rowadr[i] + d->C_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i]; } // factorize in-place - mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } // solve mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv); mju_copy(qacc, qfrc, m->nv); mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } // advance state and time @@ -1003,7 +1003,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { // fully implicit in velocity, possibly skipping factorization void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { TM_START; - int nv = m->nv, nM = m->nM, nD = m->nD; + int nv = m->nv, nM = m->nM, nD = m->nD, nC = m->nC; mj_markStack(d); mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum); @@ -1047,16 +1047,16 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mju_addScl(MhB, d->qM, MhB, -m->opt.timestep, nM); // gather qH <- MhB (legacy to CSR) - mju_gather(d->qH, MhB, d->mapM2M, nM); + mju_gather(d->qH, MhB, d->mapM2C, nC); // factorize in-place - mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } // solve for qacc: (qM - dt*qDeriv) * qacc = qfrc mju_copy(qacc, qfrc, nv); mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } else { mjERROR("integrator must be implicit or implicitfast"); diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 108844d8..5d04118e 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -536,12 +536,12 @@ void mj_island(const mjModel* m, mjData* d) { } // local CSR copy of qM - mjtNum* qM = mjSTACKALLOC(d, m->nM, mjtNum); - mju_gather(qM, d->qM, d->mapM2M, m->nM); + mjtNum* qM = mjSTACKALLOC(d, m->nC, mjtNum); + mju_gather(qM, d->qM, d->mapM2C, m->nC); // inertia: block-diagonalize both iLD <- qLD and iM <- qM mju_blockDiagSparse(d->iLD, d->iM_rownnz, d->iM_rowadr, d->iM_colind, - d->qLD, d->M_rownnz, d->M_rowadr, d->M_colind, + d->qLD, d->C_rownnz, d->C_rowadr, d->C_colind, nidof, nisland, d->map_idof2dof, d->map_dof2idof, d->island_idofadr, d->island_idofadr, diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 61a9ef02..bb9f4a9a 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1126,12 +1126,12 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printInertia("QM", d->qM, m, fp, float_format); - printSparse("QLD", d->qLD, m->nv, d->M_rownnz, - d->M_rowadr, d->M_colind, fp, float_format); + printSparse("QLD", d->qLD, m->nv, d->C_rownnz, + d->C_rowadr, d->C_colind, fp, float_format); printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format); if (!mju_isZero(d->qHDiagInv, m->nv)) { - printSparse("QH", d->qH, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind, fp, float_format); + printSparse("QH", d->qH, m->nv, d->C_rownnz, d->C_rowadr, d->C_colind, fp, float_format); printArray("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format); } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 354386bb..c2f15709 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -881,10 +881,10 @@ static void CGpointers(const mjModel* m, const mjData* d, mjCGContext* ctx, int ctx->qacc = d->qacc; // inertia - ctx->M_rownnz = d->M_rownnz; - ctx->M_rowadr = d->M_rowadr; + ctx->M_rownnz = d->C_rownnz; + ctx->M_rowadr = d->C_rowadr; ctx->M_diagnum = m->dof_simplenum; - ctx->M_colind = d->M_colind; + ctx->M_colind = d->C_colind; ctx->dof_Madr = m->dof_Madr; ctx->dof_parentid = m->dof_parentid; ctx->qM = d->qM; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 5eb0c9fd..362292da 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1047,14 +1047,14 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) // non-simple: add off-diagonals if (!m->dof_simplenum[i]) { - int adr = d->M_rowadr[i]; - res[i] += mju_dotSparse(qLD+adr, vec, d->M_rownnz[i] - 1, d->M_colind+adr); + int adr = d->C_rowadr[i]; + res[i] += mju_dotSparse(qLD+adr, vec, d->C_rownnz[i] - 1, d->C_colind+adr); } } // res *= sqrt(D) for (int i=0; i < nv; i++) { - int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1; + int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1; res[i] *= mju_sqrt(qLD[diag]); } } diff --git a/test/benchmark/factorI_benchmark_test.cc b/test/benchmark/factorI_benchmark_test.cc index f2feea1e..998af271 100644 --- a/test/benchmark/factorI_benchmark_test.cc +++ b/test/benchmark/factorI_benchmark_test.cc @@ -45,8 +45,8 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { mj_markStack(d); // M: mass matrix in CSR format - mjtNum* M = mj_stackAllocNum(d, m->nM); - mju_gather(M, d->qM, d->mapM2M, m->nM); + mjtNum* M = mj_stackAllocNum(d, m->nC); + mju_gather(M, d->qM, d->mapM2C, m->nC); // LDlegacy: legacy LD matrix (size nM) mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM); @@ -59,7 +59,7 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { } else { mju_copy(d->qLD, M, m->nC); mj_factorI(d->qLD, d->qLDiagInv, m->nv, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } } diff --git a/test/benchmark/inertia_benchmark_test.cc b/test/benchmark/inertia_benchmark_test.cc index 3bdeddfd..c8460cda 100644 --- a/test/benchmark/inertia_benchmark_test.cc +++ b/test/benchmark/inertia_benchmark_test.cc @@ -47,8 +47,8 @@ static void BM_solve(benchmark::State& state, SolveType type) { mj_markStack(d); // M: mass matrix in CSR format - mjtNum* M = mj_stackAllocNum(d, m->nM); - mju_gather(M, d->qM, d->mapM2M, m->nM); + mjtNum* M = mj_stackAllocNum(d, m->nC); + mju_gather(M, d->qM, d->mapM2C, m->nC); // LDlegacy: legacy LD matrix (size nM) mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM); @@ -73,9 +73,9 @@ static void BM_solve(benchmark::State& state, SolveType type) { case SolveType::kCsr: mju_copy(d->qLD, M, m->nC); mj_factorI(d->qLD, d->qLDiagInv, m->nv, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } } diff --git a/test/benchmark/solveLD_benchmark_test.cc b/test/benchmark/solveLD_benchmark_test.cc index 6204425f..1ff28a4b 100644 --- a/test/benchmark/solveLD_benchmark_test.cc +++ b/test/benchmark/solveLD_benchmark_test.cc @@ -53,7 +53,8 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { // scatter into legacy matrix mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM); - mju_scatter(LDlegacy, d->qLD, d->mapM2M, m->nM); + mju_zero(LDlegacy, m->nM); + mju_scatter(LDlegacy, d->qLD, d->mapM2C, m->nC); // benchmark while (state.KeepRunningBatch(kNumBenchmarkSteps)) { @@ -63,7 +64,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv); } else { mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 7e4c113a..6adb8764 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -649,7 +649,7 @@ TEST_F(CoreSmoothTest, FactorI) { int nv = model->nv; vector Ldense(nv*nv, 0); mju_sparse2dense(Ldense.data(), data->qLD, nv, nv, - data->M_rownnz, data->M_rowadr, data->M_colind); + data->C_rownnz, data->C_rowadr, data->C_colind); for (int i=0; i < nv; i++) { // set diagonal to 1 Ldense[i*nv+i] = 1; @@ -658,7 +658,7 @@ TEST_F(CoreSmoothTest, FactorI) { // dense D matrix vector Ddense(nv*nv); mju_sparse2dense(Ddense.data(), data->qLD, nv, nv, - data->M_rownnz, data->M_rowadr, data->M_colind); + data->C_rownnz, data->C_rowadr, data->C_colind); for (int i=0; i < nv; i++) { for (int j=0; j < nv; j++) { // zero everything except the diagonal @@ -694,15 +694,16 @@ TEST_F(CoreSmoothTest, SolveLDs) { int nv = m->nv; int nM = m->nM; + int nC = m->nC; - // scatter M into LD: Legacy format - vector LDlegacy(nM); - mju_scatter(LDlegacy.data(), d->qLD, d->mapM2M, nM); + // copy M into LD: Legacy format + vector LDlegacy(nM, 0); + mju_scatter(LDlegacy.data(), d->qLD, d->mapM2C, nC); // compare LD and LDs densified matrices vector LDdense(nv*nv); mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, - d->M_rownnz, d->M_rowadr, d->M_colind); + d->C_rownnz, d->C_rowadr, d->C_colind); vector LDdense2(nv*nv); mj_fullM(m, LDdense2.data(), LDlegacy.data()); @@ -721,7 +722,7 @@ TEST_F(CoreSmoothTest, SolveLDs) { mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv); mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect vectors to match up to floating point precision for (int i=0; i < nv; i++) { @@ -742,11 +743,10 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { mj_forward(m, d); int nv = m->nv; - int nM = m->nM; - // scatter LD into LDlegacy: Legacy format - vector LDlegacy(nM); - mju_scatter(LDlegacy.data(), d->qLD, d->mapM2M, nM); + // copy LD into LDlegacy: Legacy format + vector LDlegacy(m->nM, 0); + mju_scatter(LDlegacy.data(), d->qLD, d->mapM2C, m->nC); // compare n LD and LDs vector solve int n = 3; @@ -757,7 +757,7 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv); mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect vectors to match up to floating point precision for (int i=0; i < nv*n; i++) { @@ -781,7 +781,7 @@ TEST_F(CoreSmoothTest, SolveM2) { int nv = m->nv; vector sqrtInvD(nv); for (int i=0; i < nv; i++) { - int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1; + int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1; sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]); } @@ -795,7 +795,7 @@ TEST_F(CoreSmoothTest, SolveM2) { mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n); mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v) for (int i=0; i < n; i++) { @@ -816,27 +816,25 @@ TEST_F(CoreSmoothTest, FactorIs) { mjData* d = mj_makeData(m); mj_forward(m, d); - int nM = m->nM, nv = m->nv; + int nC = m->nC, nM = m->nM, nv = m->nv; // copy qM into into qLDlegacy and factorize vector qLDlegacy(nM); mj_factorI_legacy(m, d, d->qM, qLDlegacy.data(), d->qLDiagInv); // copy qLDlegacy into qLDexpected: CSR format - vector qLDexpected(nM); - for (int i=0; i < nM; i++) { - qLDexpected[i] = qLDlegacy[d->mapM2M[i]]; - } + vector qLDexpected(nC); + mju_gather(qLDexpected.data(), qLDlegacy.data(), d->mapM2C, nC); - // gather qM into qLD: CSR format - vector qLD(nM); - mju_gather(qLD.data(), d->qM, d->mapM2M, nM); + // copy qM into qLD: CSR format + vector qLD(nC); + mju_gather(qLD.data(), d->qM, d->mapM2C, nC); vector qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv); vector qLDiagInv(nv, 0); mj_factorI(qLD.data(), qLDiagInv.data(), nv, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect outputs to match to floating point precision EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected)); @@ -846,12 +844,12 @@ TEST_F(CoreSmoothTest, FactorIs) { vector LDdense(nv*nv); mju_sparse2dense(LDdense.data(), qLDexpected.data(), nv, nv, - d->M_rownnz, d->M_rowadr, d->M_colind); - PrintMatrix(LDdense.data(), nv, nv, 2, "qLDexpected"); + d->C_rownnz, d->C_rowadr, d->C_colind); + PrintMatrix(LDdense.data(), nv, nv, 2); - mju_sparse2dense(LDdense.data(), qLD.data(), nv, nv, - d->M_rownnz, d->M_rowadr, d->M_colind); - PrintMatrix(LDdense.data(), nv, nv, 2, "qLD"); + mju_sparse2dense(LDdense.data(), qLDs.data(), nv, nv, + d->C_rownnz, d->C_rowadr, d->C_colind); + PrintMatrix(LDdense.data(), nv, nv, 2); */ mj_deleteData(d); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 027578e7..12b9ca88 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -436,7 +436,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { Ac[nv*nv + i*nv + i] = -m->dof_damping[i]; } mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // A = [dt*Ac; Ac] mju_transpose(A, Ac, 2*nv, nv); @@ -464,7 +464,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz, d->moment_rowadr, d->moment_colind); mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, - d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); mju_transpose(BcT, Bc, nu, nv); mju_scl(B, BcT, dt*dt, nu*nv); mju_scl(B+nu*nv, BcT, dt, nu*nv);