Move tendon sparsity fields from mjData to mjModel

PiperOrigin-RevId: 875087590
Change-Id: I1a5489d2d2011795ee38b09d547e68acd72e3cc7
This commit is contained in:
Taylor Howell
2026-02-25 04:31:37 -08:00
committed by Copybara-Service
parent 253494739a
commit 52ed96bc3a
25 changed files with 332 additions and 178 deletions
+4 -4
View File
@@ -289,9 +289,6 @@ struct mjData_ {
// computed by mj_fwdPosition/mj_tendon // computed by mj_fwdPosition/mj_tendon
int* ten_wrapadr; // start address of tendon's path (ntendon x 1) int* ten_wrapadr; // start address of tendon's path (ntendon x 1)
int* ten_wrapnum; // number of wrap points in path (ntendon x 1) int* ten_wrapnum; // number of wrap points in path (ntendon x 1)
int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1)
int* ten_J_rowadr; // row start address in colind array (ntendon x 1)
int* ten_J_colind; // column indices in sparse Jacobian (nJten x 1)
mjtNum* ten_J; // tendon Jacobian (nJten x 1) mjtNum* ten_J; // tendon Jacobian (nJten x 1)
mjtNum* ten_length; // tendon lengths (ntendon x 1) mjtNum* ten_length; // tendon lengths (ntendon x 1)
int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap x 2) int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap x 2)
@@ -1070,6 +1067,7 @@ struct mjModel_ {
mjtSize nexclude; // number of excluded geom pairs mjtSize nexclude; // number of excluded geom pairs
mjtSize neq; // number of equality constraints mjtSize neq; // number of equality constraints
mjtSize ntendon; // number of tendons mjtSize ntendon; // number of tendons
mjtSize nJten; // number of non-zeros in sparse ten_J matrix
mjtSize nwrap; // number of wrap objects in all tendon paths mjtSize nwrap; // number of wrap objects in all tendon paths
mjtSize nsensor; // number of sensors mjtSize nsensor; // number of sensors
mjtSize nnumeric; // number of numeric custom fields mjtSize nnumeric; // number of numeric custom fields
@@ -1096,7 +1094,6 @@ struct mjModel_ {
// sizes set after mjModel construction // sizes set after mjModel construction
mjtSize nnames_map; // number of slots in the names hash map mjtSize nnames_map; // number of slots in the names hash map
mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix
mjtSize nJten; // number of non-zeros in sparse ten_J matrix
mjtSize ngravcomp; // number of bodies with nonzero gravcomp mjtSize ngravcomp; // number of bodies with nonzero gravcomp
mjtSize nemax; // number of potential equality-constraint rows mjtSize nemax; // number of potential equality-constraint rows
mjtSize njmax; // number of available rows in constraint Jacobian (legacy) mjtSize njmax; // number of available rows in constraint Jacobian (legacy)
@@ -1490,6 +1487,9 @@ struct mjModel_ {
int* tendon_group; // group for visibility (ntendon x 1) int* tendon_group; // group for visibility (ntendon x 1)
int* tendon_treenum; // number of trees along tendon's path (ntendon x 1) int* tendon_treenum; // number of trees along tendon's path (ntendon x 1)
int* tendon_treeid; // first two trees along tendon's path (ntendon x 2) int* tendon_treeid; // first two trees along tendon's path (ntendon x 2)
int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1)
int* ten_J_rowadr; // row start address in colind array (ntendon x 1)
int* ten_J_colind; // column indices in sparse Jacobian (nJten x 1)
mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1) mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1)
mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1) mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1)
mjtNum* tendon_width; // width for rendering (ntendon x 1) mjtNum* tendon_width; // width for rendering (ntendon x 1)
-3
View File
@@ -323,9 +323,6 @@ struct mjData_ {
// computed by mj_fwdPosition/mj_tendon // computed by mj_fwdPosition/mj_tendon
int* ten_wrapadr; // start address of tendon's path (ntendon x 1) int* ten_wrapadr; // start address of tendon's path (ntendon x 1)
int* ten_wrapnum; // number of wrap points in path (ntendon x 1) int* ten_wrapnum; // number of wrap points in path (ntendon x 1)
int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1)
int* ten_J_rowadr; // row start address in colind array (ntendon x 1)
int* ten_J_colind; // column indices in sparse Jacobian (nJten x 1)
mjtNum* ten_J; // tendon Jacobian (nJten x 1) mjtNum* ten_J; // tendon Jacobian (nJten x 1)
mjtNum* ten_length; // tendon lengths (ntendon x 1) mjtNum* ten_length; // tendon lengths (ntendon x 1)
int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap x 2) int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap x 2)
+4 -1
View File
@@ -728,6 +728,7 @@ struct mjModel_ {
mjtSize nexclude; // number of excluded geom pairs mjtSize nexclude; // number of excluded geom pairs
mjtSize neq; // number of equality constraints mjtSize neq; // number of equality constraints
mjtSize ntendon; // number of tendons mjtSize ntendon; // number of tendons
mjtSize nJten; // number of non-zeros in sparse ten_J matrix
mjtSize nwrap; // number of wrap objects in all tendon paths mjtSize nwrap; // number of wrap objects in all tendon paths
mjtSize nsensor; // number of sensors mjtSize nsensor; // number of sensors
mjtSize nnumeric; // number of numeric custom fields mjtSize nnumeric; // number of numeric custom fields
@@ -754,7 +755,6 @@ struct mjModel_ {
// sizes set after mjModel construction // sizes set after mjModel construction
mjtSize nnames_map; // number of slots in the names hash map mjtSize nnames_map; // number of slots in the names hash map
mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix
mjtSize nJten; // number of non-zeros in sparse ten_J matrix
mjtSize ngravcomp; // number of bodies with nonzero gravcomp mjtSize ngravcomp; // number of bodies with nonzero gravcomp
mjtSize nemax; // number of potential equality-constraint rows mjtSize nemax; // number of potential equality-constraint rows
mjtSize njmax; // number of available rows in constraint Jacobian (legacy) mjtSize njmax; // number of available rows in constraint Jacobian (legacy)
@@ -1148,6 +1148,9 @@ struct mjModel_ {
int* tendon_group; // group for visibility (ntendon x 1) int* tendon_group; // group for visibility (ntendon x 1)
int* tendon_treenum; // number of trees along tendon's path (ntendon x 1) int* tendon_treenum; // number of trees along tendon's path (ntendon x 1)
int* tendon_treeid; // first two trees along tendon's path (ntendon x 2) int* tendon_treeid; // first two trees along tendon's path (ntendon x 2)
int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1)
int* ten_J_rowadr; // row start address in colind array (ntendon x 1)
int* ten_J_colind; // column indices in sparse Jacobian (nJten x 1)
mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1) mjtByte* tendon_limited; // does tendon have length limits (ntendon x 1)
mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1) mjtByte* tendon_actfrclimited; // does tendon have actuator force limits (ntendon x 1)
mjtNum* tendon_width; // width for rendering (ntendon x 1) mjtNum* tendon_width; // width for rendering (ntendon x 1)
+4 -4
View File
@@ -215,6 +215,7 @@
X( nexclude ) \ X( nexclude ) \
X( neq ) \ X( neq ) \
X( ntendon ) \ X( ntendon ) \
X( nJten ) \
X( nwrap ) \ X( nwrap ) \
X( nsensor ) \ X( nsensor ) \
X( nnumeric ) \ X( nnumeric ) \
@@ -239,7 +240,6 @@
X( npaths ) \ X( npaths ) \
X( nnames_map ) \ X( nnames_map ) \
X( nJmom ) \ X( nJmom ) \
X( nJten ) \
X( ngravcomp ) \ X( ngravcomp ) \
X( nemax ) \ X( nemax ) \
X( njmax ) \ X( njmax ) \
@@ -629,6 +629,9 @@
X ( int, tendon_group, ntendon, 1 ) \ X ( int, tendon_group, ntendon, 1 ) \
X ( int, tendon_treenum, ntendon, 1 ) \ X ( int, tendon_treenum, ntendon, 1 ) \
X ( int, tendon_treeid, ntendon, 2 ) \ X ( int, tendon_treeid, ntendon, 2 ) \
X ( int, ten_J_rownnz, ntendon, 1 ) \
X ( int, ten_J_rowadr, ntendon, 1 ) \
X ( int, ten_J_colind, nJten, 1 ) \
X ( mjtByte, tendon_limited, ntendon, 1 ) \ X ( mjtByte, tendon_limited, ntendon, 1 ) \
X ( mjtByte, tendon_actfrclimited, ntendon, 1 ) \ X ( mjtByte, tendon_actfrclimited, ntendon, 1 ) \
X ( mjtNum, tendon_width, ntendon, 1 ) \ X ( mjtNum, tendon_width, ntendon, 1 ) \
@@ -848,9 +851,6 @@
X ( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ X ( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \
X ( int, ten_wrapadr, ntendon, 1 ) \ X ( int, ten_wrapadr, ntendon, 1 ) \
X ( int, ten_wrapnum, ntendon, 1 ) \ X ( int, ten_wrapnum, ntendon, 1 ) \
X ( int, ten_J_rownnz, ntendon, 1 ) \
X ( int, ten_J_rowadr, ntendon, 1 ) \
X ( int, ten_J_colind, nJten, 1 ) \
X ( mjtNum, ten_J, nJten, 1 ) \ X ( mjtNum, ten_J, nJten, 1 ) \
X ( mjtNum, ten_length, ntendon, 1 ) \ X ( mjtNum, ten_length, ntendon, 1 ) \
X ( int, wrap_obj, nwrap, 2 ) \ X ( int, wrap_obj, nwrap, 2 ) \
+6 -6
View File
@@ -1129,9 +1129,9 @@ def _put_data_jax(
mujoco.mju_sparse2dense( mujoco.mju_sparse2dense(
ten_J, ten_J,
d.ten_J, d.ten_J,
d.ten_J_rownnz, m.ten_J_rownnz,
d.ten_J_rowadr, m.ten_J_rowadr,
d.ten_J_colind, m.ten_J_colind,
) )
else: else:
ten_J = np.zeros((m.ntendon, m.nv)) ten_J = np.zeros((m.ntendon, m.nv))
@@ -1251,6 +1251,9 @@ def _put_data_c(
for f in types.DataC.fields() for f in types.DataC.fields()
if hasattr(d, f.name) if hasattr(d, f.name)
} }
for f in types.DataC.fields():
if not hasattr(d, f.name) and hasattr(m, f.name):
impl_fields[f.name] = getattr(m, f.name)
# TODO(stunya): support islanding via C impl. # TODO(stunya): support islanding via C impl.
impl_fields['solver_niter'] = impl_fields['solver_niter'][0] impl_fields['solver_niter'] = impl_fields['solver_niter'][0]
@@ -1662,9 +1665,6 @@ def _get_data_into(
) )
else: else:
ten_j = d_i._impl.ten_J ten_j = d_i._impl.ten_J
result_i.ten_J_rownnz[:] = ten_j_rownnz
result_i.ten_J_rowadr[:] = ten_j_rowadr
result_i.ten_J_colind[:] = ten_j_colind
result_i.ten_J[:] = ten_j result_i.ten_J[:] = ten_j
continue continue
+6 -6
View File
@@ -125,9 +125,9 @@ class SmoothTest(absltest.TestCase):
mujoco.mju_sparse2dense( mujoco.mju_sparse2dense(
ten_J, ten_J,
d.ten_J, d.ten_J,
d.ten_J_rownnz, m.ten_J_rownnz,
d.ten_J_rowadr, m.ten_J_rowadr,
d.ten_J_colind, m.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')
@@ -407,9 +407,9 @@ class TendonTest(parameterized.TestCase):
mujoco.mju_sparse2dense( mujoco.mju_sparse2dense(
ten_J, ten_J,
d.ten_J, d.ten_J,
d.ten_J_rownnz, m.ten_J_rownnz,
d.ten_J_rowadr, m.ten_J_rowadr,
d.ten_J_colind, m.ten_J_colind,
) )
_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')
+9 -6
View File
@@ -952,9 +952,12 @@ def put_data(
d.flexedge_J = wp.array(np.tile(mjd.flexedge_J.reshape(-1), (nworld, 1)).reshape((nworld, 1, -1)), dtype=float) d.flexedge_J = wp.array(np.tile(mjd.flexedge_J.reshape(-1), (nworld, 1)).reshape((nworld, 1, -1)), dtype=float)
ten_J = np.zeros((mjm.ntendon, mjm.nv)) if mjm.ntendon:
mujoco.mju_sparse2dense(ten_J, mjd.ten_J.reshape(-1), mjd.ten_J_rownnz, mjd.ten_J_rowadr, mjd.ten_J_colind.reshape(-1)) ten_J = np.zeros((mjm.ntendon, mjm.nv))
d.ten_J = wp.array(np.full((nworld, mjm.ntendon, mjm.nv), ten_J), dtype=float) mujoco.mju_sparse2dense(ten_J, mjd.ten_J.reshape(-1), mjm.ten_J_rownnz, mjm.ten_J_rowadr, mjm.ten_J_colind.reshape(-1))
d.ten_J = wp.array(np.full((nworld, mjm.ntendon, mjm.nv), ten_J), dtype=float)
else:
d.ten_J = wp.array(np.full((nworld, mjm.ntendon, mjm.nv), 0.0), dtype=float)
# TODO(taylorhowell): sparse actuator_moment # TODO(taylorhowell): sparse actuator_moment
actuator_moment = np.zeros((mjm.nu, mjm.nv)) actuator_moment = np.zeros((mjm.nu, mjm.nv))
@@ -1165,9 +1168,9 @@ def get_data_into(
mujoco.mju_dense2sparse( mujoco.mju_dense2sparse(
result.ten_J, result.ten_J,
ten_J, ten_J,
result.ten_J_rownnz, mjm.ten_J_rownnz,
result.ten_J_rowadr, mjm.ten_J_rowadr,
result.ten_J_colind, mjm.ten_J_colind,
) )
else: else:
result.ten_J[:] = d.ten_J.numpy()[world_id] result.ten_J[:] = d.ten_J.numpy()[world_id]
+3 -3
View File
@@ -153,9 +153,9 @@ class ForwardTest(parameterized.TestCase):
mujoco.mju_sparse2dense( mujoco.mju_sparse2dense(
ten_J, ten_J,
d.ten_J, d.ten_J,
d.ten_J_rownnz, m.ten_J_rownnz,
d.ten_J_rowadr, m.ten_J_rowadr,
d.ten_J_colind, m.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')
+4 -5
View File
@@ -259,7 +259,10 @@
X( mjtNum, tendon, _length0, ntendon, 1 ) \ X( mjtNum, tendon, _length0, ntendon, 1 ) \
X( mjtNum, tendon, _invweight0, ntendon, 1 ) \ X( mjtNum, tendon, _invweight0, ntendon, 1 ) \
X( mjtNum, tendon, _user, ntendon, MJ_M(nuser_tendon) ) \ X( mjtNum, tendon, _user, ntendon, MJ_M(nuser_tendon) ) \
X( float, tendon, _rgba, ntendon, 4 ) X( float, tendon, _rgba, ntendon, 4 ) \
X( int, ten_, J_rownnz, ntendon, 1 ) \
X( int, ten_, J_rowadr, ntendon, 1 ) \
X( int, ten_, J_colind, ntendon, MJ_M(nv) )
#define MJMODEL_TEXTURE \ #define MJMODEL_TEXTURE \
X( int, tex_, type, ntex, 1 ) \ X( int, tex_, type, ntex, 1 ) \
@@ -402,11 +405,7 @@
#define MJDATA_TENDON \ #define MJDATA_TENDON \
X( int, ten_, wrapadr , ntendon, 1 ) \ X( int, ten_, wrapadr , ntendon, 1 ) \
X( int, ten_, wrapnum , ntendon, 1 ) \ X( int, ten_, wrapnum , ntendon, 1 ) \
X( int, ten_, J_rownnz, ntendon, 1 ) \
X( int, ten_, J_rowadr, ntendon, 1 ) \
X( int, ten_, J_colind, ntendon, MJ_M(nv) ) \
X( mjtNum, ten_, length , ntendon, 1 ) \ X( mjtNum, ten_, length , ntendon, 1 ) \
X( mjtNum, ten_, J , ntendon, MJ_M(nv) ) \
X( mjtNum, ten_, velocity, ntendon, 1 ) X( mjtNum, ten_, velocity, ntendon, 1 )
#define MJDATA_VIEW_GROUPS \ #define MJDATA_VIEW_GROUPS \
+29 -29
View File
@@ -1122,6 +1122,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='mjtSize'), type=ValueType(name='mjtSize'),
doc='number of tendons', doc='number of tendons',
), ),
StructFieldDecl(
name='nJten',
type=ValueType(name='mjtSize'),
doc='number of non-zeros in sparse ten_J matrix',
),
StructFieldDecl( StructFieldDecl(
name='nwrap', name='nwrap',
type=ValueType(name='mjtSize'), type=ValueType(name='mjtSize'),
@@ -1242,11 +1247,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='mjtSize'), type=ValueType(name='mjtSize'),
doc='number of non-zeros in sparse actuator_moment matrix', doc='number of non-zeros in sparse actuator_moment matrix',
), ),
StructFieldDecl(
name='nJten',
type=ValueType(name='mjtSize'),
doc='number of non-zeros in sparse ten_J matrix',
),
StructFieldDecl( StructFieldDecl(
name='ngravcomp', name='ngravcomp',
type=ValueType(name='mjtSize'), type=ValueType(name='mjtSize'),
@@ -3927,6 +3927,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc="first two trees along tendon's path", doc="first two trees along tendon's path",
array_extent=('ntendon', 2), array_extent=('ntendon', 2),
), ),
StructFieldDecl(
name='ten_J_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='number of non-zeros in Jacobian row',
array_extent=('ntendon',),
),
StructFieldDecl(
name='ten_J_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='row start address in colind array',
array_extent=('ntendon',),
),
StructFieldDecl(
name='ten_J_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='column indices in sparse Jacobian',
array_extent=('nJten',),
),
StructFieldDecl( StructFieldDecl(
name='tendon_limited', name='tendon_limited',
type=PointerType( type=PointerType(
@@ -5812,30 +5836,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='number of wrap points in path', doc='number of wrap points in path',
array_extent=('ntendon',), array_extent=('ntendon',),
), ),
StructFieldDecl(
name='ten_J_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='number of non-zeros in Jacobian row',
array_extent=('ntendon',),
),
StructFieldDecl(
name='ten_J_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='row start address in colind array',
array_extent=('ntendon',),
),
StructFieldDecl(
name='ten_J_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='column indices in sparse Jacobian',
array_extent=('nJten',),
),
StructFieldDecl( StructFieldDecl(
name='ten_J', name='ten_J',
type=PointerType( type=PointerType(
+23 -23
View File
@@ -568,16 +568,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// copy Jacobian: sparse or dense // copy Jacobian: sparse or dense
if (issparse) { if (issparse) {
if (j == 0) { if (j == 0) {
NV = d->ten_J_rownnz[id[j]]; NV = m->ten_J_rownnz[id[j]];
mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); mju_copyInt(chain, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV);
mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV); mju_copy(jac[j], d->ten_J+m->ten_J_rowadr[id[j]], NV);
} else { } else {
NV2 = d->ten_J_rownnz[id[j]]; NV2 = m->ten_J_rownnz[id[j]];
mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); mju_copyInt(chain2, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV2);
mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2); mju_copy(jac[j], d->ten_J+m->ten_J_rowadr[id[j]], NV2);
} }
} else { } else {
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); mju_sparse2dense(jac[j], d->ten_J, 1, nv, m->ten_J_rownnz+id[j], m->ten_J_rowadr+id[j], m->ten_J_colind);
} }
} }
} }
@@ -737,13 +737,13 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
int efcadr = d->nefc; int efcadr = d->nefc;
// add constraint // add constraint
if (issparse) { if (issparse) {
mj_addConstraint(m, d, d->ten_J + d->ten_J_rowadr[i], mj_addConstraint(m, d, d->ten_J + m->ten_J_rowadr[i],
0, 0, m->tendon_frictionloss[i], 0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i, 1, mjCNSTR_FRICTION_TENDON, i,
d->ten_J_rownnz[i], m->ten_J_rownnz[i],
d->ten_J_colind+d->ten_J_rowadr[i]); m->ten_J_colind+m->ten_J_rowadr[i]);
} else { } else {
mju_sparse2dense(jac, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind); mju_sparse2dense(jac, d->ten_J, 1, nv, m->ten_J_rownnz+i, m->ten_J_rowadr+i, m->ten_J_colind);
mj_addConstraint(m, d, jac, 0, 0, m->tendon_frictionloss[i], mj_addConstraint(m, d, jac, 0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i, 0, NULL); 1, mjCNSTR_FRICTION_TENDON, i, 0, NULL);
} }
@@ -885,13 +885,13 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// prepare Jacobian // prepare Jacobian
int efcadr = d->nefc; 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+m->ten_J_rowadr[i], -side, m->ten_J_rownnz[i]);
mj_addConstraint(m, d, jac, &dist, &margin, 0, mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i, 1, mjCNSTR_LIMIT_TENDON, i,
d->ten_J_rownnz[i], m->ten_J_rownnz[i],
d->ten_J_colind+d->ten_J_rowadr[i]); m->ten_J_colind+m->ten_J_rowadr[i]);
} else { } else {
mju_sparse2dense(jac, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind); mju_sparse2dense(jac, d->ten_J, 1, nv, m->ten_J_rownnz+i, m->ten_J_rowadr+i, m->ten_J_colind);
mju_scl(jac, jac, -side, nv); mju_scl(jac, jac, -side, nv);
mj_addConstraint(m, d, jac, &dist, &margin, 0, mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i, 0, NULL); 1, mjCNSTR_LIMIT_TENDON, i, 0, NULL);
@@ -1738,11 +1738,11 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
} }
} else { } else {
if (!j) { if (!j) {
NV = d->ten_J_rownnz[id[j]]; NV = m->ten_J_rownnz[id[j]];
mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); mju_copyInt(chain, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV);
} else { } else {
NV2 = d->ten_J_rownnz[id[j]]; NV2 = m->ten_J_rownnz[id[j]];
mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); mju_copyInt(chain2, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV2);
} }
} }
} }
@@ -1840,8 +1840,8 @@ static int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
for (int i=0; i < ntendon; i++) { for (int i=0; i < ntendon; i++) {
if (m->tendon_frictionloss[i] > 0) { if (m->tendon_frictionloss[i] > 0) {
nf += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]); nf += mj_addConstraintCount(m, 1, m->ten_J_rownnz[i]);
if (nnz) *nnz += d->ten_J_rownnz[i]; if (nnz) *nnz += m->ten_J_rownnz[i];
} }
} }
@@ -1908,8 +1908,8 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
for (int i=0; i < ntendon; i++) { for (int i=0; i < ntendon; i++) {
int count = tendonLimit(m, d->ten_length, i); int count = tendonLimit(m, d->ten_length, i);
for (int j = 0; j < count; j++) { for (int j = 0; j < count; j++) {
nl += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]); nl += mj_addConstraintCount(m, 1, m->ten_J_rownnz[i]);
if (nnz) *nnz += d->ten_J_rownnz[i]; if (nnz) *nnz += m->ten_J_rownnz[i];
} }
} }
+20 -26
View File
@@ -905,7 +905,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 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; const int *rownnz = m->ten_J_rownnz, *rowadr = m->ten_J_rowadr, *colind = m->ten_J_colind;
mjtNum *L = d->ten_length, *J = d->ten_J; mjtNum *L = d->ten_length, *J = d->ten_J;
if (!nten) { if (!nten) {
@@ -913,22 +913,20 @@ void mj_tendon(const mjModel* m, mjData* d) {
} }
// allocate stack arrays // allocate stack arrays
int *chain, *buf_ind; int *chain;
mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf; mjtNum *jac1, *jac2, *jacdif, *tmp;
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);
chain = mjSTACKALLOC(d, nv, int); chain = mjSTACKALLOC(d, nv, int);
buf_ind = mjSTACKALLOC(d, nv, int);
sparse_buf = mjSTACKALLOC(d, nv, mjtNum);
// clear results // clear results
mju_zero(L, nten); mju_zero(L, nten);
// clear Jacobian // clear Jacobian
mju_zeroInt(rownnz, nten); mju_zero(J, m->nJten);
// 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;
@@ -947,9 +945,6 @@ void mj_tendon(const mjModel* m, mjData* d) {
d->ten_wrapnum[i] = 0; d->ten_wrapnum[i] = 0;
int tendon_num = m->tendon_num[i]; int tendon_num = m->tendon_num[i];
// sparse Jacobian row init
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) {
// process all defined joints // process all defined joints
@@ -960,11 +955,10 @@ void mj_tendon(const mjModel* m, mjData* d) {
// add to length // add to length
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 mjtNum coef = 1;
rownnz[i] = mju_combineSparse(J+rowadr[i], &m->wrap_prm[adr+j], 1, 1, int dofadr = m->jnt_dofadr[k];
rownnz[i], 1, mju_combineSparseInc(J + rowadr[i], &coef, m->nv, 1, m->wrap_prm[adr+j],
colind+rowadr[i], &m->jnt_dofadr[k], rownnz[i], 1, colind + rowadr[i], &dofadr);
sparse_buf, buf_ind);
} }
continue; continue;
@@ -1059,9 +1053,9 @@ void mj_tendon(const mjModel* m, mjData* d) {
mju_mulMatTVec(tmp, jacdif, dif, 3, NV); mju_mulMatTVec(tmp, jacdif, dif, 3, NV);
// add to existing // add to existing
rownnz[i] = mju_combineSparse(J+rowadr[i], tmp, 1, 1/divisor, mju_combineSparseInc(J+rowadr[i], tmp, nv, 1, 1/divisor,
rownnz[i], NV, colind+rowadr[i], rownnz[i], NV, colind+rowadr[i],
chain, sparse_buf, buf_ind); chain);
} }
} }
@@ -1419,10 +1413,10 @@ void mj_transmission(const mjModel* m, mjData* d) {
// moment // moment
{ {
int ten_J_rownnz = d->ten_J_rownnz[id]; int ten_J_rownnz = m->ten_J_rownnz[id];
int ten_J_rowadr = d->ten_J_rowadr[id]; int ten_J_rowadr = m->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, m->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);
} }
@@ -1717,9 +1711,9 @@ void mj_tendonArmature(const mjModel* m, mjData* d) {
} }
// get sparse info for tendon k // get sparse info for tendon k
int J_rowadr = d->ten_J_rowadr[k]; int J_rowadr = m->ten_J_rowadr[k];
int J_rownnz = d->ten_J_rownnz[k]; int J_rownnz = m->ten_J_rownnz[k];
const int* J_colind = d->ten_J_colind + J_rowadr; const int* J_colind = m->ten_J_colind + J_rowadr;
mjtNum* ten_J = d->ten_J + J_rowadr; mjtNum* ten_J = d->ten_J + J_rowadr;
// M += armature * ten_J' * ten_J // M += armature * ten_J' * ten_J
@@ -2649,9 +2643,9 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) {
if (coef) { if (coef) {
// sparse // sparse
int nnz = d->ten_J_rownnz[i]; int nnz = m->ten_J_rownnz[i];
int adr = d->ten_J_rowadr[i]; int adr = m->ten_J_rowadr[i];
const int* colind = d->ten_J_colind + adr; const int* colind = m->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];
+1 -1
View File
@@ -1776,7 +1776,7 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
} }
// add sparse // add sparse
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, m->ten_J_rownnz, m->ten_J_rowadr, m->ten_J_colind);
} }
} }
+1 -1
View File
@@ -232,7 +232,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// tendon velocity: always sparse // tendon velocity: always sparse
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); m->ten_J_rownnz, m->ten_J_rowadr, m->ten_J_colind, NULL);
// actuator velocity: always sparse // actuator velocity: always sparse
if (!mjDISABLED(mjDSBL_ACTUATION)) { if (!mjDISABLED(mjDSBL_ACTUATION)) {
+14 -12
View File
@@ -211,10 +211,10 @@ void mj_makeModel(mjModel** dest,
mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin,
mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone,
mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata,
mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nwrap, mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten,
mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, mjtSize ntextdata, mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext,
mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, mjtSize nplugin, mjtSize ntextdata, mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap,
mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom, mjtSize nplugin, mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom,
mjtSize nuser_site, mjtSize nuser_cam, mjtSize nuser_tendon, mjtSize nuser_actuator, mjtSize nuser_site, mjtSize nuser_cam, mjtSize nuser_tendon, mjtSize nuser_actuator,
mjtSize nuser_sensor, mjtSize nnames, mjtSize npaths) { mjtSize nuser_sensor, mjtSize nnames, mjtSize npaths) {
intptr_t offset = 0; intptr_t offset = 0;
@@ -224,7 +224,7 @@ void mj_makeModel(mjModel** dest,
// CHECK SIZE PARAMETERS // CHECK SIZE PARAMETERS
{ {
// dummy variables for MJMODEL_SIZES set after mjModel construction // dummy variables for MJMODEL_SIZES set after mjModel construction
int nnames_map = 0, nJmom = 0, nJten = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0; int nnames_map = 0, nJmom = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0;
int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0; int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0;
// sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata // sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata
@@ -243,7 +243,7 @@ void mj_makeModel(mjModel** dest,
#undef X #undef X
// suppress unused variable warnings // suppress unused variable warnings
(void)nnames_map; (void)nJmom; (void)nJten; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax; (void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax;
(void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena; (void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena;
(void)nbuffer; (void)nbuffer;
} }
@@ -323,6 +323,7 @@ void mj_makeModel(mjModel** dest,
m->nexclude = nexclude; m->nexclude = nexclude;
m->neq = neq; m->neq = neq;
m->ntendon = ntendon; m->ntendon = ntendon;
m->nJten = nJten;
m->nwrap = nwrap; m->nwrap = nwrap;
m->nsensor = nsensor; m->nsensor = nsensor;
m->nnumeric = nnumeric; m->nnumeric = nnumeric;
@@ -410,11 +411,11 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata, src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude, src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric, src->neq, src->ntendon, src->nJten, src->nwrap, src->nsensor,
src->nnumericdata, src->ntext, src->ntextdata, src->ntuple, src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata,
src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr, src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin,
src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site, src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom,
src->nuser_cam, src->nuser_tendon, src->nuser_actuator, src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths); src->nuser_sensor, src->nnames, src->npaths);
} }
if (!dest) { if (!dest) {
@@ -597,7 +598,8 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
sizes[49], sizes[50], sizes[51], sizes[52], sizes[53], sizes[54], sizes[55], sizes[49], sizes[50], sizes[51], sizes[52], sizes[53], sizes[54], sizes[55],
sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62], sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62],
sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69], sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69],
sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76]); sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76],
sizes[77]);
// mj_makeModel may fail if the input buffer has invalid sizes // mj_makeModel may fail if the input buffer has invalid sizes
if (!m) { if (!m) {
+4 -4
View File
@@ -58,10 +58,10 @@ void mj_makeModel(mjModel** dest,
mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin,
mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone,
mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata,
mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nwrap, mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten,
mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, mjtSize ntextdata, mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext,
mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, mjtSize nplugin, mjtSize ntextdata, mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap,
mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom, mjtSize nplugin, mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom,
mjtSize nuser_site, mjtSize nuser_cam, mjtSize nuser_tendon, mjtSize nuser_actuator, mjtSize nuser_site, mjtSize nuser_cam, mjtSize nuser_tendon, mjtSize nuser_actuator,
mjtSize nuser_sensor, mjtSize nnames, mjtSize npaths); mjtSize nuser_sensor, mjtSize nnames, mjtSize npaths);
+3 -3
View File
@@ -473,9 +473,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// transform to joint torque, add to qfrc_{spring, damper} // transform to joint torque, add to qfrc_{spring, damper}
if (frc_spring || frc_damper) { if (frc_spring || frc_damper) {
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; int end = m->ten_J_rowadr[i] + m->ten_J_rownnz[i];
for (int j=d->ten_J_rowadr[i]; j < end; j++) { for (int j=m->ten_J_rowadr[i]; j < end; j++) {
int k = d->ten_J_colind[j]; int k = m->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;
+6 -6
View File
@@ -1441,12 +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);
mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL, mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, m->ten_J_rowadr, NULL,
d->ten_J_rownnz, NULL, d->ten_J_colind, fp); m->ten_J_rownnz, NULL, m->ten_J_colind, fp);
printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, m->ten_J_rownnz, fp);
printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, m->ten_J_rowadr, fp);
printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, printSparse("TEN_J", d->ten_J, m->ntendon, m->ten_J_rownnz,
d->ten_J_rowadr, d->ten_J_colind, fp, float_format); m->ten_J_rowadr, m->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++) {
+89 -1
View File
@@ -282,6 +282,93 @@ static void setFixed(mjModel* m, mjData* d) {
mj_freeStack(d); mj_freeStack(d);
} }
// compute tendon Jacobian sparsity
static void makeTendonSparse(mjModel* m) {
int ntendon = m->ntendon;
int* rownnz = m->ten_J_rownnz;
int* rowadr = m->ten_J_rowadr;
int* colind = m->ten_J_colind;
if (!ntendon) {
return;
}
// clear
mju_zeroInt(rownnz, ntendon);
mju_zeroInt(rowadr, ntendon);
// compute rownnz, rowadr, and colind for each tendon
for (int i = 0; i < ntendon; i++) {
rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0);
int adr = m->tendon_adr[i];
int num = m->tendon_num[i];
// joint tendon: each wrap object is a joint, colind is its dofadr
if (m->wrap_type[adr] == mjWRAP_JOINT) {
for (int j = 0; j < num; j++) {
colind[rowadr[i] + j] = m->jnt_dofadr[m->wrap_objid[adr + j]];
}
rownnz[i] = num;
} else {
// spatial tendon: collect used dofs from wrap object bodies
int nnz = 0;
for (int j = 0; j < num; j++) {
int type = m->wrap_type[adr + j];
// get body id from site or geom wrap object
int bodyid = -1;
if (type == mjWRAP_SITE) {
bodyid = m->site_bodyid[m->wrap_objid[adr + j]];
} else if (type == mjWRAP_SPHERE || type == mjWRAP_CYLINDER) {
bodyid = m->geom_bodyid[m->wrap_objid[adr + j]];
}
// walk up the body tree, collecting used dofs
if (bodyid > 0) {
int bid = bodyid;
while (bid > 0) {
int bdofadr = m->body_dofadr[bid];
int bdofnum = m->body_dofnum[bid];
for (int k = 0; k < bdofnum; k++) {
int dof = bdofadr + k;
// check if dof already in colind
int found = 0;
for (int l = 0; l < nnz; l++) {
if (colind[rowadr[i] + l] == dof) {
found = 1;
break;
}
}
// append new dof
if (!found) {
colind[rowadr[i] + nnz] = dof;
nnz++;
}
}
bid = m->body_parentid[bid];
}
}
}
rownnz[i] = nnz;
}
// sort colind for this tendon
int nnz = rownnz[i];
for (int j = 0; j < nnz - 1; j++) {
for (int k = j + 1; k < nnz; k++) {
// swap out-of-order entries
if (colind[rowadr[i] + k] < colind[rowadr[i] + j]) {
int tmp = colind[rowadr[i] + j];
colind[rowadr[i] + j] = colind[rowadr[i] + k];
colind[rowadr[i] + k] = tmp;
}
}
}
}
}
// compute flex sparsity: flexedge_J_{rowadr,rownnz,colind} and flexvert_J_{rowadr,rownnz} // compute flex sparsity: flexedge_J_{rowadr,rownnz,colind} and flexvert_J_{rowadr,rownnz}
static void makeFlexSparse(mjModel* m, mjData* d) { static void makeFlexSparse(mjModel* m, mjData* d) {
int nv = m->nv; int nv = m->nv;
@@ -552,6 +639,7 @@ static void mj_alignFlex(mjModel* m, mjData* d) {
// set quantities that depend on qpos0 // set quantities that depend on qpos0
static void set0(mjModel* m, mjData* d) { static void set0(mjModel* m, mjData* d) {
makeTendonSparse(m);
makeFlexSparse(m, d); makeFlexSparse(m, d);
mj_alignFlex(m, d); mj_alignFlex(m, d);
int nv = m->nv; int nv = m->nv;
@@ -756,7 +844,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++) {
mju_sparse2dense(tmp, d->ten_J, 1, nv, d->ten_J_rownnz+i, d->ten_J_rowadr+i, d->ten_J_colind); mju_sparse2dense(tmp, d->ten_J, 1, nv, m->ten_J_rownnz+i, m->ten_J_rowadr+i, m->ten_J_colind);
// solve into tmp+nv // solve into tmp+nv
mj_solveM(m, d, tmp+nv, tmp, 1); mj_solveM(m, d, tmp+nv, tmp, 1);
+33 -3
View File
@@ -907,6 +907,7 @@ void mjCModel::ComputeSparseSizes() {
// no dofs, quick return // no dofs, quick return
if (nv == 0) { if (nv == 0) {
nM = nD = nB = nC = 0; nM = nD = nB = nC = 0;
nJten = 0;
return; return;
} }
@@ -1084,6 +1085,37 @@ void mjCModel::ComputeSparseSizes() {
} }
} }
nC = nOD + nv; nC = nOD + nv;
nJten = 0;
if (nv > 0) {
std::vector<bool> dof_bitmap(nv, false);
for (const auto* tendon : tendons_) {
if (!tendon->path.empty() &&
tendon->path[0]->Type() == mjWRAP_JOINT) {
nJten += tendon->path.size();
continue;
}
std::fill(dof_bitmap.begin(), dof_bitmap.end(), false);
for (const auto* wrap : tendon->path) {
int bodyid = GetBodyIdFromWrap(wrap);
if (bodyid > 0) {
mjCBody* b = bodies_[bodyid];
while (b && b->id > 0) {
for (const auto* jnt : b->joints) {
for (int k = 0; k < jnt->nv(); k++) {
dof_bitmap[jnt->dofadr_ + k] = true;
}
}
b = b->GetParent();
}
}
}
for (int j = 0; j < nv; j++) {
nJten += dof_bitmap[j];
}
}
}
} }
@@ -5073,7 +5105,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmeshpoly, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmeshpoly,
nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, nskinface, nskinbone, nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, nskinface, nskinbone,
nskinbonevert, nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude, nskinbonevert, nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
neq, ntendon, nwrap, nsensor, nnumeric, nnumericdata, ntext, ntextdata, neq, ntendon, nJten, nwrap, nsensor, nnumeric, nnumericdata, ntext, ntextdata,
ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr, ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr,
nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam, nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam,
nuser_tendon, nuser_actuator, nuser_sensor, nnames, npaths); nuser_tendon, nuser_actuator, nuser_sensor, nnames, npaths);
@@ -5105,8 +5137,6 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// compute non-zeros in actuator_moment // compute non-zeros in actuator_moment
m->nJmom = nJmom = CountNJmom(m); m->nJmom = nJmom = CountNJmom(m);
// compute non-zeros in ten_J
m->nJten = nJten = CountNJten(m);
// scale mass // scale mass
if (compiler.settotalmass > 0) { if (compiler.settotalmass > 0) {
+1 -1
View File
@@ -73,6 +73,7 @@ class mjCModel_ : public mjsElement {
mjtSize nexclude; // number of excluded body pairs mjtSize nexclude; // number of excluded body pairs
mjtSize neq; // number of equality constraints mjtSize neq; // number of equality constraints
mjtSize ntendon; // number of tendons mjtSize ntendon; // number of tendons
mjtSize nJten; // number of non-zeros in sparse ten_J matrix
mjtSize nsensor; // number of sensors mjtSize nsensor; // number of sensors
mjtSize nnumeric; // number of numeric fields mjtSize nnumeric; // number of numeric fields
mjtSize ntext; // number of text fields mjtSize ntext; // number of text fields
@@ -130,7 +131,6 @@ class mjCModel_ : public mjsElement {
mjtSize nC; // number of non-zeros in reduced sparse dof-dof matrix mjtSize nC; // number of non-zeros in reduced sparse dof-dof matrix
mjtSize nD; // number of non-zeros in sparse dof-dof matrix mjtSize nD; // number of non-zeros in sparse dof-dof matrix
mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix
mjtSize nJten; // number of non-zeros in sparse ten_J matrix
// statistics, as computed by mj_setConst // statistics, as computed by mj_setConst
double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst
+4 -8
View File
@@ -154,12 +154,8 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
mj_fwdPosition(model, data); mj_fwdPosition(model, data);
int rowadr = data->ten_J_rowadr[0]; mjtNum* J = data->ten_J;
int* colind = data->ten_J_colind + rowadr;
mjtNum* J = data->ten_J + rowadr;
EXPECT_THAT(vector<mjtNum>(J, J + 3), ElementsAre(1, 2, 3)); EXPECT_THAT(vector<mjtNum>(J, J + 3), ElementsAre(1, 2, 3));
EXPECT_THAT(vector<int>(colind, colind + 3), ElementsAre(0, 1, 2));
mj_deleteData(data); mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
@@ -253,11 +249,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
int rowadr = d->ten_J_rowadr[j]; int rowadr = m->ten_J_rowadr[j];
int* rownnz = d->ten_J_rownnz + j; int* rownnz = m->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, m->ten_J_colind + rowadr);
// 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);
+42
View File
@@ -199,6 +199,48 @@ TEST_F(UserModelTest, ActuatorSparsity) {
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserModelTest, FixedTendonSparsity) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size=".1"/>
<joint name="0"/>
</body>
<body pos="1 0 0">
<geom size=".1"/>
<joint name="1"/>
</body>
<body pos="2 0 0">
<geom size=".1"/>
<joint name="2"/>
</body>
</worldbody>
<tendon>
<fixed>
<joint coef="3" joint="2"/>
<joint coef="2" joint="1"/>
<joint coef="1" joint="0"/>
</fixed>
</tendon>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
EXPECT_EQ(m->nJten, 3);
EXPECT_EQ(m->ten_J_rownnz[0], 3);
EXPECT_EQ(m->ten_J_rowadr[0], 0);
EXPECT_EQ(m->wrap_type[m->tendon_adr[0]], mjWRAP_JOINT);
int rowadr = m->ten_J_rowadr[0];
int* colind = m->ten_J_colind + rowadr;
EXPECT_THAT(std::vector<int>(colind, colind + 3), ElementsAre(0, 1, 2));
mj_deleteModel(m);
}
TEST_F(UserModelTest, NestedZeroMassBodiesOK) { TEST_F(UserModelTest, NestedZeroMassBodiesOK) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
+4 -4
View File
@@ -4994,9 +4994,6 @@ public unsafe struct mjData_ {
public double* bvh_aabb_dyn; public double* bvh_aabb_dyn;
public int* ten_wrapadr; public int* ten_wrapadr;
public int* ten_wrapnum; public int* ten_wrapnum;
public int* ten_J_rownnz;
public int* ten_J_rowadr;
public int* ten_J_colind;
public double* ten_J; public double* ten_J;
public double* ten_length; public double* ten_length;
public int* wrap_obj; public int* wrap_obj;
@@ -5347,6 +5344,7 @@ public unsafe struct mjModel_ {
public Int64 nexclude; public Int64 nexclude;
public Int64 neq; public Int64 neq;
public Int64 ntendon; public Int64 ntendon;
public Int64 nJten;
public Int64 nwrap; public Int64 nwrap;
public Int64 nsensor; public Int64 nsensor;
public Int64 nnumeric; public Int64 nnumeric;
@@ -5371,7 +5369,6 @@ public unsafe struct mjModel_ {
public Int64 npaths; public Int64 npaths;
public Int64 nnames_map; public Int64 nnames_map;
public Int64 nJmom; public Int64 nJmom;
public Int64 nJten;
public Int64 ngravcomp; public Int64 ngravcomp;
public Int64 nemax; public Int64 nemax;
public Int64 njmax; public Int64 njmax;
@@ -5712,6 +5709,9 @@ public unsafe struct mjModel_ {
public int* tendon_group; public int* tendon_group;
public int* tendon_treenum; public int* tendon_treenum;
public int* tendon_treeid; public int* tendon_treeid;
public int* ten_J_rownnz;
public int* ten_J_rowadr;
public int* ten_J_colind;
public byte* tendon_limited; public byte* tendon_limited;
public byte* tendon_actfrclimited; public byte* tendon_actfrclimited;
public double* tendon_width; public double* tendon_width;
+18 -18
View File
@@ -3934,6 +3934,12 @@ struct MjModel {
void set_ntendon(int value) { void set_ntendon(int value) {
ptr_->ntendon = static_cast<mjtSize>(value); ptr_->ntendon = static_cast<mjtSize>(value);
} }
int nJten() const {
return static_cast<int>(ptr_->nJten);
}
void set_nJten(int value) {
ptr_->nJten = static_cast<mjtSize>(value);
}
int nwrap() const { int nwrap() const {
return static_cast<int>(ptr_->nwrap); return static_cast<int>(ptr_->nwrap);
} }
@@ -4078,12 +4084,6 @@ struct MjModel {
void set_nJmom(int value) { void set_nJmom(int value) {
ptr_->nJmom = static_cast<mjtSize>(value); ptr_->nJmom = static_cast<mjtSize>(value);
} }
int nJten() const {
return static_cast<int>(ptr_->nJten);
}
void set_nJten(int value) {
ptr_->nJten = static_cast<mjtSize>(value);
}
int ngravcomp() const { int ngravcomp() const {
return static_cast<int>(ptr_->ngravcomp); return static_cast<int>(ptr_->ngravcomp);
} }
@@ -5125,6 +5125,15 @@ struct MjModel {
emscripten::val tendon_treeid() const { emscripten::val tendon_treeid() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon * 2, ptr_->tendon_treeid)); return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon * 2, ptr_->tendon_treeid));
} }
emscripten::val ten_J_rownnz() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->ten_J_rownnz));
}
emscripten::val ten_J_rowadr() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->ten_J_rowadr));
}
emscripten::val ten_J_colind() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nJten, ptr_->ten_J_colind));
}
emscripten::val tendon_limited() const { emscripten::val tendon_limited() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_limited)); return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_limited));
} }
@@ -6649,15 +6658,6 @@ struct MjData {
emscripten::val ten_wrapnum() const { emscripten::val ten_wrapnum() const {
return emscripten::val(emscripten::typed_memory_view(model->ntendon, ptr_->ten_wrapnum)); return emscripten::val(emscripten::typed_memory_view(model->ntendon, ptr_->ten_wrapnum));
} }
emscripten::val ten_J_rownnz() const {
return emscripten::val(emscripten::typed_memory_view(model->ntendon, ptr_->ten_J_rownnz));
}
emscripten::val ten_J_rowadr() const {
return emscripten::val(emscripten::typed_memory_view(model->ntendon, ptr_->ten_J_rowadr));
}
emscripten::val ten_J_colind() const {
return emscripten::val(emscripten::typed_memory_view(model->nJten, ptr_->ten_J_colind));
}
emscripten::val ten_J() const { emscripten::val ten_J() const {
return emscripten::val(emscripten::typed_memory_view(model->nJten, ptr_->ten_J)); return emscripten::val(emscripten::typed_memory_view(model->nJten, ptr_->ten_J));
} }
@@ -11550,9 +11550,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("subtree_com", &MjData::subtree_com) .property("subtree_com", &MjData::subtree_com)
.property("subtree_linvel", &MjData::subtree_linvel) .property("subtree_linvel", &MjData::subtree_linvel)
.property("ten_J", &MjData::ten_J) .property("ten_J", &MjData::ten_J)
.property("ten_J_colind", &MjData::ten_J_colind)
.property("ten_J_rowadr", &MjData::ten_J_rowadr)
.property("ten_J_rownnz", &MjData::ten_J_rownnz)
.property("ten_length", &MjData::ten_length) .property("ten_length", &MjData::ten_length)
.property("ten_velocity", &MjData::ten_velocity) .property("ten_velocity", &MjData::ten_velocity)
.property("ten_wrapadr", &MjData::ten_wrapadr) .property("ten_wrapadr", &MjData::ten_wrapadr)
@@ -12100,6 +12097,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("skin_vertadr", &MjModel::skin_vertadr) .property("skin_vertadr", &MjModel::skin_vertadr)
.property("skin_vertnum", &MjModel::skin_vertnum) .property("skin_vertnum", &MjModel::skin_vertnum)
.property("stat", &MjModel::stat, reference()) .property("stat", &MjModel::stat, reference())
.property("ten_J_colind", &MjModel::ten_J_colind)
.property("ten_J_rowadr", &MjModel::ten_J_rowadr)
.property("ten_J_rownnz", &MjModel::ten_J_rownnz)
.property("tendon_actfrclimited", &MjModel::tendon_actfrclimited) .property("tendon_actfrclimited", &MjModel::tendon_actfrclimited)
.property("tendon_actfrcrange", &MjModel::tendon_actfrcrange) .property("tendon_actfrcrange", &MjModel::tendon_actfrcrange)
.property("tendon_adr", &MjModel::tendon_adr) .property("tendon_adr", &MjModel::tendon_adr)