diff --git a/doc/includes/references.h b/doc/includes/references.h index c3226bd3..d3b5a2f6 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -309,21 +309,6 @@ struct mjData_ { 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 - int* B_rownnz; // body-dof: non-zeros in each row (nbody x 1) - int* B_rowadr; // body-dof: address of each row in B_colind (nbody x 1) - int* B_colind; // body-dof: column indices of non-zeros (nB x 1) - int* M_rownnz; // reduced inertia: non-zeros in each row (nv x 1) - int* M_rowadr; // reduced inertia: address of each row in M_colind (nv x 1) - int* M_colind; // reduced inertia: column indices of non-zeros (nC x 1) - int* mapM2M; // index mapping from qM to M (nC x 1) - int* D_rownnz; // full inertia: non-zeros in each row (nv x 1) - int* D_rowadr; // full inertia: address of each row in D_colind (nv x 1) - int* D_diag; // full inertia: index of diagonal element (nv x 1) - int* D_colind; // full inertia: column indices of non-zeros (nD x 1) - int* mapM2D; // index mapping from qM to D (nD x 1) - int* mapD2M; // index mapping from D to qM (nM x 1) - // computed by mj_implicit/mj_derivative mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index fd1acfe1..6e12cb98 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -337,21 +337,6 @@ struct mjData_ { 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 - int* B_rownnz; // body-dof: non-zeros in each row (nbody x 1) - int* B_rowadr; // body-dof: address of each row in B_colind (nbody x 1) - int* B_colind; // body-dof: column indices of non-zeros (nB x 1) - int* M_rownnz; // reduced inertia: non-zeros in each row (nv x 1) - int* M_rowadr; // reduced inertia: address of each row in M_colind (nv x 1) - int* M_colind; // reduced inertia: column indices of non-zeros (nC x 1) - int* mapM2M; // index mapping from qM to M (nC x 1) - int* D_rownnz; // full inertia: non-zeros in each row (nv x 1) - int* D_rowadr; // full inertia: address of each row in D_colind (nv x 1) - int* D_diag; // full inertia: index of diagonal element (nv x 1) - int* D_colind; // full inertia: column indices of non-zeros (nD x 1) - int* mapM2D; // index mapping from qM to D (nD x 1) - int* mapD2M; // index mapping from D to qM (nM x 1) - // computed by mj_implicit/mj_derivative mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index be28f9a5..08aca3c1 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -700,19 +700,6 @@ X ( mjtNum, subtree_angmom, nbody, 3 ) \ XNV ( mjtNum, qH, nC, 1 ) \ X ( mjtNum, qHDiagInv, nv, 1 ) \ - XNV ( int, B_rownnz, nbody, 1 ) \ - XNV ( int, B_rowadr, nbody, 1 ) \ - XNV ( int, B_colind, nB, 1 ) \ - XNV ( int, M_rownnz, nv, 1 ) \ - XNV ( int, M_rowadr, nv, 1 ) \ - XNV ( int, M_colind, nC, 1 ) \ - XNV ( int, mapM2M, nC, 1 ) \ - XNV ( int, D_rownnz, nv, 1 ) \ - XNV ( int, D_rowadr, nv, 1 ) \ - XNV ( int, D_diag, nv, 1 ) \ - XNV ( int, D_colind, nD, 1 ) \ - XNV ( int, mapM2D, nD, 1 ) \ - XNV ( int, mapD2M, nM, 1 ) \ XNV ( mjtNum, qDeriv, nD, 1 ) \ XNV ( mjtNum, qLU, nD, 1 ) \ X ( mjtNum, actuator_force, nu, 1 ) \ diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index d42333d0..165b5066 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -764,19 +764,6 @@ def _make_data_c( 'ten_velocity': (m.ntendon, float_), 'actuator_velocity': (m.nu, float_), 'plugin_data': (get(m, 'nplugin'), np.uint64), - 'B_rownnz': (m.nbody, np.int32), - 'B_rowadr': (m.nbody, np.int32), - 'B_colind': (m.nB, np.int32), - 'M_rownnz': (m.nv, np.int32), - 'M_rowadr': (m.nv, np.int32), - 'M_colind': (m.nC, np.int32), - 'mapM2M': (m.nC, np.int32), - 'D_rownnz': (m.nv, np.int32), - 'D_rowadr': (m.nv, np.int32), - 'D_diag': (m.nv, np.int32), - 'D_colind': (m.nD, np.int32), - 'mapM2D': (m.nD, np.int32), - 'mapD2M': (m.nM, np.int32), 'qDeriv': (m.nD, float_), 'qLU': (m.nD, float_), 'qfrc_spring': (m.nv, float_), @@ -1497,7 +1484,7 @@ def _get_data_into( # TODO(taylorhowell): remove mapping once qM is deprecated # map inertia (sparse) to reduced inertia (compressed sparse) representation - result_i.M[:] = result_i.qM[result_i.mapM2M] + result_i.M[:] = result_i.qM[m.mapM2M] # recalculate qLD and qLDiagInv as MJX and MuJoCo have different # representations of the Cholesky decomposition. diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 4f9621a4..4e8b3711 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -94,7 +94,7 @@ class SmoothTest(absltest.TestCase): 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.nC): - qLDLegacy[d.mapM2M[i]] = d.qLD[i] + qLDLegacy[m.mapM2M[i]] = d.qLD[i] _assert_eq(qLDLegacy, dx._impl.qLD, 'qLD') _assert_attr_eq(d, dx._impl, 'qLDiagInv') # com_vel diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 0425477f..8d9ade18 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -599,6 +599,19 @@ class ModelC(PyTreeNode): sensor_plugin: jax.Array plugin: jax.Array plugin_stateadr: jax.Array + B_rownnz: jax.Array # pylint:disable=invalid-name + B_rowadr: jax.Array # pylint:disable=invalid-name + B_colind: jax.Array # pylint:disable=invalid-name + M_rownnz: jax.Array # pylint:disable=invalid-name + M_rowadr: jax.Array # pylint:disable=invalid-name + M_colind: jax.Array # pylint:disable=invalid-name + mapM2M: jax.Array # pylint:disable=invalid-name + D_rownnz: jax.Array # pylint:disable=invalid-name + D_rowadr: jax.Array # pylint:disable=invalid-name + D_diag: jax.Array # pylint:disable=invalid-name + D_colind: jax.Array # pylint:disable=invalid-name + mapM2D: jax.Array # pylint:disable=invalid-name + mapD2M: jax.Array # pylint:disable=invalid-name class ModelJAX(PyTreeNode): @@ -1020,19 +1033,6 @@ class DataC(PyTreeNode): plugin_data: jax.Array qH: jax.Array # pylint:disable=invalid-name qHDiagInv: jax.Array # pylint:disable=invalid-name - B_rownnz: jax.Array # pylint:disable=invalid-name - B_rowadr: jax.Array # pylint:disable=invalid-name - B_colind: jax.Array # pylint:disable=invalid-name - M_rownnz: jax.Array # pylint:disable=invalid-name - M_rowadr: jax.Array # pylint:disable=invalid-name - M_colind: jax.Array # pylint:disable=invalid-name - mapM2M: jax.Array # pylint:disable=invalid-name - D_rownnz: jax.Array # pylint:disable=invalid-name - D_rowadr: jax.Array # pylint:disable=invalid-name - D_diag: jax.Array # pylint:disable=invalid-name - D_colind: jax.Array # pylint:disable=invalid-name - mapM2D: jax.Array # pylint:disable=invalid-name - mapD2M: jax.Array # pylint:disable=invalid-name qDeriv: jax.Array # pylint:disable=invalid-name qLU: jax.Array # pylint:disable=invalid-name qfrc_spring: jax.Array diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index d786de4c..9508e14f 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -142,9 +142,6 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: # calculate some fields that cannot be easily computed inline nlsp = mjm.opt.ls_iterations # TODO(team): how to set nlsp? - # unfortunately we must create Data in order to get some model fields like M_rownnz - mjd = mujoco.MjData(mjm) - # dof lower triangle row and column indices (used in solver) dof_tri_row, dof_tri_col = np.tril_indices(mjm.nv) @@ -182,11 +179,11 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: for k in range(mjm.nv): # skip diagonal rows - if mjd.M_rownnz[k] == 1: + if mjm.M_rownnz[k] == 1: continue dof_depth[k] = dof_depth[mjm.dof_parentid[k]] + 1 i = mjm.dof_parentid[k] - diag_k = mjd.M_rowadr[k] + mjd.M_rownnz[k] - 1 + diag_k = mjm.M_rowadr[k] + mjm.M_rownnz[k] - 1 Madr_ki = diag_k - 1 while i > -1: qLD_updates.setdefault(dof_depth[i], []).append((i, k, Madr_ki)) @@ -487,10 +484,10 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: qM_mulm_j=wp.array(qM_mulm_j, dtype=int), qM_madr_ij=wp.array(qM_madr_ij, dtype=int), qLD_updates=qLD_updates, - M_rownnz=wp.array(mjd.M_rownnz, dtype=int), - M_rowadr=wp.array(mjd.M_rowadr, dtype=int), - M_colind=wp.array(mjd.M_colind, dtype=int), - mapM2M=wp.array(mjd.mapM2M, dtype=int), + M_rownnz=wp.array(mjm.M_rownnz, dtype=int), + M_rowadr=wp.array(mjm.M_rowadr, dtype=int), + M_colind=wp.array(mjm.M_colind, dtype=int), + mapM2M=wp.array(mjm.mapM2M, dtype=int), qM_tiles=qM_tiles, body_tree=body_tree, body_parentid=wp.array(mjm.body_parentid, dtype=int), diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index a3dbfb2b..71de6e3e 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -5770,110 +5770,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='1/diag(D) of modified M', array_extent=('nv',), ), - StructFieldDecl( - name='B_rownnz', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='body-dof: non-zeros in each row', - array_extent=('nbody',), - ), - StructFieldDecl( - name='B_rowadr', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='body-dof: address of each row in B_colind', - array_extent=('nbody',), - ), - StructFieldDecl( - name='B_colind', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='body-dof: column indices of non-zeros', - array_extent=('nB',), - ), - StructFieldDecl( - name='M_rownnz', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='reduced inertia: non-zeros in each row', - array_extent=('nv',), - ), - StructFieldDecl( - name='M_rowadr', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='reduced inertia: address of each row in M_colind', - array_extent=('nv',), - ), - StructFieldDecl( - name='M_colind', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='reduced inertia: column indices of non-zeros', - array_extent=('nC',), - ), - StructFieldDecl( - name='mapM2M', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='index mapping from qM to M', - array_extent=('nC',), - ), - StructFieldDecl( - name='D_rownnz', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='full inertia: non-zeros in each row', - array_extent=('nv',), - ), - StructFieldDecl( - name='D_rowadr', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='full inertia: address of each row in D_colind', - array_extent=('nv',), - ), - StructFieldDecl( - name='D_diag', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='full inertia: index of diagonal element', - array_extent=('nv',), - ), - StructFieldDecl( - name='D_colind', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='full inertia: column indices of non-zeros', - array_extent=('nD',), - ), - StructFieldDecl( - name='mapM2D', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='index mapping from qM to D', - array_extent=('nD',), - ), - StructFieldDecl( - name='mapD2M', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='index mapping from D to qM', - array_extent=('nM',), - ), StructFieldDecl( name='qDeriv', type=PointerType( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b77d546e..63c17db2 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2037,7 +2037,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 = m->M_rowadr[i] + m->M_rownnz[i] - 1; sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]); } @@ -2074,10 +2074,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } // traverse row j of C, marking new unique nonzeros - int nnzC = d->M_rownnz[j]; - int adrC = d->M_rowadr[j]; + int nnzC = m->M_rownnz[j]; + int adrC = m->M_rowadr[j]; for (int k=0; k < nnzC; k++) { - int c = d->M_colind[adrC + k]; + int c = m->M_colind[adrC + k]; if (marker[c] != r) { marker[c] = r; nnz++; @@ -2157,10 +2157,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { continue; } int j = B_colind[i]; - int adrC = d->M_rowadr[j]; + int adrC = m->M_rowadr[j]; mju_addToSclSparseInc(B + adrB, d->qLD + adrC, nnzB, B_colind + adrB, - d->M_rownnz[j]-1, d->M_colind + adrC, -b); + m->M_rownnz[j]-1, m->M_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 dda34cc1..d7310adf 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1472,9 +1472,9 @@ void mj_transmission(const mjModel* m, mjData* d) { // add tendon armature to M void mj_tendonArmature(const mjModel* m, mjData* d) { int nv = m->nv, ntendon = m->ntendon, issparse = mj_isSparse(m); - const int* M_rownnz = d->M_rownnz; - const int* M_rowadr = d->M_rowadr; - const int* M_colind = d->M_colind; + const int* M_rownnz = m->M_rownnz; + const int* M_rowadr = m->M_rowadr; + const int* M_colind = m->M_colind; for (int k=0; k < ntendon; k++) { mjtNum armature = m->tendon_armature[k]; @@ -1555,7 +1555,7 @@ void mj_crb(const mjModel* m, mjData* d) { if (m->dof_simplenum[i]) { int n = i + m->dof_simplenum[i]; for (; i < n; i++) { - d->M[d->M_rowadr[i]] = m->dof_M0[i]; + d->M[m->M_rowadr[i]] = m->dof_M0[i]; } // finish or else fall through with next row @@ -1565,7 +1565,7 @@ void mj_crb(const mjModel* m, mjData* d) { } // init M(i,i) with armature inertia - int Madr_ij = d->M_rowadr[i] + d->M_rownnz[i] - 1; + int Madr_ij = m->M_rowadr[i] + m->M_rownnz[i] - 1; d->M[Madr_ij] = m->dof_armature[i]; // precompute buf = crb_body_i * cdof_i @@ -1585,7 +1585,7 @@ void mj_makeM(const mjModel* m, mjData* d) { TM_START; mj_crb(m, d); mj_tendonArmature(m, d); - mju_scatter(d->qM, d->M, d->mapM2M, m->nC); + mju_scatter(d->qM, d->M, m->mapM2M, m->nC); TM_END(mjTIMER_POS_INERTIA); } @@ -1659,7 +1659,7 @@ void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD void mj_factorM(const mjModel* m, mjData* d) { TM_START; mju_copy(d->qLD, d->M, m->nC); - mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind); + mj_factorI(d->qLD, d->qLDiagInv, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind); TM_ADD(mjTIMER_POS_INERTIA); } @@ -1893,7 +1893,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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); } @@ -1904,9 +1904,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 = m->M_rownnz; + const int* rowadr = m->M_rowadr; + const int* colind = m->M_colind; const int* diagnum = m->dof_simplenum; const mjtNum* qLD = d->qLD; diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 7bd35a95..a17a167a 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -458,9 +458,9 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { mju_scl(row, Dcfrcbody + (m->dof_bodyid[i]*6+k)*nv, d->cdof[i*6+k], nv); // dense to sparse: qDeriv -= row - int end = d->D_rowadr[i] + d->D_rownnz[i]; - for (int adr=d->D_rowadr[i]; adr < end; adr++) { - d->qDeriv[adr] -= row[d->D_colind[adr]]; + int end = m->D_rowadr[i] + m->D_rownnz[i]; + for (int adr=m->D_rowadr[i]; adr < end; adr++) { + d->qDeriv[adr] -= row[m->D_colind[adr]]; } } } @@ -492,7 +492,7 @@ static void copyFromParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { } // copy: guaranteed to be at beginning of sparse array, due to sorting - mju_copy(mat + 6*d->B_rowadr[n], mat + 6*d->B_rowadr[m->body_parentid[n]], 6*ndof); + mju_copy(mat + 6*m->B_rowadr[n], mat + 6*m->B_rowadr[m->body_parentid[n]], 6*ndof); } @@ -507,10 +507,10 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { // find matching nonzeros int np = m->body_parentid[n]; int i = 0, ip = 0; - while (i < d->B_rownnz[n] && ip < d->B_rownnz[np]) { + while (i < m->B_rownnz[n] && ip < m->B_rownnz[np]) { // columns match - if (d->B_colind[d->B_rowadr[n] + i] == d->B_colind[d->B_rowadr[np] + ip]) { - mju_addTo(mat + 6*(d->B_rowadr[np] + ip), mat + 6*(d->B_rowadr[n] + i), 6); + if (m->B_colind[m->B_rowadr[n] + i] == m->B_colind[m->B_rowadr[np] + ip]) { + mju_addTo(mat + 6*(m->B_rowadr[np] + ip), mat + 6*(m->B_rowadr[n] + i), 6); // advance both i++; @@ -518,7 +518,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { } // mismatch columns: advance parent - else if (d->B_colind[d->B_rowadr[n] + i] > d->B_colind[d->B_rowadr[np] + ip]) { + else if (m->B_colind[m->B_rowadr[n] + i] > m->B_colind[m->B_rowadr[np] + ip]) { ip++; } @@ -534,7 +534,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { // derivative of cvel, cdof_dot w.r.t qvel static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot) { int nv = m->nv, nbody = m->nbody; - int* Badr = d->B_rowadr, * Dadr = d->D_rowadr; + int* Badr = m->B_rowadr, * Dadr = m->D_rowadr; mjtNum mat[36], matT[36]; // 6x6 matrices // forward pass over bodies: accumulate Dcvel, set Dcdofdot @@ -603,9 +603,9 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D // subtract d qfrc_bias / d qvel from qDeriv static void mjd_rne_vel(const mjModel* m, mjData* d) { int nv = m->nv, nbody = m->nbody; - const int* Badr = d->B_rowadr; - const int* Dadr = d->D_rowadr; - const int* Bnnz = d->B_rownnz; + const int* Badr = m->B_rowadr; + const int* Dadr = m->D_rowadr; + const int* Bnnz = m->B_rownnz; mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; @@ -710,10 +710,10 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* mju_scl(row, J+j*nv, J[i*nv+k] * B[i*n+j], nv); // add row to qDeriv(k,:) - int rownnz_k = d->D_rownnz[k]; + int rownnz_k = m->D_rownnz[k]; for (int s=0; s < rownnz_k; s++) { - int adr = d->D_rowadr[k] + s; - d->qDeriv[adr] += row[d->D_colind[adr]]; + int adr = m->D_rowadr[k] + s; + d->qDeriv[adr] += row[m->D_colind[adr]]; } } } @@ -748,8 +748,8 @@ static void addJTBJSparse( int colik = J_colind[ik]; // qDeriv(k,:) += J(j,:) * J(i,k)*B(i,j) - mju_addToSclSparseInc(d->qDeriv + d->D_rowadr[colik], J + adr_j, - d->D_rownnz[colik], d->D_colind + d->D_rowadr[colik], + mju_addToSclSparseInc(d->qDeriv + m->D_rowadr[colik], J + adr_j, + m->D_rownnz[colik], m->D_colind + m->D_rowadr[colik], nnz_j, J_colind + adr_j, J[ik]*B[i*n+j]); } @@ -1433,12 +1433,12 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { // dof damping for (int i=0; i < nv; i++) { - int nnz_i = d->D_rownnz[i]; + int nnz_i = m->D_rownnz[i]; for (int j=0; j < nnz_i; j++) { - int ij = d->D_rowadr[i] + j; + int ij = m->D_rowadr[i] + j; // identify diagonal element - if (d->D_colind[ij] == i) { + if (m->D_colind[ij] == i) { d->qDeriv[ij] -= m->dof_damping[i]; break; } diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index b5741232..f928b854 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -204,8 +204,8 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { // copy to i-th column of qDeriv for (int j=0; j < nv; j++) { - int adr = d->D_rowadr[j] + cnt[j]; - if (cnt[j] < d->D_rownnz[j] && d->D_colind[adr] == i) { + int adr = m->D_rowadr[j] + cnt[j]; + if (cnt[j] < m->D_rownnz[j] && m->D_colind[adr] == i) { d->qDeriv[adr] = fd[j]; cnt[j]++; } @@ -263,8 +263,8 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { // copy to sparse qDeriv for (int j=0; j < nv; j++) { - if (cnt[j] < d->D_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]] == i) { - d->qDeriv[d->D_rowadr[j]+cnt[j]] = fd[j]; + if (cnt[j] < m->D_rownnz[j] && m->D_colind[m->D_rowadr[j]+cnt[j]] == i) { + d->qDeriv[m->D_rowadr[j]+cnt[j]] = fd[j]; cnt[j]++; } } @@ -272,7 +272,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { // make sure final row counters equal rownnz for (int i=0; i < nv; i++) { - if (cnt[i] != d->D_rownnz[i]) { + if (cnt[i] != m->D_rownnz[i]) { mjERROR("error in constructing FD sparse derivative"); } } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 3acaa713..20e602ce 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -869,18 +869,18 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // qH = M + h*diag(B) mju_copy(d->qH, d->M, 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[m->M_rowadr[i] + m->M_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, d->M_colind); + mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); } // advance state and time @@ -1025,18 +1025,18 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mjd_smooth_vel(m, d, /* flg_bias = */ 1); // gather qLU <- qM (lower to full) - mju_gather(d->qLU, d->qM, d->mapM2D, nD); + mju_gather(d->qLU, d->qM, m->mapM2D, nD); // set qLU = qM - dt*qDeriv mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD); // factorize qLU int* scratch = mjSTACKALLOC(d, nv, int); - mju_factorLUSparse(d->qLU, nv, scratch, d->D_rownnz, d->D_rowadr, d->D_colind); + mju_factorLUSparse(d->qLU, nv, scratch, m->D_rownnz, m->D_rowadr, m->D_colind); } // solve for qacc: (qM - dt*qDeriv) * qacc = qfrc - mju_solveLUSparse(qacc, d->qLU, qfrc, nv, d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind); + mju_solveLUSparse(qacc, d->qLU, qfrc, nv, m->D_rownnz, m->D_rowadr, m->D_diag, m->D_colind); } // IMPLICITFAST @@ -1047,22 +1047,22 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { // modified mass matrix: gather MhB <- qDeriv (full to lower) mjtNum* MhB = mjSTACKALLOC(d, nM, mjtNum); - mju_gather(MhB, d->qDeriv, d->mapD2M, nM); + mju_gather(MhB, d->qDeriv, m->mapD2M, nM); // set MhB = M - dt*qDeriv mju_addScl(MhB, d->qM, MhB, -m->opt.timestep, nM); // gather qH <- MhB (legacy to CSR) - mju_gather(d->qH, MhB, d->mapM2M, nC); + mju_gather(d->qH, MhB, m->mapM2M, nC); // factorize in-place - mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, d->M_colind); + mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); } else { mjERROR("integrator must be implicit or implicitfast"); diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index b16b0d02..5a8973c4 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -116,14 +116,14 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { mjd_smooth_vel(m, d, /* flg_bias = */ 1); // gather qLU <- qM (lower to full) - mju_gather(d->qLU, d->qM, d->mapM2D, nD); + mju_gather(d->qLU, d->qM, m->mapM2D, nD); // set qLU = qM - dt*qDeriv mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD); // set qfrc = qLU * qacc mju_mulMatVecSparse(qfrc, d->qLU, qacc, nv, - d->D_rownnz, d->D_rowadr, d->D_colind, /*rowsuper=*/NULL); + m->D_rownnz, m->D_rowadr, m->D_colind, /*rowsuper=*/NULL); break; case mjINT_IMPLICITFAST: @@ -137,7 +137,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { // set M = M - dt*qDeriv (reduced to M nonzeros) mjtNum* qDerivReduced = mjSTACKALLOC(d, m->nM, mjtNum); for (int i=0; i < nM; i++) { - qDerivReduced[i] = d->qDeriv[d->mapD2M[i]]; + qDerivReduced[i] = d->qDeriv[m->mapD2M[i]]; } mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 9bd6dc93..3adb83bc 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1118,18 +1118,18 @@ void mj_makeBSparse(int nv, int nbody, int nB, // check D and B sparsity for consistency -static void checkDBSparse(const mjModel* m, mjData* d) { +static void checkDBSparse(const mjModel* m) { // process all dofs for (int j = 0; j < m->nv; j++) { // get body for this dof int i = m->dof_bodyid[j]; // D[row j] and B[row i] should be identical - if (d->D_rownnz[j] != d->B_rownnz[i]) { + if (m->D_rownnz[j] != m->B_rownnz[i]) { mjERROR("rows have different nnz"); } - for (int k = 0; k < d->D_rownnz[j]; k++) { - if (d->D_colind[d->D_rowadr[j] + k] != d->B_colind[d->B_rowadr[i] + k]) { + for (int k = 0; k < m->D_rownnz[j]; k++) { + if (m->D_colind[m->D_rowadr[j] + k] != m->B_colind[m->B_rowadr[i] + k]) { mjERROR("rows have different colind"); } } @@ -2004,36 +2004,8 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { } } - // construct sparse matrix representations - if (m->body_dofadr) { - mj_markStack(d); - int* remaining = mjSTACKALLOC(d, m->nv, int); - int* count = mjSTACKALLOC(d, m->nbody, int); - int* M = mjSTACKALLOC(d, m->nM, int); - int* D = mjSTACKALLOC(d, m->nD, int); - - // make D - mj_makeDofDofSparse(m->nv, m->nC, m->nD, m->nM, m->dof_parentid, m->dof_simplenum, - d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind, - /*reduced=*/0, /*upper=*/1, remaining); - - // make B, check D and B - mj_makeBSparse(m->nv, m->nbody, m->nB, m->body_dofnum, m->body_parentid, - m->body_dofadr, d->B_rownnz, d->B_rowadr, d->B_colind, count); - checkDBSparse(m, d); - - // make C - mj_makeDofDofSparse(m->nv, m->nC, m->nD, m->nM, m->dof_parentid, m->dof_simplenum, - d->M_rownnz, d->M_rowadr, NULL, d->M_colind, - /*reduced=*/1, /*upper=*/0, remaining); - - // make index mappings: mapM2D, mapD2M, mapM2C, mapM2M - mj_makeDofDofMaps(m->nv, m->nM, m->nC, m->nD, m->dof_Madr, m->dof_simplenum, m->dof_parentid, - d->D_rownnz, d->D_rowadr, d->D_colind, d->M_rownnz, d->M_rowadr, - d->mapM2D, d->mapD2M, d->mapM2M, remaining, M, D); - - mj_freeStack(d); - } + // check consistency of sparse matrix representations + checkDBSparse(m); // restore pluginstate and plugindata if (d->nplugin) { diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 7f898f8c..8382776d 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -521,7 +521,7 @@ void mj_island(const mjModel* m, mjData* d) { // 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, m->M_rownnz, m->M_rowadr, m->M_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 024de78c..13f6e9c6 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1155,51 +1155,26 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena d->moment_rowadr, d->moment_colind, fp, float_format); printArray("CRB", m->nbody, 10, d->crb, fp, float_format); printInertia("QM", d->qM, m, fp, float_format); - printSparse("M", d->M, m->nv, d->M_rownnz, - d->M_rowadr, d->M_colind, fp, float_format); - printSparse("QLD", d->qLD, m->nv, d->M_rownnz, - d->M_rowadr, d->M_colind, fp, float_format); + printSparse("M", d->M, m->nv, m->M_rownnz, + m->M_rowadr, m->M_colind, fp, float_format); + printSparse("QLD", d->qLD, m->nv, m->M_rownnz, + m->M_rowadr, m->M_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, m->M_rownnz, m->M_rowadr, m->M_colind, fp, float_format); printArray("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format); } - // B sparse structure - mj_printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, NULL, d->B_rownnz, NULL, - d->B_colind, fp); - printArrayInt("B_ROWNNZ", 1, m->nbody, d->B_rownnz, fp); - printArrayInt("B_ROWADR", 1, m->nbody, d->B_rowadr, fp); - printArrayInt("B_COLIND", 1, m->nB, d->B_colind, fp); - - - // M sparse structure - mj_printSparsity("M: reduced inertia matrix", m->nv, m->nv, d->M_rowadr, NULL, d->M_rownnz, - NULL, d->M_colind, fp); - printArrayInt("M_ROWNNZ", 1, m->nv, d->M_rownnz, fp); - printArrayInt("M_ROWADR", 1, m->nv, d->M_rowadr, fp); - printArrayInt("M_COLIND", 1, m->nC, d->M_colind, fp); - printArrayInt("MAPM2M", 1, m->nC, d->mapM2M, fp); - - // D sparse structure - mj_printSparsity("D: dof-dof matrix", m->nv, m->nv, - d->D_rowadr, d->D_diag, d->D_rownnz, NULL, d->D_colind, fp); - printArrayInt("D_ROWNNZ", 1, m->nv, d->D_rownnz, fp); - printArrayInt("D_ROWADR", 1, m->nv, d->D_rowadr, fp); - printArrayInt("D_COLIND", 1, m->nD, d->D_colind, fp); - printArrayInt("MAPM2D", 1, m->nD, d->mapM2D, fp); - printArrayInt("MAPD2M", 1, m->nM, d->mapD2M, fp); - // print qDeriv if (!mju_isZero(d->qDeriv, m->nD)) { - printSparse("QDERIV", d->qDeriv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind, + printSparse("QDERIV", d->qDeriv, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind, fp, float_format); } // print qLU if (!mju_isZero(d->qLU, m->nD)) { - printSparse("QLU", d->qLU, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind, fp, float_format); + printSparse("QLU", d->qLU, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind, fp, float_format); } // contact diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 16286d4a..6e8cfba4 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -885,9 +885,9 @@ 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_colind = d->M_colind; + ctx->M_rownnz = m->M_rownnz; + ctx->M_rowadr = m->M_rowadr; + ctx->M_colind = m->M_colind; ctx->M = d->M; ctx->qLD = d->qLD; ctx->qLDiagInv = d->qLDiagInv; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 99ec4d65..107545b1 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1058,14 +1058,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 = m->M_rowadr[i]; + res[i] += mju_dotSparse(qLD+adr, vec, m->M_rownnz[i] - 1, m->M_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 = m->M_rowadr[i] + m->M_rownnz[i] - 1; res[i] *= mju_sqrt(qLD[diag]); } } @@ -1084,7 +1084,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* buf_ind = mjSTACKALLOC(d, nv, int); mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, - d->M, d->M_rownnz, d->M_rowadr, d->M_colind, + d->M, m->M_rownnz, m->M_rowadr, m->M_colind, buf_val, buf_ind); mj_freeStack(d); @@ -1092,7 +1092,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, // dense else { - mju_addToSymSparse(dst, d->M, nv, d->M_rownnz, d->M_rowadr, d->M_colind, /*flg_upper=*/ 1); + mju_addToSymSparse(dst, d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind, /*flg_upper=*/ 1); } } diff --git a/test/benchmark/factorI_benchmark_test.cc b/test/benchmark/factorI_benchmark_test.cc index 62a42c6c..835856b2 100644 --- a/test/benchmark/factorI_benchmark_test.cc +++ b/test/benchmark/factorI_benchmark_test.cc @@ -46,7 +46,7 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { // M: mass matrix in CSR format mjtNum* M = mj_stackAllocNum(d, m->nC); - mju_gather(M, d->qM, d->mapM2M, m->nC); + mju_gather(M, d->qM, m->mapM2M, 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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); } } } diff --git a/test/benchmark/inertia_benchmark_test.cc b/test/benchmark/inertia_benchmark_test.cc index 457aa32a..a0fb14c0 100644 --- a/test/benchmark/inertia_benchmark_test.cc +++ b/test/benchmark/inertia_benchmark_test.cc @@ -48,7 +48,7 @@ static void BM_solve(benchmark::State& state, SolveType type) { // M: mass matrix in CSR format mjtNum* M = mj_stackAllocNum(d, m->nC); - mju_gather(M, d->qM, d->mapM2M, m->nC); + mju_gather(M, d->qM, m->mapM2M, 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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, - d->M_rownnz, d->M_rowadr, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); } } } diff --git a/test/benchmark/solveLD_benchmark_test.cc b/test/benchmark/solveLD_benchmark_test.cc index 65cf700c..20646b84 100644 --- a/test/benchmark/solveLD_benchmark_test.cc +++ b/test/benchmark/solveLD_benchmark_test.cc @@ -54,7 +54,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { // scatter into legacy matrix mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM); mju_zero(LDlegacy, m->nM); - mju_scatter(LDlegacy, d->qLD, d->mapM2M, m->nC); + mju_scatter(LDlegacy, d->qLD, m->mapM2M, m->nC); // benchmark while (state.KeepRunningBatch(kNumBenchmarkSteps)) { @@ -64,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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); } } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 87897f7c..dae8dd99 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -236,7 +236,7 @@ TEST_F(CoreSmoothTest, TendonArmature) { // put only CRB inertia in M2 mj_crb(m, d); - mju_scatter(d->qM, d->M, d->mapM2M, m->nC); + mju_scatter(d->qM, d->M, m->mapM2M, m->nC); vector M2(nv*nv); mj_fullM(m, M2.data(), d->qM); @@ -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); + model->M_rownnz, model->M_rowadr, model->M_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); + model->M_rownnz, model->M_rowadr, model->M_colind); for (int i=0; i < nv; i++) { for (int j=0; j < nv; j++) { // zero everything except the diagonal @@ -698,12 +698,12 @@ TEST_F(CoreSmoothTest, SolveLDs) { // copy M into LD: Legacy format vector LDlegacy(nM, 0); - mju_scatter(LDlegacy.data(), d->qLD, d->mapM2M, nC); + mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, 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); + m->M_rownnz, m->M_rowadr, m->M_colind); vector LDdense2(nv*nv); mj_fullM(m, LDdense2.data(), LDlegacy.data()); @@ -722,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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); // expect vectors to match up to floating point precision for (int i=0; i < nv; i++) { @@ -746,7 +746,7 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { // copy LD into LDlegacy: Legacy format vector LDlegacy(m->nM, 0); - mju_scatter(LDlegacy.data(), d->qLD, d->mapM2M, m->nC); + mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, 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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_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 = m->M_rowadr[i] + m->M_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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_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++) { @@ -824,17 +824,17 @@ TEST_F(CoreSmoothTest, FactorIs) { // copy qLDlegacy into qLDexpected: CSR format vector qLDexpected(nC); - mju_gather(qLDexpected.data(), qLDlegacy.data(), d->mapM2M, nC); + mju_gather(qLDexpected.data(), qLDlegacy.data(), m->mapM2M, nC); // copy qM into qLD: CSR format vector qLD(nC); - mju_gather(qLD.data(), d->qM, d->mapM2M, nC); + mju_gather(qLD.data(), d->qM, m->mapM2M, 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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); // expect outputs to match to floating point precision EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected)); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 3b0a8d92..5bfb5e89 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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_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, d->M_colind); + m->M_rownnz, m->M_rowadr, m->M_colind); mju_transpose(BcT, Bc, nu, nv); mju_scl(B, BcT, dt*dt, nu*nv); mju_scl(B+nu*nv, BcT, dt, nu*nv); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 8b566883..a7027b63 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4983,19 +4983,6 @@ public unsafe struct mjData_ { public double* subtree_angmom; public double* qH; public double* qHDiagInv; - public int* B_rownnz; - public int* B_rowadr; - public int* B_colind; - public int* M_rownnz; - public int* M_rowadr; - public int* M_colind; - public int* mapM2M; - public int* D_rownnz; - public int* D_rowadr; - public int* D_diag; - public int* D_colind; - public int* mapM2D; - public int* mapD2M; public double* qDeriv; public double* qLU; public double* actuator_force;