Move tendon sparsity fields from mjData to mjModel
PiperOrigin-RevId: 875087590 Change-Id: I1a5489d2d2011795ee38b09d547e68acd72e3cc7
This commit is contained in:
committed by
Copybara-Service
parent
253494739a
commit
52ed96bc3a
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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 ) \
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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')
|
||||
|
||||
+9
-6
@@ -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]
|
||||
|
||||
@@ -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')
|
||||
|
||||
@@ -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 \
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -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];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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)) {
|
||||
|
||||
+14
-12
@@ -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) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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++) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
+33
-3
@@ -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<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,
|
||||
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) {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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<mjtNum>(J, J + 3), ElementsAre(1, 2, 3));
|
||||
EXPECT_THAT(vector<int>(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);
|
||||
|
||||
@@ -199,6 +199,48 @@ TEST_F(UserModelTest, ActuatorSparsity) {
|
||||
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) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -3934,6 +3934,12 @@ struct MjModel {
|
||||
void set_ntendon(int 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 {
|
||||
return static_cast<int>(ptr_->nwrap);
|
||||
}
|
||||
@@ -4078,12 +4084,6 @@ struct MjModel {
|
||||
void set_nJmom(int 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 {
|
||||
return static_cast<int>(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)
|
||||
|
||||
Reference in New Issue
Block a user