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

Remove dense code path for tendon Jacobian

Browse Source 
PiperOrigin-RevId: 872444531
Change-Id: I6180101abc49469a72aee8bec5726e1e94f142ec
This commit is contained in:
Taylor Howell
2026-02-19 09:53:11 -08:00
committed by Copybara-Service
parent 10524c28dd
commit 9efe41c0c1
11 changed files with 124 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mjx/mujoco/mjx/_src/io.py
+1 -3
View File
@@ -1111,7 +1111,7 @@ def _put_data_jax(
impl_fields['actuator_moment'] = moment impl_fields['actuator_moment'] = moment
# convert ten_J to dense matrix # convert ten_J to dense matrix
if mujoco.mj_isSparse(m): if m.ntendon:
ten_J = np.zeros((m.ntendon, m.nv)) ten_J = np.zeros((m.ntendon, m.nv))
mujoco.mju_sparse2dense( mujoco.mju_sparse2dense(
ten_J, ten_J,
@@ -1120,8 +1120,6 @@ def _put_data_jax(
d.ten_J_rowadr, d.ten_J_rowadr,
d.ten_J_colind, d.ten_J_colind,
) )
elif m.ntendon:
ten_J = d.ten_J.reshape((m.ntendon, m.nv))
else: else:
ten_J = np.zeros((m.ntendon, m.nv)) ten_J = np.zeros((m.ntendon, m.nv))
impl_fields['ten_J'] = ten_J impl_fields['ten_J'] = ten_J
mjx/mujoco/mjx/_src/smooth_test.py
+16 -15
View File
@@ -121,8 +121,14 @@ class SmoothTest(absltest.TestCase):
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
# tendon # tendon
dx = jax.jit(mjx.tendon)(mx, mjx.put_data(m, d)) 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 = np.zeros((m.ntendon, m.nv))
ten_J = d.ten_J.reshape((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(ten_J, dx._impl.ten_J, 'ten_J')
_assert_attr_eq(d, dx, 'ten_length') _assert_attr_eq(d, dx, 'ten_length')
# transmission # transmission
@@ -397,19 +403,14 @@ class TendonTest(parameterized.TestCase):
dx = jax.jit(mjx.forward)(mx, dx) dx = jax.jit(mjx.forward)(mx, dx)
_assert_eq(d.ten_length, dx.ten_length, 'ten_length') _assert_eq(d.ten_length, dx.ten_length, 'ten_length')
# convert ten_J for comparison based on jacobian mode ten_J = np.zeros((m.ntendon, m.nv))
if mujoco.mj_isSparse(m): mujoco.mju_sparse2dense(
ten_J = np.zeros((m.ntendon, m.nv)) ten_J,
mujoco.mju_sparse2dense( d.ten_J,
ten_J, d.ten_J_rownnz,
d.ten_J, d.ten_J_rowadr,
d.ten_J_rownnz, d.ten_J_colind,
d.ten_J_rowadr, )
d.ten_J_colind,
)
else:
# dense mode: just reshape
ten_J = d.ten_J.reshape((m.ntendon, m.nv))
_assert_eq(ten_J, dx._impl.ten_J, 'ten_J') _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_wrapnum, dx._impl.ten_wrapnum, 'ten_wrapnum')
_assert_eq(d.ten_wrapadr, dx._impl.ten_wrapadr, 'ten_wrapadr') _assert_eq(d.ten_wrapadr, dx._impl.ten_wrapadr, 'ten_wrapadr')
mjx/mujoco/mjx/warp/forward_test.py
+8 -1
View File
@@ -149,7 +149,14 @@ class ForwardTest(parameterized.TestCase):
tu.assert_attr_eq(dx, d, 'cam_xpos') 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_eq(dx.cam_xmat, d.cam_xmat.reshape((-1, 3, 3)), 'cam_xmat')
tu.assert_attr_eq(dx, d, 'ten_length') 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_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_wrapadr')
tu.assert_attr_eq(dx._impl, d, 'ten_wrapnum') tu.assert_attr_eq(dx._impl, d, 'ten_wrapnum')
src/engine/engine_core_constraint.c
+22 -16
View File
@@ -567,7 +567,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// copy Jacobian: sparse or dense // copy Jacobian: sparse or dense
if (issparse) { if (issparse) {
// add first or second chain
if (j == 0) { if (j == 0) {
NV = d->ten_J_rownnz[id[j]]; NV = d->ten_J_rownnz[id[j]];
mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); 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); mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2);
} }
} else { } 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) { if (m->tendon_frictionloss[i] > 0) {
int efcadr = d->nefc; int efcadr = d->nefc;
// add constraint // add constraint
mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv), if (issparse) {
0, 0, m->tendon_frictionloss[i], mj_addConstraint(m, d, d->ten_J + d->ten_J_rowadr[i],
1, mjCNSTR_FRICTION_TENDON, i, 0, 0, m->tendon_frictionloss[i],
issparse ? d->ten_J_rownnz[i] : 0, 1, mjCNSTR_FRICTION_TENDON, i,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL); 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 // set tendon_efcadr
if (d->tendon_efcadr[i] == -1) { if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr; d->tendon_efcadr[i] = efcadr;
@@ -877,19 +882,20 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// detect tendon limit // detect tendon limit
if (dist < margin) { if (dist < margin) {
// prepare Jacobian: sparse or dense // prepare Jacobian
int efcadr = d->nefc;
if (issparse) { if (issparse) {
mju_scl(jac, d->ten_J+d->ten_J_rowadr[i], -side, d->ten_J_rownnz[i]); 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 { } 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 // set tendon_efcadr
if (d->tendon_efcadr[i] == -1) { if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr; d->tendon_efcadr[i] = efcadr;
src/engine/engine_core_smooth.c
+52 -127
View File
@@ -904,7 +904,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// compute tendon lengths and moments // compute tendon lengths and moments
void mj_tendon(const mjModel* m, mjData* d) { 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; 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; mjtNum *L = d->ten_length, *J = d->ten_J;
@@ -913,28 +913,22 @@ void mj_tendon(const mjModel* m, mjData* d) {
} }
// allocate stack arrays // allocate stack arrays
int *chain = NULL, *buf_ind = NULL; int *chain, *buf_ind;
mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf = NULL; mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf;
mj_markStack(d); mj_markStack(d);
jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); jac1 = mjSTACKALLOC(d, 3*nv, mjtNum);
jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); jac2 = mjSTACKALLOC(d, 3*nv, mjtNum);
jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); jacdif = mjSTACKALLOC(d, 3*nv, mjtNum);
tmp = mjSTACKALLOC(d, nv, mjtNum); tmp = mjSTACKALLOC(d, nv, mjtNum);
if (issparse) { chain = mjSTACKALLOC(d, nv, int);
chain = mjSTACKALLOC(d, nv, int); buf_ind = mjSTACKALLOC(d, nv, int);
buf_ind = mjSTACKALLOC(d, nv, int); sparse_buf = mjSTACKALLOC(d, nv, mjtNum);
sparse_buf = mjSTACKALLOC(d, nv, mjtNum);
}
// clear results // clear results
mju_zero(L, nten); mju_zero(L, nten);
// clear Jacobian: sparse or dense // clear Jacobian
if (issparse) { mju_zeroInt(rownnz, nten);
mju_zeroInt(rownnz, nten);
} else {
mju_zero(J, nten*nv);
}
// sleep filtering // sleep filtering
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; 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]; int tendon_num = m->tendon_num[i];
// sparse Jacobian row init // 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 // process fixed tendon
if (m->wrap_type[adr] == mjWRAP_JOINT) { 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]]; L[i] += m->wrap_prm[adr+j] * d->qpos[m->jnt_qposadr[k]];
// add to moment // add to moment
if (issparse) { rownnz[i] = mju_combineSparse(J+rowadr[i], &m->wrap_prm[adr+j], 1, 1,
rownnz[i] = mju_combineSparse(J+rowadr[i], &m->wrap_prm[adr+j], 1, 1, rownnz[i], 1,
rownnz[i], 1, colind+rowadr[i], &m->jnt_dofadr[k],
colind+rowadr[i], &m->jnt_dofadr[k], sparse_buf, buf_ind);
sparse_buf, buf_ind);
}
// add to moment: dense
else {
J[i*nv + m->jnt_dofadr[k]] = m->wrap_prm[adr+j];
}
} }
continue; continue;
@@ -1060,40 +1045,23 @@ void mj_tendon(const mjModel* m, mjData* d) {
mji_sub3(dif, wpnt+3*k+3, wpnt+3*k); mji_sub3(dif, wpnt+3*k+3, wpnt+3*k);
mju_normalize3(dif); mju_normalize3(dif);
// sparse // get endpoint Jacobians, subtract
if (issparse) { int NV = mj_jacDifPair(m, d, chain,
// get endpoint Jacobians, subtract wbody[k], wbody[k+1], wpnt+3*k, wpnt+3*k+3,
int NV = mj_jacDifPair(m, d, chain, jac1, jac2, jacdif, NULL, NULL, NULL, /*issparse=*/1);
wbody[k], wbody[k+1], wpnt+3*k, wpnt+3*k+3,
jac1, jac2, jacdif, NULL, NULL, NULL, /*issparse=*/1);
// no dofs: skip // no dofs: skip
if (!NV) { if (!NV) {
continue; 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);
} }
// dense // apply chain rule to compute tendon Jacobian
else { mju_mulMatTVec(tmp, jacdif, dif, 3, NV);
// 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 // add to existing
mju_mulMatTVec(tmp, jacdif, dif, 3, nv); rownnz[i] = mju_combineSparse(J+rowadr[i], tmp, 1, 1/divisor,
rownnz[i], NV, colind+rowadr[i],
// add to existing chain, sparse_buf, buf_ind);
mju_addToScl(J + i*nv, tmp, 1/divisor, nv);
}
} }
} }
@@ -1449,28 +1417,14 @@ void mj_transmission(const mjModel* m, mjData* d) {
case mjTRN_TENDON: // tendon case mjTRN_TENDON: // tendon
length[i] = d->ten_length[id]*gear[0]; length[i] = d->ten_length[id]*gear[0];
// moment: sparse or dense // moment
if (issparse) { {
// sparsity
int ten_J_rownnz = d->ten_J_rownnz[id]; int ten_J_rownnz = d->ten_J_rownnz[id];
int ten_J_rowadr = d->ten_J_rowadr[id]; int ten_J_rowadr = d->ten_J_rowadr[id];
rownnz[i] = ten_J_rownnz; rownnz[i] = ten_J_rownnz;
mju_copyInt(colind + adr, d->ten_J_colind + ten_J_rowadr, 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); 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; break;
@@ -1743,7 +1697,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// add tendon armature to M // add tendon armature to M
void mj_tendonArmature(const mjModel* m, mjData* d) { 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_rownnz = m->M_rownnz;
const int* M_rowadr = m->M_rowadr; const int* M_rowadr = m->M_rowadr;
const int* M_colind = m->M_colind; const int* M_colind = m->M_colind;
@@ -1762,47 +1716,25 @@ void mj_tendonArmature(const mjModel* m, mjData* d) {
continue; continue;
} }
// dense // get sparse info for tendon k
if (!issparse) { int J_rowadr = d->ten_J_rowadr[k];
// M += armature * ten_J' * ten_J int J_rownnz = d->ten_J_rownnz[k];
mjtNum* ten_J = d->ten_J + nv*k; const int* J_colind = d->ten_J_colind + J_rowadr;
for (int i=0; i < nv; i++) { mjtNum* ten_J = d->ten_J + J_rowadr;
mjtNum ten_J_i = ten_J[i];
if (!ten_J_i) {
continue;
}
// M[i,:] += armature * ten_J[i] * ten_J // M += armature * ten_J' * ten_J
int start = M_rowadr[i]; for (int j=0; j < J_rownnz; j++) {
int end = start + M_rownnz[i]; mjtNum ten_J_i = ten_J[j];
for (int adr = start; adr < end; adr++) { if (!ten_J_i) {
d->M[adr] += armature * ten_J_i * ten_J[M_colind[adr]]; continue;
}
} }
}
// sparse // M[i,:] += armature * ten_J[i] * ten_J
else { int i = J_colind[j];
// get sparse info for tendon k int M_adr = M_rowadr[i];
int J_rowadr = d->ten_J_rowadr[k]; mju_addToSclSparseInc(d->M + M_adr, ten_J,
int J_rownnz = d->ten_J_rownnz[k]; M_rownnz[i], M_colind + M_adr,
const int* J_colind = d->ten_J_colind + J_rowadr; J_rownnz, J_colind, armature * ten_J_i);
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);
}
} }
} }
} }
@@ -2686,7 +2618,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// add bias force due to tendon armature // add bias force due to tendon armature
void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) {
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; 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; mjtNum* ten_Jdot = NULL;
mj_markStack(d); 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); mjtNum coef = armature * mju_dot(ten_Jdot, d->qvel, nv);
if (coef) { if (coef) {
// dense
if (!issparse) {
mju_addToScl(qfrc, d->ten_J + nv*i, coef, nv);
}
// sparse // sparse
else { int nnz = d->ten_J_rownnz[i];
int nnz = d->ten_J_rownnz[i]; int adr = d->ten_J_rowadr[i];
int adr = d->ten_J_rowadr[i]; const int* colind = d->ten_J_colind + adr;
const int* colind = d->ten_J_colind + adr; const mjtNum* ten_J = d->ten_J + adr;
const mjtNum* ten_J = d->ten_J + adr; for (int j=0; j < nnz; j++) {
for (int j=0; j < nnz; j++) { qfrc[colind[j]] += coef * ten_J[j];
qfrc[colind[j]] += coef * ten_J[j];
}
} }
} }
} }
src/engine/engine_derivative.c
+2 -6
View File
@@ -1775,12 +1775,8 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
continue; continue;
} }
// add sparse or dense // add sparse
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);
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);
}
} }
} }
src/engine/engine_forward.c
+3 -7
View File
@@ -229,13 +229,9 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
mju_mulMatVecSparse(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge, 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); m->flexedge_J_rownnz, m->flexedge_J_rowadr, m->flexedge_J_colind, NULL);
// tendon velocity: dense or sparse // tendon velocity: always sparse
if (mj_isSparse(m)) { mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon,
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);
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);
}
// actuator velocity: always sparse // actuator velocity: always sparse
if (!mjDISABLED(mjDSBL_ACTUATION)) { if (!mjDISABLED(mjDSBL_ACTUATION)) {
src/engine/engine_passive.c
+8 -14
View File
@@ -119,7 +119,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
int nv = m->nv, ntendon = m->ntendon; int nv = m->nv, ntendon = m->ntendon;
int has_spring = !mjDISABLED(mjDSBL_SPRING); int has_spring = !mjDISABLED(mjDSBL_SPRING);
int has_damping = !mjDISABLED(mjDSBL_DAMPER); int has_damping = !mjDISABLED(mjDSBL_DAMPER);
int issparse = mj_isSparse(m);
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
int nbody = sleep_filter ? d->nbody_awake : m->nbody; 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 // compute damper linear force along tendon
mjtNum frc_damper = -damping * d->ten_velocity[i]; mjtNum frc_damper = -damping * d->ten_velocity[i];
// transform to joint torque, add to qfrc_{spring, damper}: dense or sparse // transform to joint torque, add to qfrc_{spring, damper}
if (issparse) { if (frc_spring || frc_damper) {
if (frc_spring || frc_damper) { int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; for (int j=d->ten_J_rowadr[i]; j < end; j++) {
for (int j=d->ten_J_rowadr[i]; j < end; j++) { int k = d->ten_J_colind[j];
int k = d->ten_J_colind[j]; mjtNum J = d->ten_J[j];
mjtNum J = d->ten_J[j]; d->qfrc_spring[k] += J * frc_spring;
d->qfrc_spring[k] += J * frc_spring; d->qfrc_damper[k] += J * frc_damper;
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);
} }
} }
} }
src/engine/engine_print.c
+6 -10
View File
@@ -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("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format);
printArray2d("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format); printArray2d("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format);
if (!mj_isSparse(m)) { mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL,
printArray2d("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format); d->ten_J_rownnz, NULL, d->ten_J_colind, fp);
} else { printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp);
mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL, printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp);
d->ten_J_rownnz, NULL, d->ten_J_colind, fp); printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz,
printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); d->ten_J_rowadr, d->ten_J_colind, fp, float_format);
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++) { for (int i=0; i < m->ntendon; i++) {
fprintf(fp, "TENDON %d: %d wrap points\n", i, d->ten_wrapnum[i]); fprintf(fp, "TENDON %d: %d wrap points\n", i, d->ten_wrapnum[i]);
for (int j=0; j < d->ten_wrapnum[i]; j++) { for (int j=0; j < d->ten_wrapnum[i]; j++) {
src/engine/engine_setconst.c
+1 -10
View File
@@ -756,16 +756,7 @@ static void set0(mjModel* m, mjData* d) {
// compute tendon_invweight0 // compute tendon_invweight0
for (int i=0; i < m->ntendon; i++) { for (int i=0; i < m->ntendon; i++) {
// make dense vector into tmp mju_sparse2dense(tmp, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind);
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);
}
// solve into tmp+nv // solve into tmp+nv
mj_solveM(m, d, tmp+nv, tmp, 1); mj_solveM(m, d, tmp+nv, tmp, 1);
test/engine/engine_core_smooth_test.cc
+5 -9
View File
@@ -253,15 +253,11 @@ TEST_F(CoreSmoothTest, TendonArmature) {
// add tendon inertias to M2 using outer product // add tendon inertias to M2 using outer product
for (int j=0; j < m->ntendon; j++) { for (int j=0; j < m->ntendon; j++) {
// get tendon Jacobian // get tendon Jacobian
if (mj_isSparse(m)) { int rowadr = d->ten_J_rowadr[j];
int rowadr = d->ten_J_rowadr[j]; int* rownnz = d->ten_J_rownnz + j;
int* rownnz = d->ten_J_rownnz + j; int zero = 0;
int zero = 0; mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv,
mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, rownnz, &zero, d->ten_J_colind + rowadr);
rownnz, &zero, d->ten_J_colind + rowadr);
} else {
mju_copy(ten_J.data(), d->ten_J + j*nv, nv);
}
// get tendon inertia only, using outer product // get tendon inertia only, using outer product
mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv); mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);