From 9efe41c0c167f4299899ca443030aedf1823129a Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Thu, 19 Feb 2026 09:53:11 -0800 Subject: [PATCH] Remove dense code path for tendon Jacobian PiperOrigin-RevId: 872444531 Change-Id: I6180101abc49469a72aee8bec5726e1e94f142ec --- mjx/mujoco/mjx/_src/io.py | 4 +- mjx/mujoco/mjx/_src/smooth_test.py | 31 ++--- mjx/mujoco/mjx/warp/forward_test.py | 9 +- src/engine/engine_core_constraint.c | 38 +++--- src/engine/engine_core_smooth.c | 179 +++++++------------------ src/engine/engine_derivative.c | 8 +- src/engine/engine_forward.c | 10 +- src/engine/engine_passive.c | 22 ++- src/engine/engine_print.c | 16 +-- src/engine/engine_setconst.c | 11 +- test/engine/engine_core_smooth_test.cc | 14 +- 11 files changed, 124 insertions(+), 218 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index e683633b..8feef14d 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -1111,7 +1111,7 @@ def _put_data_jax( impl_fields['actuator_moment'] = moment # convert ten_J to dense matrix - if mujoco.mj_isSparse(m): + if m.ntendon: ten_J = np.zeros((m.ntendon, m.nv)) mujoco.mju_sparse2dense( ten_J, @@ -1120,8 +1120,6 @@ def _put_data_jax( d.ten_J_rowadr, d.ten_J_colind, ) - elif m.ntendon: - ten_J = d.ten_J.reshape((m.ntendon, m.nv)) else: ten_J = np.zeros((m.ntendon, m.nv)) impl_fields['ten_J'] = ten_J diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 667b3d63..ce8c2588 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -121,8 +121,14 @@ class SmoothTest(absltest.TestCase): mujoco.mj_forward(m, d) # tendon dx = jax.jit(mjx.tendon)(mx, mjx.put_data(m, d)) - # with dense jacobian mode, d.ten_J is already dense (ntendon*nv,), just reshape - ten_J = d.ten_J.reshape((m.ntendon, m.nv)) + ten_J = np.zeros((m.ntendon, m.nv)) + mujoco.mju_sparse2dense( + ten_J, + d.ten_J, + d.ten_J_rownnz, + d.ten_J_rowadr, + d.ten_J_colind, + ) _assert_eq(ten_J, dx._impl.ten_J, 'ten_J') _assert_attr_eq(d, dx, 'ten_length') # transmission @@ -397,19 +403,14 @@ class TendonTest(parameterized.TestCase): dx = jax.jit(mjx.forward)(mx, dx) _assert_eq(d.ten_length, dx.ten_length, 'ten_length') - # convert ten_J for comparison based on jacobian mode - if mujoco.mj_isSparse(m): - ten_J = np.zeros((m.ntendon, m.nv)) - mujoco.mju_sparse2dense( - ten_J, - d.ten_J, - d.ten_J_rownnz, - d.ten_J_rowadr, - d.ten_J_colind, - ) - else: - # dense mode: just reshape - ten_J = d.ten_J.reshape((m.ntendon, m.nv)) + ten_J = np.zeros((m.ntendon, m.nv)) + mujoco.mju_sparse2dense( + ten_J, + d.ten_J, + d.ten_J_rownnz, + d.ten_J_rowadr, + d.ten_J_colind, + ) _assert_eq(ten_J, dx._impl.ten_J, 'ten_J') _assert_eq(d.ten_wrapnum, dx._impl.ten_wrapnum, 'ten_wrapnum') _assert_eq(d.ten_wrapadr, dx._impl.ten_wrapadr, 'ten_wrapadr') diff --git a/mjx/mujoco/mjx/warp/forward_test.py b/mjx/mujoco/mjx/warp/forward_test.py index 020e924a..30eb21cf 100644 --- a/mjx/mujoco/mjx/warp/forward_test.py +++ b/mjx/mujoco/mjx/warp/forward_test.py @@ -149,7 +149,14 @@ class ForwardTest(parameterized.TestCase): tu.assert_attr_eq(dx, d, 'cam_xpos') tu.assert_eq(dx.cam_xmat, d.cam_xmat.reshape((-1, 3, 3)), 'cam_xmat') tu.assert_attr_eq(dx, d, 'ten_length') - ten_J = d.ten_J.reshape((m.ntendon, m.nv)) + ten_J = np.zeros((m.ntendon, m.nv)) + mujoco.mju_sparse2dense( + ten_J, + d.ten_J, + d.ten_J_rownnz, + d.ten_J_rowadr, + d.ten_J_colind, + ) tu.assert_eq(dx._impl.ten_J, ten_J, 'ten_J') tu.assert_attr_eq(dx._impl, d, 'ten_wrapadr') tu.assert_attr_eq(dx._impl, d, 'ten_wrapnum') diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index ab6abd39..d550766c 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -567,7 +567,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // copy Jacobian: sparse or dense if (issparse) { - // add first or second chain if (j == 0) { NV = d->ten_J_rownnz[id[j]]; mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); @@ -578,7 +577,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2); } } else { - mju_copy(jac[j], d->ten_J+id[j]*nv, nv); + mju_sparse2dense(jac[j], d->ten_J, 1, nv, d->ten_J_rownnz+id[j], d->ten_J_rowadr+id[j], d->ten_J_colind); } } } @@ -737,11 +736,17 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { if (m->tendon_frictionloss[i] > 0) { int efcadr = d->nefc; // add constraint - mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv), - 0, 0, m->tendon_frictionloss[i], - 1, mjCNSTR_FRICTION_TENDON, i, - issparse ? d->ten_J_rownnz[i] : 0, - issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL); + if (issparse) { + mj_addConstraint(m, d, d->ten_J + d->ten_J_rowadr[i], + 0, 0, m->tendon_frictionloss[i], + 1, mjCNSTR_FRICTION_TENDON, i, + d->ten_J_rownnz[i], + d->ten_J_colind+d->ten_J_rowadr[i]); + } else { + mju_sparse2dense(jac, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind); + mj_addConstraint(m, d, jac, 0, 0, m->tendon_frictionloss[i], + 1, mjCNSTR_FRICTION_TENDON, i, 0, NULL); + } // set tendon_efcadr if (d->tendon_efcadr[i] == -1) { d->tendon_efcadr[i] = efcadr; @@ -877,19 +882,20 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { // detect tendon limit if (dist < margin) { - // prepare Jacobian: sparse or dense + // prepare Jacobian + int efcadr = d->nefc; if (issparse) { mju_scl(jac, d->ten_J+d->ten_J_rowadr[i], -side, d->ten_J_rownnz[i]); + mj_addConstraint(m, d, jac, &dist, &margin, 0, + 1, mjCNSTR_LIMIT_TENDON, i, + d->ten_J_rownnz[i], + d->ten_J_colind+d->ten_J_rowadr[i]); } else { - mju_scl(jac, d->ten_J+i*nv, -side, nv); + mju_sparse2dense(jac, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind); + mju_scl(jac, jac, -side, nv); + mj_addConstraint(m, d, jac, &dist, &margin, 0, + 1, mjCNSTR_LIMIT_TENDON, i, 0, NULL); } - - // add constraint - int efcadr = d->nefc; - mj_addConstraint(m, d, jac, &dist, &margin, 0, - 1, mjCNSTR_LIMIT_TENDON, i, - issparse ? d->ten_J_rownnz[i] : 0, - issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL); // set tendon_efcadr if (d->tendon_efcadr[i] == -1) { d->tendon_efcadr[i] = efcadr; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ba5c7254..1d3fed48 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -904,7 +904,7 @@ void mj_flex(const mjModel* m, mjData* d) { // compute tendon lengths and moments void mj_tendon(const mjModel* m, mjData* d) { - int issparse = mj_isSparse(m), nv = m->nv, nten = m->ntendon; + int nv = m->nv, nten = m->ntendon; int *rownnz = d->ten_J_rownnz, *rowadr = d->ten_J_rowadr, *colind = d->ten_J_colind; mjtNum *L = d->ten_length, *J = d->ten_J; @@ -913,28 +913,22 @@ void mj_tendon(const mjModel* m, mjData* d) { } // allocate stack arrays - int *chain = NULL, *buf_ind = NULL; - mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf = NULL; + int *chain, *buf_ind; + mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf; mj_markStack(d); jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); tmp = mjSTACKALLOC(d, nv, mjtNum); - if (issparse) { - chain = mjSTACKALLOC(d, nv, int); - buf_ind = mjSTACKALLOC(d, nv, int); - sparse_buf = mjSTACKALLOC(d, nv, mjtNum); - } + chain = mjSTACKALLOC(d, nv, int); + buf_ind = mjSTACKALLOC(d, nv, int); + sparse_buf = mjSTACKALLOC(d, nv, mjtNum); // clear results mju_zero(L, nten); - // clear Jacobian: sparse or dense - if (issparse) { - mju_zeroInt(rownnz, nten); - } else { - mju_zero(J, nten*nv); - } + // clear Jacobian + mju_zeroInt(rownnz, nten); // sleep filtering int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; @@ -954,9 +948,7 @@ void mj_tendon(const mjModel* m, mjData* d) { int tendon_num = m->tendon_num[i]; // sparse Jacobian row init - if (issparse) { - rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0); - } + rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0); // process fixed tendon if (m->wrap_type[adr] == mjWRAP_JOINT) { @@ -969,17 +961,10 @@ void mj_tendon(const mjModel* m, mjData* d) { L[i] += m->wrap_prm[adr+j] * d->qpos[m->jnt_qposadr[k]]; // add to moment - if (issparse) { - rownnz[i] = mju_combineSparse(J+rowadr[i], &m->wrap_prm[adr+j], 1, 1, - rownnz[i], 1, - colind+rowadr[i], &m->jnt_dofadr[k], - sparse_buf, buf_ind); - } - - // add to moment: dense - else { - J[i*nv + m->jnt_dofadr[k]] = m->wrap_prm[adr+j]; - } + rownnz[i] = mju_combineSparse(J+rowadr[i], &m->wrap_prm[adr+j], 1, 1, + rownnz[i], 1, + colind+rowadr[i], &m->jnt_dofadr[k], + sparse_buf, buf_ind); } continue; @@ -1060,40 +1045,23 @@ void mj_tendon(const mjModel* m, mjData* d) { mji_sub3(dif, wpnt+3*k+3, wpnt+3*k); mju_normalize3(dif); - // sparse - if (issparse) { - // get endpoint Jacobians, subtract - int NV = mj_jacDifPair(m, d, chain, - wbody[k], wbody[k+1], wpnt+3*k, wpnt+3*k+3, - jac1, jac2, jacdif, NULL, NULL, NULL, /*issparse=*/1); + // get endpoint Jacobians, subtract + int NV = mj_jacDifPair(m, d, chain, + wbody[k], wbody[k+1], wpnt+3*k, wpnt+3*k+3, + jac1, jac2, jacdif, NULL, NULL, NULL, /*issparse=*/1); - // no dofs: skip - if (!NV) { - continue; - } - - // apply chain rule to compute tendon Jacobian - mju_mulMatTVec(tmp, jacdif, dif, 3, NV); - - // add to existing - rownnz[i] = mju_combineSparse(J+rowadr[i], tmp, 1, 1/divisor, - rownnz[i], NV, colind+rowadr[i], - chain, sparse_buf, buf_ind); + // no dofs: skip + if (!NV) { + continue; } - // dense - else { - // get endpoint Jacobians, subtract - mj_jac(m, d, jac1, 0, wpnt+3*k, wbody[k]); - mj_jac(m, d, jac2, 0, wpnt+3*k+3, wbody[k+1]); - mju_sub(jacdif, jac2, jac1, 3*nv); + // apply chain rule to compute tendon Jacobian + mju_mulMatTVec(tmp, jacdif, dif, 3, NV); - // apply chain rule to compute tendon Jacobian - mju_mulMatTVec(tmp, jacdif, dif, 3, nv); - - // add to existing - mju_addToScl(J + i*nv, tmp, 1/divisor, nv); - } + // add to existing + rownnz[i] = mju_combineSparse(J+rowadr[i], tmp, 1, 1/divisor, + rownnz[i], NV, colind+rowadr[i], + chain, sparse_buf, buf_ind); } } @@ -1449,28 +1417,14 @@ void mj_transmission(const mjModel* m, mjData* d) { case mjTRN_TENDON: // tendon length[i] = d->ten_length[id]*gear[0]; - // moment: sparse or dense - if (issparse) { - // sparsity + // moment + { int ten_J_rownnz = d->ten_J_rownnz[id]; int ten_J_rowadr = d->ten_J_rowadr[id]; rownnz[i] = ten_J_rownnz; mju_copyInt(colind + adr, d->ten_J_colind + ten_J_rowadr, ten_J_rownnz); mju_scl(moment + adr, d->ten_J + ten_J_rowadr, gear[0], ten_J_rownnz); - } else { - mju_scl(moment+adr, d->ten_J + id*nv, gear[0], nv); - - // sparsity (compress) - nnz = 0; - for (int j = 0; j < nv; j++) { - if (moment[adr+j]) { - moment[adr+nnz] = moment[adr+j]; - colind[adr+nnz] = j; - nnz++; - } - } - rownnz[i] = nnz; } break; @@ -1743,7 +1697,7 @@ 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); + int nv = m->nv, ntendon = m->ntendon; const int* M_rownnz = m->M_rownnz; const int* M_rowadr = m->M_rowadr; const int* M_colind = m->M_colind; @@ -1762,47 +1716,25 @@ void mj_tendonArmature(const mjModel* m, mjData* d) { continue; } - // dense - if (!issparse) { - // M += armature * ten_J' * ten_J - mjtNum* ten_J = d->ten_J + nv*k; - for (int i=0; i < nv; i++) { - mjtNum ten_J_i = ten_J[i]; - if (!ten_J_i) { - continue; - } + // get sparse info for tendon k + int J_rowadr = d->ten_J_rowadr[k]; + int J_rownnz = d->ten_J_rownnz[k]; + const int* J_colind = d->ten_J_colind + J_rowadr; + mjtNum* ten_J = d->ten_J + J_rowadr; - // M[i,:] += armature * ten_J[i] * ten_J - int start = M_rowadr[i]; - int end = start + M_rownnz[i]; - for (int adr = start; adr < end; adr++) { - d->M[adr] += armature * ten_J_i * ten_J[M_colind[adr]]; - } + // M += armature * ten_J' * ten_J + for (int j=0; j < J_rownnz; j++) { + mjtNum ten_J_i = ten_J[j]; + if (!ten_J_i) { + continue; } - } - // sparse - else { - // get sparse info for tendon k - int J_rowadr = d->ten_J_rowadr[k]; - int J_rownnz = d->ten_J_rownnz[k]; - const int* J_colind = d->ten_J_colind + J_rowadr; - mjtNum* ten_J = d->ten_J + J_rowadr; - - // M += armature * ten_J' * ten_J - for (int j=0; j < J_rownnz; j++) { - mjtNum ten_J_i = ten_J[j]; - if (!ten_J_i) { - continue; - } - - // M[i,:] += armature * ten_J[i] * ten_J - int i = J_colind[j]; - int M_adr = M_rowadr[i]; - mju_addToSclSparseInc(d->M + M_adr, ten_J, - M_rownnz[i], M_colind + M_adr, - J_rownnz, J_colind, armature * ten_J_i); - } + // M[i,:] += armature * ten_J[i] * ten_J + int i = J_colind[j]; + int M_adr = M_rowadr[i]; + mju_addToSclSparseInc(d->M + M_adr, ten_J, + M_rownnz[i], M_colind + M_adr, + J_rownnz, J_colind, armature * ten_J_i); } } } @@ -2686,7 +2618,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // add bias force due to tendon armature void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; - int ntendon = m->ntendon, nv = m->nv, issparse = mj_isSparse(m); + int ntendon = m->ntendon, nv = m->nv; mjtNum* ten_Jdot = NULL; mj_markStack(d); @@ -2716,20 +2648,13 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { mjtNum coef = armature * mju_dot(ten_Jdot, d->qvel, nv); if (coef) { - // dense - if (!issparse) { - mju_addToScl(qfrc, d->ten_J + nv*i, coef, nv); - } - // sparse - else { - int nnz = d->ten_J_rownnz[i]; - int adr = d->ten_J_rowadr[i]; - const int* colind = d->ten_J_colind + adr; - const mjtNum* ten_J = d->ten_J + adr; - for (int j=0; j < nnz; j++) { - qfrc[colind[j]] += coef * ten_J[j]; - } + int nnz = d->ten_J_rownnz[i]; + int adr = d->ten_J_rowadr[i]; + const int* colind = d->ten_J_colind + adr; + const mjtNum* ten_J = d->ten_J + adr; + for (int j=0; j < nnz; j++) { + qfrc[colind[j]] += coef * ten_J[j]; } } } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 6c37710f..67b0d02e 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1775,12 +1775,8 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { continue; } - // add sparse or dense - if (mj_isSparse(m)) { - addJTBJSparse(m, d, d->ten_J, &B, 1, i, d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind); - } else { - addJTBJ(m, d, d->ten_J+i*nv, &B, 1); - } + // add sparse + addJTBJSparse(m, d, d->ten_J, &B, 1, i, d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind); } } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 5b336890..6491cdca 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -229,13 +229,9 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { mju_mulMatVecSparse(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge, m->flexedge_J_rownnz, m->flexedge_J_rowadr, m->flexedge_J_colind, NULL); - // tendon velocity: dense or sparse - if (mj_isSparse(m)) { - mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, - d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind, NULL); - } else { - mju_mulMatVec(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, m->nv); - } + // tendon velocity: always sparse + mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, + d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind, NULL); // actuator velocity: always sparse if (!mjDISABLED(mjDSBL_ACTUATION)) { diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index d52e478c..5e26c2c3 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -119,7 +119,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) { int nv = m->nv, ntendon = m->ntendon; int has_spring = !mjDISABLED(mjDSBL_SPRING); int has_damping = !mjDISABLED(mjDSBL_DAMPER); - int issparse = mj_isSparse(m); int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; int nbody = sleep_filter ? d->nbody_awake : m->nbody; @@ -472,20 +471,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // compute damper linear force along tendon mjtNum frc_damper = -damping * d->ten_velocity[i]; - // transform to joint torque, add to qfrc_{spring, damper}: dense or sparse - if (issparse) { - if (frc_spring || frc_damper) { - int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; - for (int j=d->ten_J_rowadr[i]; j < end; j++) { - int k = d->ten_J_colind[j]; - mjtNum J = d->ten_J[j]; - d->qfrc_spring[k] += J * frc_spring; - d->qfrc_damper[k] += J * frc_damper; - } + // transform to joint torque, add to qfrc_{spring, damper} + if (frc_spring || frc_damper) { + int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; + for (int j=d->ten_J_rowadr[i]; j < end; j++) { + int k = d->ten_J_colind[j]; + mjtNum J = d->ten_J[j]; + d->qfrc_spring[k] += J * frc_spring; + d->qfrc_damper[k] += J * frc_damper; } - } else { - if (frc_spring) mju_addToScl(d->qfrc_spring, d->ten_J+i*nv, frc_spring, nv); - if (frc_damper) mju_addToScl(d->qfrc_damper, d->ten_J+i*nv, frc_damper, nv); } } } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 5cee8f49..0a927715 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1441,16 +1441,12 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format); printArray2d("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format); - if (!mj_isSparse(m)) { - printArray2d("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format); - } else { - mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL, - d->ten_J_rownnz, NULL, d->ten_J_colind, fp); - printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); - printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); - printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, - d->ten_J_rowadr, d->ten_J_colind, fp, float_format); - } + mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL, + d->ten_J_rownnz, NULL, d->ten_J_colind, fp); + printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); + printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); + printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, + d->ten_J_rowadr, d->ten_J_colind, fp, float_format); for (int i=0; i < m->ntendon; i++) { fprintf(fp, "TENDON %d: %d wrap points\n", i, d->ten_wrapnum[i]); for (int j=0; j < d->ten_wrapnum[i]; j++) { diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 7b5fe6bb..435291a4 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -756,16 +756,7 @@ static void set0(mjModel* m, mjData* d) { // compute tendon_invweight0 for (int i=0; i < m->ntendon; i++) { - // make dense vector into tmp - if (mj_isSparse(m)) { - mju_zero(tmp, nv); - int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; - for (int j=d->ten_J_rowadr[i]; j < end; j++) { - tmp[d->ten_J_colind[j]] = d->ten_J[j]; - } - } else { - mju_copy(tmp, d->ten_J+i*nv, nv); - } + mju_sparse2dense(tmp, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind); // solve into tmp+nv mj_solveM(m, d, tmp+nv, tmp, 1); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 53f36b38..7b292bfe 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -253,15 +253,11 @@ TEST_F(CoreSmoothTest, TendonArmature) { // add tendon inertias to M2 using outer product for (int j=0; j < m->ntendon; j++) { // get tendon Jacobian - if (mj_isSparse(m)) { - int rowadr = d->ten_J_rowadr[j]; - int* rownnz = d->ten_J_rownnz + j; - int zero = 0; - mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, - rownnz, &zero, d->ten_J_colind + rowadr); - } else { - mju_copy(ten_J.data(), d->ten_J + j*nv, nv); - } + int rowadr = d->ten_J_rowadr[j]; + int* rownnz = d->ten_J_rownnz + j; + int zero = 0; + mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, + rownnz, &zero, d->ten_J_colind + rowadr); // get tendon inertia only, using outer product mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);