Adds support for fixed tendons to MJX.

PiperOrigin-RevId: 654816559
Change-Id: I16ab379b0dc96d62a530e1c3ccd98a76b78c7f5a
This commit is contained in:
Erik Frey
2024-07-22 10:47:57 -07:00
committed by Copybara-Service
parent 33e5960653
commit 2d24c58819
15 changed files with 229 additions and 68 deletions
+5 -1
View File
@@ -18,10 +18,14 @@ MJX
^^^ ^^^
4. Added more fields to ``mjx.Model`` and ``mjx.Data`` for further compatibility with the corresponding MuJoCo structs. 4. Added more fields to ``mjx.Model`` and ``mjx.Data`` for further compatibility with the corresponding MuJoCo structs.
5. Added support for :ref:`fixed tendons <tendon-fixed>`.
6. Added support for tendon length limits (``mjCNSTR_LIMIT_TENDON`` in :ref:`mjtConstraint`).
7. Added support for tendon equality constraints (``mjEQ_TENDON`` in :ref:`mjtEq`).
8. Added support for tendon actuator transmission (``mjTRN_TENDON`` in :ref:`mjtTrn`).
Python bindings Python bindings
^^^^^^^^^^^^^^^ ^^^^^^^^^^^^^^^
5. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and 9. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and
``mujoco.spec.compile``. ``mujoco.spec.compile``.
Version 3.2.0 (Jul 15, 2024) Version 3.2.0 (Jul 15, 2024)
+5 -7
View File
@@ -188,7 +188,7 @@ The following features are **fully supported** in MJX:
* - :ref:`Joint <mjtJoint>` * - :ref:`Joint <mjtJoint>`
- ``FREE``, ``BALL``, ``SLIDE``, ``HINGE`` - ``FREE``, ``BALL``, ``SLIDE``, ``HINGE``
* - :ref:`Transmission <mjtTrn>` * - :ref:`Transmission <mjtTrn>`
- ``TRN_JOINT``, ``TRN_SITE`` - ``TRN_JOINT``, ``TRN_SITE``, ``TRN_TENDON``
* - :ref:`Actuator Dynamics <mjtDyn>` * - :ref:`Actuator Dynamics <mjtDyn>`
- ``NONE``, ``INTEGRATOR``, ``FILTER``, ``FILTEREXACT`` - ``NONE``, ``INTEGRATOR``, ``FILTER``, ``FILTEREXACT``
* - :ref:`Actuator Gain <mjtGain>` * - :ref:`Actuator Gain <mjtGain>`
@@ -200,7 +200,7 @@ The following features are **fully supported** in MJX:
* - :ref:`Constraint <mjtConstraint>` * - :ref:`Constraint <mjtConstraint>`
- ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``, ``CONTACT_ELLIPTIC`` - ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``, ``CONTACT_ELLIPTIC``
* - :ref:`Equality <mjtEq>` * - :ref:`Equality <mjtEq>`
- ``CONNECT``, ``WELD``, ``JOINT`` - ``CONNECT``, ``WELD``, ``JOINT``, ``TENDON``
* - :ref:`Integrator <mjtIntegrator>` * - :ref:`Integrator <mjtIntegrator>`
- ``EULER``, ``RK4`` - ``EULER``, ``RK4``
* - :ref:`Cone <mjtCone>` * - :ref:`Cone <mjtCone>`
@@ -211,6 +211,8 @@ The following features are **fully supported** in MJX:
- ``CG``, ``NEWTON`` - ``CG``, ``NEWTON``
* - Fluid Model * - Fluid Model
- :ref:`flInertia` - :ref:`flInertia`
* - :ref:`Tendons <tendon>`
- :ref:`Fixed <tendon-fixed>`
The following features are **in development** and coming soon: The following features are **in development** and coming soon:
@@ -230,8 +232,6 @@ The following features are **in development** and coming soon:
- ``IMPLICIT``, ``IMPLICITFAST`` - ``IMPLICIT``, ``IMPLICITFAST``
* - Dynamics * - Dynamics
- :ref:`Inverse <mj_inverse>` - :ref:`Inverse <mj_inverse>`
* - :ref:`Transmission <mjtTrn>`
- ``TRN_TENDON``
* - :ref:`Actuator Dynamics <mjtDyn>` * - :ref:`Actuator Dynamics <mjtDyn>`
- ``MUSCLE`` - ``MUSCLE``
* - :ref:`Actuator Gain <mjtGain>` * - :ref:`Actuator Gain <mjtGain>`
@@ -243,9 +243,7 @@ The following features are **in development** and coming soon:
* - Fluid Model * - Fluid Model
- :ref:`flEllipsoid` - :ref:`flEllipsoid`
* - :ref:`Tendons <tendon>` * - :ref:`Tendons <tendon>`
- :ref:`Spatial <tendon-spatial>`, :ref:`Fixed <tendon-fixed>` - :ref:`Spatial <tendon-spatial>`
* - :ref:`Equality <mjtEq>`
- ``TENDON``
* - :ref:`Sensors <mjtSensor>` * - :ref:`Sensors <mjtSensor>`
- All except ``PLUGIN``, ``USER`` - All except ``PLUGIN``, ``USER``
* - Lights * - Lights
+1
View File
@@ -39,6 +39,7 @@ from mujoco.mjx._src.smooth import crb
from mujoco.mjx._src.smooth import factor_m from mujoco.mjx._src.smooth import factor_m
from mujoco.mjx._src.smooth import kinematics from mujoco.mjx._src.smooth import kinematics
from mujoco.mjx._src.smooth import rne from mujoco.mjx._src.smooth import rne
from mujoco.mjx._src.smooth import tendon
from mujoco.mjx._src.smooth import transmission from mujoco.mjx._src.smooth import transmission
from mujoco.mjx._src.solver import solve from mujoco.mjx._src.solver import solve
from mujoco.mjx._src.support import full_m from mujoco.mjx._src.support import full_m
+73
View File
@@ -205,6 +205,44 @@ def _efc_equality_joint(m: Model, d: Data) -> Optional[_Efc]:
return rows(*args) return rows(*args)
def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]:
"""Calculates constraint rows for tendon equality constraints."""
eq_id = np.nonzero(m.eq_type == EqType.TENDON)[0]
if (m.opt.disableflags & DisableBit.EQUALITY) or eq_id.size == 0:
return None
obj1id, obj2id, data, solref, solimp = jax.tree_util.tree_map(
lambda x: x[eq_id],
(
m.eq_obj1id,
m.eq_obj2id,
m.eq_data,
m.eq_solref,
m.eq_solimp,
),
)
@jax.vmap
def rows(obj2id, data, solref, solimp, invweight, jac1, jac2, pos1, pos2):
dif = pos2 * (obj2id > -1)
dif_power = jp.power(dif, jp.arange(0, 5))
pos = pos1 - jp.dot(data[:5], dif_power)
deriv = jp.dot(data[1:5], dif_power[:4] * jp.arange(1, 5)) * (obj2id > -1)
j = jac1 + jac2 * -deriv
return _row(j, pos, pos, invweight, solref, solimp)
inv1, inv2 = m.tendon_invweight0[obj1id], m.tendon_invweight0[obj2id]
jac1, jac2 = d.ten_J[obj1id], d.ten_J[obj2id]
pos1 = d.ten_length[obj1id] - m.tendon_length0[obj1id]
pos2 = d.ten_length[obj2id] - m.tendon_length0[obj2id]
invweight = inv1 + inv2 * (obj2id > -1)
return rows(obj2id, data, solref, solimp, invweight, jac1, jac2, pos1, pos2)
def _efc_friction(m: Model, d: Data) -> Optional[_Efc]: def _efc_friction(m: Model, d: Data) -> Optional[_Efc]:
# TODO(robotics-team): implement _instantiate_friction # TODO(robotics-team): implement _instantiate_friction
del m, d del m, d
@@ -222,6 +260,8 @@ def _efc_limit_ball(m: Model, d: Data) -> Optional[_Efc]:
@jax.vmap @jax.vmap
def rows(qposadr, dofadr, jnt_range, jnt_margin, solref, solimp): def rows(qposadr, dofadr, jnt_range, jnt_margin, solref, solimp):
axis, angle = math.quat_to_axis_angle(d.qpos[jp.arange(4) + qposadr]) axis, angle = math.quat_to_axis_angle(d.qpos[jp.arange(4) + qposadr])
# ball rotation angle is always positive
axis, angle = math.normalize_with_norm(axis * angle)
pos = jp.amax(jnt_range) - angle - jnt_margin pos = jp.amax(jnt_range) - angle - jnt_margin
active = pos < 0 active = pos < 0
j = jp.zeros(m.nv).at[jp.arange(3) + dofadr].set(-axis) j = jp.zeros(m.nv).at[jp.arange(3) + dofadr].set(-axis)
@@ -263,6 +303,34 @@ def _efc_limit_slide_hinge(m: Model, d: Data) -> Optional[_Efc]:
return rows(*args) return rows(*args)
def _efc_limit_tendon(m: Model, d: Data) -> Optional[_Efc]:
"""Calculates constraint rows for tendon limits."""
tendon_id = np.nonzero(m.tendon_limited)[0]
if (m.opt.disableflags & DisableBit.LIMIT) or tendon_id.size == 0:
return None
length, j, range_, margin, invweight, solref, solimp = jax.tree_util.tree_map(
lambda x: x[tendon_id],
(
d.ten_length,
d.ten_J,
m.tendon_range,
m.tendon_margin,
m.tendon_invweight0,
m.tendon_solref_lim,
m.tendon_solimp_lim,
),
)
dist_min, dist_max = length - range_[:, 0], range_[:, 1] - length
pos = jp.minimum(dist_min, dist_max) - margin
active = pos < 0
j = jax.vmap(jp.multiply)(j, ((dist_min < dist_max) * 2 - 1) * active)
return jax.vmap(_row)(j, pos * active, pos, invweight, solref, solimp)
def _efc_contact_frictionless(m: Model, d: Data) -> Optional[_Efc]: def _efc_contact_frictionless(m: Model, d: Data) -> Optional[_Efc]:
"""Calculates constraint rows for frictionless contacts.""" """Calculates constraint rows for frictionless contacts."""
@@ -365,6 +433,7 @@ def counts(efc_type: np.ndarray) -> Tuple[int, int, int, int]:
ne = (efc_type == ConstraintType.EQUALITY).sum() ne = (efc_type == ConstraintType.EQUALITY).sum()
nf = 0 # no support for friction loss yet nf = 0 # no support for friction loss yet
nl = (efc_type == ConstraintType.LIMIT_JOINT).sum() nl = (efc_type == ConstraintType.LIMIT_JOINT).sum()
nl += (efc_type == ConstraintType.LIMIT_TENDON).sum()
nc_f = (efc_type == ConstraintType.CONTACT_FRICTIONLESS).sum() nc_f = (efc_type == ConstraintType.CONTACT_FRICTIONLESS).sum()
nc_p = (efc_type == ConstraintType.CONTACT_PYRAMIDAL).sum() nc_p = (efc_type == ConstraintType.CONTACT_PYRAMIDAL).sum()
nc_e = (efc_type == ConstraintType.CONTACT_ELLIPTIC).sum() nc_e = (efc_type == ConstraintType.CONTACT_ELLIPTIC).sum()
@@ -387,10 +456,12 @@ def make_efc_type(
num_rows = (m.eq_type == EqType.CONNECT).sum() * 3 num_rows = (m.eq_type == EqType.CONNECT).sum() * 3
num_rows += (m.eq_type == EqType.WELD).sum() * 6 num_rows += (m.eq_type == EqType.WELD).sum() * 6
num_rows += (m.eq_type == EqType.JOINT).sum() num_rows += (m.eq_type == EqType.JOINT).sum()
num_rows += (m.eq_type == EqType.TENDON).sum()
efc_types += [ConstraintType.EQUALITY] * num_rows efc_types += [ConstraintType.EQUALITY] * num_rows
if not m.opt.disableflags & DisableBit.LIMIT: if not m.opt.disableflags & DisableBit.LIMIT:
efc_types += [ConstraintType.LIMIT_JOINT] * m.jnt_limited.sum() efc_types += [ConstraintType.LIMIT_JOINT] * m.jnt_limited.sum()
efc_types += [ConstraintType.LIMIT_TENDON] * m.tendon_limited.sum()
if not m.opt.disableflags & DisableBit.CONTACT: if not m.opt.disableflags & DisableBit.CONTACT:
for condim in (1, 3, 4, 6): for condim in (1, 3, 4, 6):
@@ -441,9 +512,11 @@ def make_constraint(m: Model, d: Data) -> Data:
_efc_equality_connect(m, d), _efc_equality_connect(m, d),
_efc_equality_weld(m, d), _efc_equality_weld(m, d),
_efc_equality_joint(m, d), _efc_equality_joint(m, d),
_efc_equality_tendon(m, d),
_efc_friction(m, d), _efc_friction(m, d),
_efc_limit_ball(m, d), _efc_limit_ball(m, d),
_efc_limit_slide_hinge(m, d), _efc_limit_slide_hinge(m, d),
_efc_limit_tendon(m, d),
_efc_contact_frictionless(m, d), _efc_contact_frictionless(m, d),
) )
if m.opt.cone == ConeType.ELLIPTIC: if m.opt.cone == ConeType.ELLIPTIC:
+5 -5
View File
@@ -96,21 +96,21 @@ class ConstraintTest(parameterized.TestCase):
ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m)) ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
self.assertEqual(ne, 0) self.assertEqual(ne, 0)
self.assertEqual(nf, 0) self.assertEqual(nf, 0)
self.assertEqual(nl, 3) self.assertEqual(nl, 5)
self.assertEqual(nc, 148) self.assertEqual(nc, 148)
dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
self.assertEqual(dx.efc_J.shape[0], 151) # only joint range, contact self.assertEqual(dx.efc_J.shape[0], 153) # only joint/tendon limit, contact
def test_disable_contact(self): def test_disable_contact(self):
m = test_util.load_test_file('constraints.xml') m = test_util.load_test_file('constraints.xml')
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONTACT m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONTACT
ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m)) ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
self.assertEqual(ne, 10) self.assertEqual(ne, 11)
self.assertEqual(nf, 0) self.assertEqual(nf, 0)
self.assertEqual(nl, 3) self.assertEqual(nl, 5)
self.assertEqual(nc, 0) self.assertEqual(nc, 0)
dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
self.assertEqual(dx.efc_J.shape[0], 13) # only joint range, limit self.assertEqual(dx.efc_J.shape[0], 16) # only equality, joint/tendon limit
if __name__ == '__main__': if __name__ == '__main__':
+5 -1
View File
@@ -66,6 +66,7 @@ def fwd_position(m: Model, d: Data) -> Data:
d = smooth.kinematics(m, d) d = smooth.kinematics(m, d)
d = smooth.com_pos(m, d) d = smooth.com_pos(m, d)
d = smooth.camlight(m, d) d = smooth.camlight(m, d)
d = smooth.tendon(m, d)
d = smooth.crb(m, d) d = smooth.crb(m, d)
d = smooth.factor_m(m, d) d = smooth.factor_m(m, d)
d = collision_driver.collision(m, d) d = collision_driver.collision(m, d)
@@ -77,7 +78,10 @@ def fwd_position(m: Model, d: Data) -> Data:
@named_scope @named_scope
def fwd_velocity(m: Model, d: Data) -> Data: def fwd_velocity(m: Model, d: Data) -> Data:
"""Velocity-dependent computations.""" """Velocity-dependent computations."""
d = d.replace(actuator_velocity=d.actuator_moment @ d.qvel) d = d.replace(
actuator_velocity=d.actuator_moment @ d.qvel,
ten_velocity=d.ten_J @ d.qvel,
)
d = smooth.com_vel(m, d) d = smooth.com_vel(m, d)
d = passive.passive(m, d) d = passive.passive(m, d)
d = smooth.rne(m, d) d = smooth.rne(m, d)
+2 -4
View File
@@ -15,7 +15,7 @@
"""Functions to initialize, load, or save data.""" """Functions to initialize, load, or save data."""
import copy import copy
from typing import Any, Dict, List, Tuple, Union from typing import List, Tuple, Union
import jax import jax
from jax import numpy as jp from jax import numpy as jp
@@ -74,9 +74,6 @@ def put_model(
) -> types.Model: ) -> types.Model:
"""Puts mujoco.MjModel onto a device, resulting in mjx.Model.""" """Puts mujoco.MjModel onto a device, resulting in mjx.Model."""
if _check_unsupported and m.ntendon:
raise NotImplementedError('tendons are not supported')
mesh_geomid = set() mesh_geomid = set()
for g1, g2, ip in collision_driver.geom_pairs(m): for g1, g2, ip in collision_driver.geom_pairs(m):
t1, t2 = m.geom_type[[g1, g2]] t1, t2 = m.geom_type[[g1, g2]]
@@ -104,6 +101,7 @@ def put_model(
(m.actuator_gaintype, types.GainType, mujoco.mjtGain), (m.actuator_gaintype, types.GainType, mujoco.mjtGain),
(m.actuator_trntype, types.TrnType, mujoco.mjtTrn), (m.actuator_trntype, types.TrnType, mujoco.mjtTrn),
(m.eq_type, types.EqType, mujoco.mjtEq), (m.eq_type, types.EqType, mujoco.mjtEq),
(m.wrap_type, types.WrapType, mujoco.mjtWrap),
): ):
missing = set(enum_field) - set(enum_type) missing = set(enum_field) - set(enum_type)
if _check_unsupported and missing: if _check_unsupported and missing:
+15 -7
View File
@@ -148,20 +148,28 @@ class ModelIOTest(parameterized.TestCase):
) )
) )
def test_tendon_not_implemented(self): def test_spatial_tendon_not_implemented(self):
with self.assertRaises(NotImplementedError): with self.assertRaises(NotImplementedError):
mjx.put_model(mujoco.MjModel.from_xml_string(""" mjx.put_model(mujoco.MjModel.from_xml_string("""
<mujoco> <mujoco>
<worldbody> <worldbody>
<body> <body name="arm">
<joint name="left_hip" type="hinge"/> <joint name="arm" axis="0 1 0"/>
<geom size="0.05"/> <geom name="shoulder" type="sphere" size=".05"/>
<site name="arm" pos="-.1 0 .05"/>
</body>
<body name="slider" pos=".05 0 -.2">
<joint name="slider" type="slide" damping="1"/>
<geom name="slider" type="box" size=".01 .01 .01"/>
<site name="slider" pos="0 0 .01"/>
</body> </body>
</worldbody> </worldbody>
<tendon> <tendon>
<fixed> <spatial name="rope" range="0 .35">
<joint coef="1" joint="left_hip"/> <site site="slider"/>
</fixed> <site site="arm"/>
</spatial>
</tendon> </tendon>
</mujoco>""")) </mujoco>"""))
+7
View File
@@ -70,6 +70,13 @@ def _spring_damper(m: Model, d: Data) -> jax.Array:
# dof-level dampers # dof-level dampers
qfrc -= m.dof_damping * d.qvel qfrc -= m.dof_damping * d.qvel
# tendon-level spring-dampers
below, above = m.tendon_lengthspring.T - d.ten_length
frc_spring = jp.where(below > 0, m.tendon_stiffness * below, 0)
frc_spring = jp.where(above < 0, m.tendon_stiffness * above, frc_spring)
frc_damper = -m.tendon_damping * d.ten_velocity
qfrc += d.ten_J.T @ (frc_spring + frc_damper)
return qfrc return qfrc
+1 -1
View File
@@ -42,7 +42,7 @@ class PassiveTest(absltest.TestCase):
m = test_util.load_test_file('pendula.xml') m = test_util.load_test_file('pendula.xml')
d = mujoco.MjData(m) d = mujoco.MjData(m)
# give the system a little kick to ensure we have non-identity rotations # give the system a little kick to ensure we have non-identity rotations
d.ctrl = np.array([0.1, -0.1, 0.2, 0.3, -0.4, 0.5, -0.6, 0.1]) d.ctrl = np.array([0.1, -0.1, 0.2, 0.3, -0.4, 0.5, -0.6, 0.1, -0.2])
mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
mx = mjx.put_model(m) mx = mjx.put_model(m)
+20 -2
View File
@@ -311,7 +311,9 @@ def factor_m(m: Model, d: Data) -> Data:
if j == -1: if j == -1:
break break
out_beg, out_end = tuple(m.dof_Madr[j : j + 2]) out_beg, out_end = tuple(m.dof_Madr[j : j + 2])
updates.setdefault(depth[j], []).append((out_beg, out_end, madr_d, madr_ij)) updates.setdefault(depth[j], []).append(
(out_beg, out_end, madr_d, madr_ij)
)
qld = d.qM qld = d.qM
@@ -463,6 +465,20 @@ def rne(m: Model, d: Data) -> Data:
return d return d
def tendon(m: Model, d: Data) -> Data:
"""Computes tendon lengths and moments."""
if not m.ntendon:
return d
ten_id = np.repeat(np.arange(m.ntendon), m.tendon_num)
length = m.wrap_prm * d.qpos[m.jnt_qposadr[m.wrap_objid]]
ten_length = jax.ops.segment_sum(length, ten_id, m.ntendon)
ten_j = jp.zeros((m.ntendon, m.nv))
ten_j = ten_j.at[ten_id, m.jnt_dofadr[m.wrap_objid]].set(m.wrap_prm)
return d.replace(ten_length=ten_length, ten_J=ten_j)
def _site_dof_mask(m: Model) -> np.ndarray: def _site_dof_mask(m: Model) -> np.ndarray:
"""Creates a dof mask for site transmissions.""" """Creates a dof mask for site transmissions."""
mask = np.ones((m.nu, m.nv)) mask = np.ones((m.nu, m.nv))
@@ -494,7 +510,6 @@ def _site_dof_mask(m: Model) -> np.ndarray:
def transmission(m: Model, d: Data) -> Data: def transmission(m: Model, d: Data) -> Data:
"""Computes actuator/transmission lengths and moments.""" """Computes actuator/transmission lengths and moments."""
# TODO: consider combining transmission calculation into fwd_actuation.
if not m.nu: if not m.nu:
return d return d
@@ -545,6 +560,9 @@ def transmission(m: Model, d: Data) -> Data:
jac = jp.concatenate((jacp, jacr), axis=1) * site_dof_mask[:, None] jac = jp.concatenate((jacp, jacr), axis=1) * site_dof_mask[:, None]
wrench = jp.concatenate((frame_xmat @ gear[:3], frame_xmat @ gear[3:])) wrench = jp.concatenate((frame_xmat @ gear[:3], frame_xmat @ gear[3:]))
moment = jac @ wrench moment = jac @ wrench
elif trntype == TrnType.TENDON:
length = d.ten_length[trnid[0]] * gear[:1]
moment = d.ten_J[trnid[0]] * gear[0]
else: else:
raise RuntimeError(f'unrecognized trntype: {TrnType(trntype)}') raise RuntimeError(f'unrecognized trntype: {TrnType(trntype)}')
+18 -6
View File
@@ -48,7 +48,7 @@ class SmoothTest(absltest.TestCase):
"""Tests MJX smooth functions match MuJoCo smooth functions.""" """Tests MJX smooth functions match MuJoCo smooth functions."""
m = test_util.load_test_file('pendula.xml') m = test_util.load_test_file('pendula.xml')
# # force MJX sparse for testing: # tell MJX to use sparse mass matrices:
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
d = mujoco.MjData(m) d = mujoco.MjData(m)
# give the system a little kick to ensure we have non-identity rotations # give the system a little kick to ensure we have non-identity rotations
@@ -75,6 +75,10 @@ class SmoothTest(absltest.TestCase):
_assert_attr_eq(d, dx, 'subtree_com') _assert_attr_eq(d, dx, 'subtree_com')
_assert_attr_eq(d, dx, 'cinert') _assert_attr_eq(d, dx, 'cinert')
_assert_attr_eq(d, dx, 'cdof') _assert_attr_eq(d, dx, 'cdof')
# camlight
dx = jax.jit(mjx.camlight)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'cam_xpos')
_assert_eq(d.cam_xmat.reshape((-1, 3, 3)), dx.cam_xmat, 'cam_xmat')
# crb # crb
dx = jax.jit(mjx.crb)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.crb)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'crb') _assert_attr_eq(d, dx, 'crb')
@@ -90,14 +94,22 @@ class SmoothTest(absltest.TestCase):
# rne # rne
dx = jax.jit(mjx.rne)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.rne)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'qfrc_bias') _assert_attr_eq(d, dx, 'qfrc_bias')
# set dense jacobian for tendon:
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_DENSE
d = mujoco.MjData(m)
# give the system a little kick to ensure we have non-identity rotations
d.qvel = np.random.random(m.nv)
mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero
mujoco.mj_forward(m, d)
# tendon
dx = jax.jit(mjx.tendon)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'ten_J')
_assert_attr_eq(d, dx, 'ten_length')
# transmission # transmission
dx = jax.jit(mjx.transmission)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.transmission)(mx, dx)
_assert_attr_eq(d, dx, 'actuator_length') _assert_attr_eq(d, dx, 'actuator_length')
_assert_attr_eq(d, dx, 'actuator_moment') _assert_attr_eq(d, dx, 'actuator_moment')
# camlight
dx = jax.jit(mjx.camlight)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'cam_xpos')
_assert_eq(d.cam_xmat.reshape((-1, 3, 3)), dx.cam_xmat, 'cam_xmat')
def test_disable_gravity(self): def test_disable_gravity(self):
m = mujoco.MjModel.from_xml_string(""" m = mujoco.MjModel.from_xml_string("""
+48 -34
View File
@@ -25,7 +25,7 @@ import numpy as np
class DisableBit(enum.IntFlag): class DisableBit(enum.IntFlag):
"""Disable default feature bitflags. """Disable default feature bitflags.
Attributes: Members:
CONSTRAINT: entire constraint solver CONSTRAINT: entire constraint solver
EQUALITY: equality constraints EQUALITY: equality constraints
FRICTIONLOSS: joint and tendon frictionloss constraints FRICTIONLOSS: joint and tendon frictionloss constraints
@@ -56,7 +56,7 @@ class DisableBit(enum.IntFlag):
class JointType(enum.IntEnum): class JointType(enum.IntEnum):
"""Type of degree of freedom. """Type of degree of freedom.
Attributes: Members:
FREE: global position and orientation (quat) (7,) FREE: global position and orientation (quat) (7,)
BALL: orientation (quat) relative to parent (4,) BALL: orientation (quat) relative to parent (4,)
SLIDE: sliding distance along body-fixed axis (1,) SLIDE: sliding distance along body-fixed axis (1,)
@@ -77,7 +77,7 @@ class JointType(enum.IntEnum):
class IntegratorType(enum.IntEnum): class IntegratorType(enum.IntEnum):
"""Integrator mode. """Integrator mode.
Attributes: Members:
EULER: semi-implicit Euler EULER: semi-implicit Euler
RK4: 4th-order Runge Kutta RK4: 4th-order Runge Kutta
""" """
@@ -89,7 +89,7 @@ class IntegratorType(enum.IntEnum):
class GeomType(enum.IntEnum): class GeomType(enum.IntEnum):
"""Type of geometry. """Type of geometry.
Attributes: Members:
PLANE: plane PLANE: plane
HFIELD: height field HFIELD: height field
SPHERE: sphere SPHERE: sphere
@@ -115,7 +115,7 @@ class GeomType(enum.IntEnum):
class ConvexMesh(PyTreeNode): class ConvexMesh(PyTreeNode):
"""Geom properties for convex meshes. """Geom properties for convex meshes.
Attributes: Members:
vert: vertices of the convex mesh vert: vertices of the convex mesh
face: faces of the convex mesh face: faces of the convex mesh
face_normal: normal vectors for the faces face_normal: normal vectors for the faces
@@ -133,7 +133,7 @@ class ConvexMesh(PyTreeNode):
class ConeType(enum.IntEnum): class ConeType(enum.IntEnum):
"""Type of friction cone. """Type of friction cone.
Attributes: Members:
PYRAMIDAL: pyramidal PYRAMIDAL: pyramidal
ELLIPTIC: elliptic ELLIPTIC: elliptic
""" """
@@ -144,7 +144,7 @@ class ConeType(enum.IntEnum):
class JacobianType(enum.IntEnum): class JacobianType(enum.IntEnum):
"""Type of constraint Jacobian. """Type of constraint Jacobian.
Attributes: Members:
DENSE: dense DENSE: dense
SPARSE: sparse SPARSE: sparse
AUTO: sparse if nv>60 and device is TPU, dense otherwise AUTO: sparse if nv>60 and device is TPU, dense otherwise
@@ -157,7 +157,7 @@ class JacobianType(enum.IntEnum):
class SolverType(enum.IntEnum): class SolverType(enum.IntEnum):
"""Constraint solver algorithm. """Constraint solver algorithm.
Attributes: Members:
CG: Conjugate gradient (primal) CG: Conjugate gradient (primal)
""" """
# unsupported: PGS # unsupported: PGS
@@ -168,33 +168,46 @@ class SolverType(enum.IntEnum):
class EqType(enum.IntEnum): class EqType(enum.IntEnum):
"""Type of equality constraint. """Type of equality constraint.
Attributes: Members:
CONNECT: connect two bodies at a point (ball joint) CONNECT: connect two bodies at a point (ball joint)
WELD: fix relative position and orientation of two bodies WELD: fix relative position and orientation of two bodies
JOINT: couple the values of two scalar joints with cubic JOINT: couple the values of two scalar joints with cubic
TENDON: couple the lengths of two tendons with cubic
""" """
CONNECT = mujoco.mjtEq.mjEQ_CONNECT CONNECT = mujoco.mjtEq.mjEQ_CONNECT
WELD = mujoco.mjtEq.mjEQ_WELD WELD = mujoco.mjtEq.mjEQ_WELD
JOINT = mujoco.mjtEq.mjEQ_JOINT JOINT = mujoco.mjtEq.mjEQ_JOINT
# unsupported: TENDON, DISTANCE TENDON = mujoco.mjtEq.mjEQ_TENDON
# unsupported: DISTANCE
class WrapType(enum.IntEnum):
"""Type of tendon wrap object.
Members:
JOINT: constant moment arm
"""
JOINT = mujoco.mjtWrap.mjWRAP_JOINT
# unsupported: NONE, PULLEY, SITE, SPHERE, CYLINDER
class TrnType(enum.IntEnum): class TrnType(enum.IntEnum):
"""Type of actuator transmission. """Type of actuator transmission.
Attributes: Members:
JOINT: force on joint JOINT: force on joint
SITE: force on site SITE: force on site
""" """
JOINT = mujoco.mjtTrn.mjTRN_JOINT JOINT = mujoco.mjtTrn.mjTRN_JOINT
SITE = mujoco.mjtTrn.mjTRN_SITE SITE = mujoco.mjtTrn.mjTRN_SITE
# unsupported: JOINTINPARENT, SLIDERCRANK, TENDON, BODY TENDON = mujoco.mjtTrn.mjTRN_TENDON
# unsupported: JOINTINPARENT, SLIDERCRANK, BODY
class DynType(enum.IntEnum): class DynType(enum.IntEnum):
"""Type of actuator dynamics. """Type of actuator dynamics.
Attributes: Members:
NONE: no internal dynamics; ctrl specifies force NONE: no internal dynamics; ctrl specifies force
INTEGRATOR: integrator: da/dt = u INTEGRATOR: integrator: da/dt = u
FILTER: linear filter: da/dt = (u-a) / tau FILTER: linear filter: da/dt = (u-a) / tau
@@ -210,7 +223,7 @@ class DynType(enum.IntEnum):
class GainType(enum.IntEnum): class GainType(enum.IntEnum):
"""Type of actuator gain. """Type of actuator gain.
Attributes: Members:
FIXED: fixed gain FIXED: fixed gain
AFFINE: const + kp*length + kv*velocity AFFINE: const + kp*length + kv*velocity
""" """
@@ -222,7 +235,7 @@ class GainType(enum.IntEnum):
class BiasType(enum.IntEnum): class BiasType(enum.IntEnum):
"""Type of actuator bias. """Type of actuator bias.
Attributes: Members:
NONE: no bias NONE: no bias
AFFINE: const + kp*length + kv*velocity AFFINE: const + kp*length + kv*velocity
""" """
@@ -234,16 +247,17 @@ class BiasType(enum.IntEnum):
class ConstraintType(enum.IntEnum): class ConstraintType(enum.IntEnum):
"""Type of constraint. """Type of constraint.
Attributes: Members:
EQUALITY: equality constraint EQUALITY: equality constraint
LIMIT_JOINT: joint limit LIMIT_JOINT: joint limit
LIMIT_TENDON: tendon limit
CONTACT_FRICTIONLESS: frictionless contact CONTACT_FRICTIONLESS: frictionless contact
CONTACT_PYRAMIDAL: frictional contact, pyramidal friction cone CONTACT_PYRAMIDAL: frictional contact, pyramidal friction cone
""" """
EQUALITY = mujoco.mjtConstraint.mjCNSTR_EQUALITY EQUALITY = mujoco.mjtConstraint.mjCNSTR_EQUALITY
# unsupported: FRICTION_DOF, FRICTION_TENDON # unsupported: FRICTION_DOF, FRICTION_TENDON
LIMIT_JOINT = mujoco.mjtConstraint.mjCNSTR_LIMIT_JOINT LIMIT_JOINT = mujoco.mjtConstraint.mjCNSTR_LIMIT_JOINT
# unsupported: LIMIT_TENDON LIMIT_TENDON = mujoco.mjtConstraint.mjCNSTR_LIMIT_TENDON
CONTACT_FRICTIONLESS = mujoco.mjtConstraint.mjCNSTR_CONTACT_FRICTIONLESS CONTACT_FRICTIONLESS = mujoco.mjtConstraint.mjCNSTR_CONTACT_FRICTIONLESS
CONTACT_PYRAMIDAL = mujoco.mjtConstraint.mjCNSTR_CONTACT_PYRAMIDAL CONTACT_PYRAMIDAL = mujoco.mjtConstraint.mjCNSTR_CONTACT_PYRAMIDAL
CONTACT_ELLIPTIC = mujoco.mjtConstraint.mjCNSTR_CONTACT_ELLIPTIC CONTACT_ELLIPTIC = mujoco.mjtConstraint.mjCNSTR_CONTACT_ELLIPTIC
@@ -252,7 +266,7 @@ class ConstraintType(enum.IntEnum):
class CamLightType(enum.IntEnum): class CamLightType(enum.IntEnum):
"""Type of camera light. """Type of camera light.
Attributes: Members:
FIXED: pos and rot fixed in body FIXED: pos and rot fixed in body
TRACK: pos tracks body, rot fixed in global TRACK: pos tracks body, rot fixed in global
TRACKCOM: pos tracks subtree com, rot fixed in body TRACKCOM: pos tracks subtree com, rot fixed in body
@@ -900,18 +914,18 @@ class Model(PyTreeNode):
tendon_adr: np.ndarray tendon_adr: np.ndarray
tendon_num: np.ndarray tendon_num: np.ndarray
tendon_limited: np.ndarray tendon_limited: np.ndarray
tendon_solref_lim: np.ndarray tendon_solref_lim: jax.Array
tendon_solimp_lim: np.ndarray tendon_solimp_lim: jax.Array
tendon_solref_fri: np.ndarray tendon_solref_fri: jax.Array
tendon_solimp_fri: np.ndarray tendon_solimp_fri: jax.Array
tendon_range: np.ndarray tendon_range: jax.Array
tendon_margin: np.ndarray tendon_margin: jax.Array
tendon_stiffness: np.ndarray tendon_stiffness: jax.Array
tendon_damping: np.ndarray tendon_damping: jax.Array
tendon_frictionloss: np.ndarray tendon_frictionloss: jax.Array
tendon_lengthspring: np.ndarray tendon_lengthspring: jax.Array
tendon_length0: np.ndarray tendon_length0: jax.Array
tendon_invweight0: np.ndarray tendon_invweight0: jax.Array
wrap_type: np.ndarray wrap_type: np.ndarray
wrap_objid: np.ndarray wrap_objid: np.ndarray
wrap_prm: np.ndarray wrap_prm: np.ndarray
@@ -1193,7 +1207,7 @@ class Data(PyTreeNode):
qM: jax.Array # pylint:disable=invalid-name qM: jax.Array # pylint:disable=invalid-name
qLD: jax.Array # pylint:disable=invalid-name qLD: jax.Array # pylint:disable=invalid-name
qLDiagInv: jax.Array # pylint:disable=invalid-name qLDiagInv: jax.Array # pylint:disable=invalid-name
qLDiagSqrtInv: jax.Array qLDiagSqrtInv: jax.Array # pylint:disable=invalid-name
bvh_aabb_dyn: jax.Array bvh_aabb_dyn: jax.Array
bvh_active: jax.Array bvh_active: jax.Array
# position, velocity dependent: # position, velocity dependent:
@@ -1243,6 +1257,6 @@ class Data(PyTreeNode):
efc_force: jax.Array efc_force: jax.Array
# sparse representation of qM, qLD, qLDiagInv, for compatibility with MuJoCo # sparse representation of qM, qLD, qLDiagInv, for compatibility with MuJoCo
# when in dense mode # when in dense mode
_qM_sparse: jax.Array _qM_sparse: jax.Array # pylint:disable=invalid-name
_qLD_sparse: jax.Array _qLD_sparse: jax.Array # pylint:disable=invalid-name
_qLDiagInv_sparse: jax.Array _qLDiagInv_sparse: jax.Array # pylint:disable=invalid-name
+12
View File
@@ -64,10 +64,22 @@
</body> </body>
</worldbody> </worldbody>
<tendon>
<fixed name="tendon_1" limited="true" range="-0.3 0.1" stiffness=".1" damping=".2">
<joint joint="joint3" coef=".1"/>
<joint joint="joint4" coef="-.2"/>
</fixed>
<fixed name="tendon_2" limited="true" range="-0.3 2" solreflimit="0.03 0.9" solimplimit="0.89 0.9 0.01 2.1">
<joint joint="joint4" coef=".3"/>
<joint joint="joint5" coef="-.4"/>
</fixed>
</tendon>
<equality> <equality>
<connect name="connect" body1="anchor1" body2="beam1" anchor="1 0 -1" /> <connect name="connect" body1="anchor1" body2="beam1" anchor="1 0 -1" />
<weld name="weld" body1="anchor2" body2="beam2" relpose="0 0 0 1 -.3 0 0" torquescale="0.002" anchor="0 -2 0"/> <weld name="weld" body1="anchor2" body2="beam2" relpose="0 0 0 1 -.3 0 0" torquescale="0.002" anchor="0 -2 0"/>
<joint name="joint" joint1="joint3" joint2="joint4" polycoef="0.5 -1 0.1 0.15 0.2" /> <joint name="joint" joint1="joint3" joint2="joint4" polycoef="0.5 -1 0.1 0.15 0.2" />
<tendon name="tendon" tendon1="tendon_1" tendon2="tendon_2" polycoef="0.5 -1 0.1 0.15 0.2"/>
</equality> </equality>
<actuator> <actuator>
+12
View File
@@ -126,6 +126,17 @@
</body> </body>
</worldbody> </worldbody>
<tendon>
<fixed name="tendon_1" limited="true" range="-0.3 0.1" stiffness=".1" damping=".2">
<joint joint="joint2" coef=".1"/>
<joint joint="joint3" coef="-.2"/>
</fixed>
<fixed name="tendon_2" limited="true" range="-0.3 2" springlength="0 0.05" stiffness=".3" damping=".4">
<joint joint="joint4" coef=".3"/>
<joint joint="joint5" coef="-.4"/>
</fixed>
</tendon>
<actuator> <actuator>
<motor gear="250 0 0" joint="joint1" name="act1"/> <motor gear="250 0 0" joint="joint1" name="act1"/>
<motor gear="0 275 0" joint="joint1" name="act2"/> <motor gear="0 275 0" joint="joint1" name="act2"/>
@@ -135,5 +146,6 @@
<motor gear="150" joint="joint15" name="act6"/> <motor gear="150" joint="joint15" name="act6"/>
<motor gear="150" joint="joint16" name="act7"/> <motor gear="150" joint="joint16" name="act7"/>
<motor gear="150" joint="joint17" name="act8"/> <motor gear="150" joint="joint17" name="act8"/>
<position tendon="tendon_2" kp="100"/>
</actuator> </actuator>
</mujoco> </mujoco>