From 52ed96bc3ac8464f3d5d51003141ec6c6cd3cd09 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 25 Feb 2026 04:31:37 -0800 Subject: [PATCH] Move tendon sparsity fields from mjData to mjModel PiperOrigin-RevId: 875087590 Change-Id: I1a5489d2d2011795ee38b09d547e68acd72e3cc7 --- doc/includes/references.h | 8 +- include/mujoco/mjdata.h | 3 - include/mujoco/mjmodel.h | 5 +- include/mujoco/mjxmacro.h | 8 +- mjx/mujoco/mjx/_src/io.py | 12 +-- mjx/mujoco/mjx/_src/smooth_test.py | 12 +-- .../mjx/third_party/mujoco_warp/_src/io.py | 15 ++-- mjx/mujoco/mjx/warp/forward_test.py | 6 +- python/mujoco/indexer_xmacro.h | 9 +- python/mujoco/introspect/structs.py | 58 ++++++------ src/engine/engine_core_constraint.c | 46 +++++----- src/engine/engine_core_smooth.c | 46 +++++----- src/engine/engine_derivative.c | 2 +- src/engine/engine_forward.c | 2 +- src/engine/engine_io.c | 26 +++--- src/engine/engine_io.h | 8 +- src/engine/engine_passive.c | 6 +- src/engine/engine_print.c | 12 +-- src/engine/engine_setconst.c | 90 ++++++++++++++++++- src/user/user_model.cc | 36 +++++++- src/user/user_model.h | 2 +- test/engine/engine_core_smooth_test.cc | 12 +-- test/user/user_model_test.cc | 42 +++++++++ unity/Runtime/Bindings/MjBindings.cs | 8 +- wasm/codegen/generated/bindings.cc | 36 ++++---- 25 files changed, 332 insertions(+), 178 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 6a30e704..9bc868ab 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -289,9 +289,6 @@ struct mjData_ { // computed by mj_fwdPosition/mj_tendon 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_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_length; // tendon lengths (ntendon x 1) 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 neq; // number of equality constraints 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 nsensor; // number of sensors mjtSize nnumeric; // number of numeric custom fields @@ -1096,7 +1094,6 @@ struct mjModel_ { // sizes set after mjModel construction mjtSize nnames_map; // number of slots in the names hash map 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 nemax; // number of potential equality-constraint rows 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_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* 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_actfrclimited; // does tendon have actuator force limits (ntendon x 1) mjtNum* tendon_width; // width for rendering (ntendon x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 314dfc0e..b7a1e635 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -323,9 +323,6 @@ struct mjData_ { // computed by mj_fwdPosition/mj_tendon 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_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_length; // tendon lengths (ntendon x 1) int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap x 2) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 20151f48..97de5c2f 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -728,6 +728,7 @@ struct mjModel_ { mjtSize nexclude; // number of excluded geom pairs mjtSize neq; // number of equality constraints 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 nsensor; // number of sensors mjtSize nnumeric; // number of numeric custom fields @@ -754,7 +755,6 @@ struct mjModel_ { // sizes set after mjModel construction mjtSize nnames_map; // number of slots in the names hash map 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 nemax; // number of potential equality-constraint rows 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_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* 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_actfrclimited; // does tendon have actuator force limits (ntendon x 1) mjtNum* tendon_width; // width for rendering (ntendon x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index cba09e7c..3894c898 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -215,6 +215,7 @@ X( nexclude ) \ X( neq ) \ X( ntendon ) \ + X( nJten ) \ X( nwrap ) \ X( nsensor ) \ X( nnumeric ) \ @@ -239,7 +240,6 @@ X( npaths ) \ X( nnames_map ) \ X( nJmom ) \ - X( nJten ) \ X( ngravcomp ) \ X( nemax ) \ X( njmax ) \ @@ -629,6 +629,9 @@ X ( int, tendon_group, ntendon, 1 ) \ X ( int, tendon_treenum, ntendon, 1 ) \ 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_actfrclimited, ntendon, 1 ) \ X ( mjtNum, tendon_width, ntendon, 1 ) \ @@ -848,9 +851,6 @@ X ( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ X ( int, ten_wrapadr, 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_length, ntendon, 1 ) \ X ( int, wrap_obj, nwrap, 2 ) \ diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index d4e0d1fa..0ba4d41c 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -1129,9 +1129,9 @@ def _put_data_jax( mujoco.mju_sparse2dense( ten_J, d.ten_J, - d.ten_J_rownnz, - d.ten_J_rowadr, - d.ten_J_colind, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, ) else: ten_J = np.zeros((m.ntendon, m.nv)) @@ -1251,6 +1251,9 @@ def _put_data_c( for f in types.DataC.fields() 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. impl_fields['solver_niter'] = impl_fields['solver_niter'][0] @@ -1662,9 +1665,6 @@ def _get_data_into( ) else: 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 continue diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index ce8c2588..7f0d8c09 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -125,9 +125,9 @@ class SmoothTest(absltest.TestCase): mujoco.mju_sparse2dense( ten_J, d.ten_J, - d.ten_J_rownnz, - d.ten_J_rowadr, - d.ten_J_colind, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, ) _assert_eq(ten_J, dx._impl.ten_J, 'ten_J') _assert_attr_eq(d, dx, 'ten_length') @@ -407,9 +407,9 @@ class TendonTest(parameterized.TestCase): mujoco.mju_sparse2dense( ten_J, d.ten_J, - d.ten_J_rownnz, - d.ten_J_rowadr, - d.ten_J_colind, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, ) _assert_eq(ten_J, dx._impl.ten_J, 'ten_J') _assert_eq(d.ten_wrapnum, dx._impl.ten_wrapnum, 'ten_wrapnum') diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index 05bd1d2b..e6533dd6 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -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) - ten_J = np.zeros((mjm.ntendon, mjm.nv)) - mujoco.mju_sparse2dense(ten_J, mjd.ten_J.reshape(-1), mjd.ten_J_rownnz, mjd.ten_J_rowadr, mjd.ten_J_colind.reshape(-1)) - d.ten_J = wp.array(np.full((nworld, mjm.ntendon, mjm.nv), ten_J), dtype=float) + if mjm.ntendon: + ten_J = np.zeros((mjm.ntendon, mjm.nv)) + 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 actuator_moment = np.zeros((mjm.nu, mjm.nv)) @@ -1165,9 +1168,9 @@ def get_data_into( mujoco.mju_dense2sparse( result.ten_J, ten_J, - result.ten_J_rownnz, - result.ten_J_rowadr, - result.ten_J_colind, + mjm.ten_J_rownnz, + mjm.ten_J_rowadr, + mjm.ten_J_colind, ) else: result.ten_J[:] = d.ten_J.numpy()[world_id] diff --git a/mjx/mujoco/mjx/warp/forward_test.py b/mjx/mujoco/mjx/warp/forward_test.py index 30eb21cf..3eb936a7 100644 --- a/mjx/mujoco/mjx/warp/forward_test.py +++ b/mjx/mujoco/mjx/warp/forward_test.py @@ -153,9 +153,9 @@ class ForwardTest(parameterized.TestCase): mujoco.mju_sparse2dense( ten_J, d.ten_J, - d.ten_J_rownnz, - d.ten_J_rowadr, - d.ten_J_colind, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, ) tu.assert_eq(dx._impl.ten_J, ten_J, 'ten_J') tu.assert_attr_eq(dx._impl, d, 'ten_wrapadr') diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 9f26000d..524f9376 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -259,7 +259,10 @@ X( mjtNum, tendon, _length0, ntendon, 1 ) \ X( mjtNum, tendon, _invweight0, ntendon, 1 ) \ 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 \ X( int, tex_, type, ntex, 1 ) \ @@ -402,11 +405,7 @@ #define MJDATA_TENDON \ X( int, ten_, wrapadr , 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_, J , ntendon, MJ_M(nv) ) \ X( mjtNum, ten_, velocity, ntendon, 1 ) #define MJDATA_VIEW_GROUPS \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index c8f6774f..433b8167 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -1122,6 +1122,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtSize'), doc='number of tendons', ), + StructFieldDecl( + name='nJten', + type=ValueType(name='mjtSize'), + doc='number of non-zeros in sparse ten_J matrix', + ), StructFieldDecl( name='nwrap', type=ValueType(name='mjtSize'), @@ -1242,11 +1247,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtSize'), 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( name='ngravcomp', type=ValueType(name='mjtSize'), @@ -3927,6 +3927,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc="first two trees along tendon's path", 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( name='tendon_limited', type=PointerType( @@ -5812,30 +5836,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='number of wrap points in path', 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( name='ten_J', type=PointerType( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index d550766c..982463d7 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -568,16 +568,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // copy Jacobian: sparse or dense if (issparse) { if (j == 0) { - NV = d->ten_J_rownnz[id[j]]; - mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); - mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV); + NV = m->ten_J_rownnz[id[j]]; + mju_copyInt(chain, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV); + mju_copy(jac[j], d->ten_J+m->ten_J_rowadr[id[j]], NV); } else { - NV2 = d->ten_J_rownnz[id[j]]; - mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); - mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2); + NV2 = m->ten_J_rownnz[id[j]]; + mju_copyInt(chain2, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV2); + mju_copy(jac[j], d->ten_J+m->ten_J_rowadr[id[j]], NV2); } } 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; // add constraint 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], 1, mjCNSTR_FRICTION_TENDON, i, - d->ten_J_rownnz[i], - d->ten_J_colind+d->ten_J_rowadr[i]); + m->ten_J_rownnz[i], + m->ten_J_colind+m->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); + 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], 1, mjCNSTR_FRICTION_TENDON, i, 0, NULL); } @@ -885,13 +885,13 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { // 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]); + 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, 1, mjCNSTR_LIMIT_TENDON, i, - d->ten_J_rownnz[i], - d->ten_J_colind+d->ten_J_rowadr[i]); + m->ten_J_rownnz[i], + m->ten_J_colind+m->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); + 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); mj_addConstraint(m, d, jac, &dist, &margin, 0, 1, mjCNSTR_LIMIT_TENDON, i, 0, NULL); @@ -1738,11 +1738,11 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { } } else { if (!j) { - NV = d->ten_J_rownnz[id[j]]; - mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); + NV = m->ten_J_rownnz[id[j]]; + mju_copyInt(chain, m->ten_J_colind+m->ten_J_rowadr[id[j]], NV); } else { - NV2 = d->ten_J_rownnz[id[j]]; - mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); + NV2 = m->ten_J_rownnz[id[j]]; + 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++) { if (m->tendon_frictionloss[i] > 0) { - nf += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]); - if (nnz) *nnz += d->ten_J_rownnz[i]; + nf += mj_addConstraintCount(m, 1, m->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++) { int count = tendonLimit(m, d->ten_length, i); for (int j = 0; j < count; j++) { - nl += mj_addConstraintCount(m, 1, d->ten_J_rownnz[i]); - if (nnz) *nnz += d->ten_J_rownnz[i]; + nl += mj_addConstraintCount(m, 1, m->ten_J_rownnz[i]); + if (nnz) *nnz += m->ten_J_rownnz[i]; } } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 1d3fed48..47e0fdee 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -905,7 +905,7 @@ void mj_flex(const mjModel* m, mjData* d) { // compute tendon lengths and moments void mj_tendon(const mjModel* m, mjData* d) { 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; if (!nten) { @@ -913,22 +913,20 @@ void mj_tendon(const mjModel* m, mjData* d) { } // allocate stack arrays - int *chain, *buf_ind; - mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf; + int *chain; + mjtNum *jac1, *jac2, *jacdif, *tmp; 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); 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 - mju_zeroInt(rownnz, nten); + mju_zero(J, m->nJten); // sleep filtering 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; 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 if (m->wrap_type[adr] == mjWRAP_JOINT) { // process all defined joints @@ -960,11 +955,10 @@ void mj_tendon(const mjModel* m, mjData* d) { // add to length L[i] += m->wrap_prm[adr+j] * d->qpos[m->jnt_qposadr[k]]; - // add to moment - 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); + mjtNum coef = 1; + int dofadr = m->jnt_dofadr[k]; + mju_combineSparseInc(J + rowadr[i], &coef, m->nv, 1, m->wrap_prm[adr+j], + rownnz[i], 1, colind + rowadr[i], &dofadr); } continue; @@ -1059,9 +1053,9 @@ void mj_tendon(const mjModel* m, mjData* d) { 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); + mju_combineSparseInc(J+rowadr[i], tmp, nv, 1, 1/divisor, + rownnz[i], NV, colind+rowadr[i], + chain); } } @@ -1419,10 +1413,10 @@ void mj_transmission(const mjModel* m, mjData* d) { // moment { - int ten_J_rownnz = d->ten_J_rownnz[id]; - int ten_J_rowadr = d->ten_J_rowadr[id]; + int ten_J_rownnz = m->ten_J_rownnz[id]; + int ten_J_rowadr = m->ten_J_rowadr[id]; 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); } @@ -1717,9 +1711,9 @@ void mj_tendonArmature(const mjModel* m, mjData* d) { } // 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; + int J_rowadr = m->ten_J_rowadr[k]; + int J_rownnz = m->ten_J_rownnz[k]; + const int* J_colind = m->ten_J_colind + J_rowadr; mjtNum* ten_J = d->ten_J + J_rowadr; // M += armature * ten_J' * ten_J @@ -2649,9 +2643,9 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { if (coef) { // sparse - int nnz = d->ten_J_rownnz[i]; - int adr = d->ten_J_rowadr[i]; - const int* colind = d->ten_J_colind + adr; + int nnz = m->ten_J_rownnz[i]; + int adr = m->ten_J_rowadr[i]; + const int* colind = m->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 67b0d02e..09dd4903 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1776,7 +1776,7 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { } // 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); } } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index c38e6508..f2a7c3c4 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -232,7 +232,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { // 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); + m->ten_J_rownnz, m->ten_J_rowadr, m->ten_J_colind, NULL); // actuator velocity: always sparse if (!mjDISABLED(mjDSBL_ACTUATION)) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 512c7661..f0cf467f 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -211,10 +211,10 @@ void mj_makeModel(mjModel** dest, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, - mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nwrap, - mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, mjtSize ntextdata, - mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, mjtSize nplugin, - mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom, + mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten, + mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, + mjtSize ntextdata, mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, + 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_sensor, mjtSize nnames, mjtSize npaths) { intptr_t offset = 0; @@ -224,7 +224,7 @@ void mj_makeModel(mjModel** dest, // CHECK SIZE PARAMETERS { // 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; // 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 // 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)nbuffer; } @@ -323,6 +323,7 @@ void mj_makeModel(mjModel** dest, m->nexclude = nexclude; m->neq = neq; m->ntendon = ntendon; + m->nJten = nJten; m->nwrap = nwrap; m->nsensor = nsensor; m->nnumeric = nnumeric; @@ -410,11 +411,11 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata, src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude, - src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric, - src->nnumericdata, src->ntext, src->ntextdata, src->ntuple, - src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr, - src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site, - src->nuser_cam, src->nuser_tendon, src->nuser_actuator, + src->neq, src->ntendon, src->nJten, src->nwrap, src->nsensor, + src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata, + src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin, + src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom, + src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator, src->nuser_sensor, src->nnames, src->npaths); } 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[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[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 if (!m) { diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 918ad5a3..0af1058f 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -58,10 +58,10 @@ void mj_makeModel(mjModel** dest, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, - mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nwrap, - mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, mjtSize ntextdata, - mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, mjtSize nplugin, - mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom, + mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten, + mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, + mjtSize ntextdata, mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, + 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_sensor, mjtSize nnames, mjtSize npaths); diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 5e26c2c3..60446c13 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -473,9 +473,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // 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]; + int end = m->ten_J_rowadr[i] + m->ten_J_rownnz[i]; + for (int j=m->ten_J_rowadr[i]; j < end; j++) { + int k = m->ten_J_colind[j]; mjtNum J = d->ten_J[j]; d->qfrc_spring[k] += J * frc_spring; d->qfrc_damper[k] += J * frc_damper; diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 0a927715..a8f407d9 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -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("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, - 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, m->ten_J_rowadr, NULL, + m->ten_J_rownnz, NULL, m->ten_J_colind, fp); + printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, m->ten_J_rownnz, fp); + printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, m->ten_J_rowadr, fp); + printSparse("TEN_J", d->ten_J, m->ntendon, m->ten_J_rownnz, + m->ten_J_rowadr, m->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 435291a4..6dd01a94 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -282,6 +282,93 @@ static void setFixed(mjModel* m, mjData* 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} static void makeFlexSparse(mjModel* m, mjData* d) { int nv = m->nv; @@ -552,6 +639,7 @@ static void mj_alignFlex(mjModel* m, mjData* d) { // set quantities that depend on qpos0 static void set0(mjModel* m, mjData* d) { + makeTendonSparse(m); makeFlexSparse(m, d); mj_alignFlex(m, d); int nv = m->nv; @@ -756,7 +844,7 @@ static void set0(mjModel* m, mjData* d) { // compute tendon_invweight0 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 mj_solveM(m, d, tmp+nv, tmp, 1); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 1c3e7e24..6c642352 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -907,6 +907,7 @@ void mjCModel::ComputeSparseSizes() { // no dofs, quick return if (nv == 0) { nM = nD = nB = nC = 0; + nJten = 0; return; } @@ -1084,6 +1085,37 @@ void mjCModel::ComputeSparseSizes() { } } nC = nOD + nv; + + nJten = 0; + if (nv > 0) { + std::vector 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, nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, nskinface, nskinbone, 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, nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam, 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 m->nJmom = nJmom = CountNJmom(m); - // compute non-zeros in ten_J - m->nJten = nJten = CountNJten(m); // scale mass if (compiler.settotalmass > 0) { diff --git a/src/user/user_model.h b/src/user/user_model.h index 2a3ac355..025f8097 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -73,6 +73,7 @@ class mjCModel_ : public mjsElement { mjtSize nexclude; // number of excluded body pairs mjtSize neq; // number of equality constraints mjtSize ntendon; // number of tendons + mjtSize nJten; // number of non-zeros in sparse ten_J matrix mjtSize nsensor; // number of sensors mjtSize nnumeric; // number of numeric 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 nD; // number of non-zeros in sparse dof-dof 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 double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 7b292bfe..0449cb4f 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -154,12 +154,8 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) { mjData* data = mj_makeData(model); mj_fwdPosition(model, data); - int rowadr = data->ten_J_rowadr[0]; - int* colind = data->ten_J_colind + rowadr; - mjtNum* J = data->ten_J + rowadr; - + mjtNum* J = data->ten_J; EXPECT_THAT(vector(J, J + 3), ElementsAre(1, 2, 3)); - EXPECT_THAT(vector(colind, colind + 3), ElementsAre(0, 1, 2)); mj_deleteData(data); mj_deleteModel(model); @@ -253,11 +249,11 @@ TEST_F(CoreSmoothTest, TendonArmature) { // add tendon inertias to M2 using outer product for (int j=0; j < m->ntendon; j++) { // get tendon Jacobian - int rowadr = d->ten_J_rowadr[j]; - int* rownnz = d->ten_J_rownnz + j; + int rowadr = m->ten_J_rowadr[j]; + int* rownnz = m->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); + rownnz, &zero, m->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); diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 8fd84caa..bfd68c0e 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -199,6 +199,48 @@ TEST_F(UserModelTest, ActuatorSparsity) { mj_deleteModel(m); } +TEST_F(UserModelTest, FixedTendonSparsity) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + )"; + 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(colind, colind + 3), ElementsAre(0, 1, 2)); + + mj_deleteModel(m); +} + TEST_F(UserModelTest, NestedZeroMassBodiesOK) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 949dc889..0bd6e798 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4994,9 +4994,6 @@ public unsafe struct mjData_ { public double* bvh_aabb_dyn; public int* ten_wrapadr; 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_length; public int* wrap_obj; @@ -5347,6 +5344,7 @@ public unsafe struct mjModel_ { public Int64 nexclude; public Int64 neq; public Int64 ntendon; + public Int64 nJten; public Int64 nwrap; public Int64 nsensor; public Int64 nnumeric; @@ -5371,7 +5369,6 @@ public unsafe struct mjModel_ { public Int64 npaths; public Int64 nnames_map; public Int64 nJmom; - public Int64 nJten; public Int64 ngravcomp; public Int64 nemax; public Int64 njmax; @@ -5712,6 +5709,9 @@ public unsafe struct mjModel_ { public int* tendon_group; public int* tendon_treenum; 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_actfrclimited; public double* tendon_width; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index d8b69429..3aa71a8c 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -3934,6 +3934,12 @@ struct MjModel { void set_ntendon(int value) { ptr_->ntendon = static_cast(value); } + int nJten() const { + return static_cast(ptr_->nJten); + } + void set_nJten(int value) { + ptr_->nJten = static_cast(value); + } int nwrap() const { return static_cast(ptr_->nwrap); } @@ -4078,12 +4084,6 @@ struct MjModel { void set_nJmom(int value) { ptr_->nJmom = static_cast(value); } - int nJten() const { - return static_cast(ptr_->nJten); - } - void set_nJten(int value) { - ptr_->nJten = static_cast(value); - } int ngravcomp() const { return static_cast(ptr_->ngravcomp); } @@ -5125,6 +5125,15 @@ struct MjModel { emscripten::val tendon_treeid() const { 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 { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_limited)); } @@ -6649,15 +6658,6 @@ struct MjData { emscripten::val ten_wrapnum() const { 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 { 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_linvel", &MjData::subtree_linvel) .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_velocity", &MjData::ten_velocity) .property("ten_wrapadr", &MjData::ten_wrapadr) @@ -12100,6 +12097,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("skin_vertadr", &MjModel::skin_vertadr) .property("skin_vertnum", &MjModel::skin_vertnum) .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_actfrcrange", &MjModel::tendon_actfrcrange) .property("tendon_adr", &MjModel::tendon_adr)