diff --git a/mjx/mujoco/mjx/__init__.py b/mjx/mujoco/mjx/__init__.py index 1c8d0661..d8e3f437 100644 --- a/mjx/mujoco/mjx/__init__.py +++ b/mjx/mujoco/mjx/__init__.py @@ -16,10 +16,17 @@ # pylint:disable=g-importing-member from mujoco.mjx._src.collision_driver import collision +from mujoco.mjx._src.constraint import count_constraints from mujoco.mjx._src.constraint import make_constraint from mujoco.mjx._src.device import device_get_into from mujoco.mjx._src.device import device_put +from mujoco.mjx._src.forward import euler from mujoco.mjx._src.forward import forward +from mujoco.mjx._src.forward import fwd_acceleration +from mujoco.mjx._src.forward import fwd_actuation +from mujoco.mjx._src.forward import fwd_position +from mujoco.mjx._src.forward import fwd_velocity +from mujoco.mjx._src.forward import rungekutta4 from mujoco.mjx._src.forward import step from mujoco.mjx._src.io import get_data from mujoco.mjx._src.io import make_data @@ -34,4 +41,5 @@ from mujoco.mjx._src.smooth import kinematics from mujoco.mjx._src.smooth import mul_m from mujoco.mjx._src.smooth import rne from mujoco.mjx._src.smooth import transmission +from mujoco.mjx._src.solver import solve from mujoco.mjx._src.types import * diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index afb7bd94..5b58f7f8 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -52,9 +52,9 @@ def _collide( mjcf: str, assets: Optional[Dict[str, str]] = None ) -> Tuple[mujoco.MjModel, mujoco.MjData, Model, Data]: m = mujoco.MjModel.from_xml_string(mjcf, assets or {}) - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) @@ -418,9 +418,9 @@ class BodyPairFilterTest(absltest.TestCase): def test_filter_parent_child(self): """Tests that parent-child collisions get filtered.""" m = mujoco.MjModel.from_xml_string(self._PARENT_CHILD) - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) @@ -435,9 +435,9 @@ class BodyPairFilterTest(absltest.TestCase): """Tests that filterparent flag disables parent-child filtering.""" m = mujoco.MjModel.from_xml_string(self._PARENT_CHILD) m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_FILTERPARENT - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) @@ -454,22 +454,14 @@ class NconTest(parameterized.TestCase): """Tests ncon.""" def test_ncon(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - d.qpos[2] = 0.0 - - mx = mjx.device_put(m) - ncon = collision_driver.ncon(mx) - self.assertEqual(ncon, 4) + m = test_util.load_test_file('constraints.xml') + ncon = collision_driver.ncon(m) + self.assertEqual(ncon, 16) def test_disable_contact(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - d.qpos[2] = 0.0 - - m.opt.disableflags = m.opt.disableflags | DisableBit.CONTACT - mx = mjx.device_put(m) - ncon = collision_driver.ncon(mx) + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags |= DisableBit.CONTACT + ncon = collision_driver.ncon(m) self.assertEqual(ncon, 0) @@ -500,12 +492,12 @@ class TopKContactTest(absltest.TestCase): def test_top_k_contacts(self): m = mujoco.MjModel.from_xml_string(self._CAPSULES) - mx_top_k = mjx.device_put(m) + mx_top_k = mjx.put_model(m) mx_all = mx_top_k.replace( nnumeric=0, name_numericadr=np.array([]), numeric_data=np.array([]) ) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) collision_jit_fn = jax.jit(mjx.collision) kinematics_jit_fn = jax.jit(mjx.kinematics) diff --git a/mjx/mujoco/mjx/_src/constraint_test.py b/mjx/mujoco/mjx/_src/constraint_test.py index 0510e1df..a2bd8cc9 100644 --- a/mjx/mujoco/mjx/_src/constraint_test.py +++ b/mjx/mujoco/mjx/_src/constraint_test.py @@ -15,159 +15,92 @@ """Tests for constraint functions.""" from absl.testing import absltest -from absl.testing import parameterized -import jax from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import constraint from mujoco.mjx._src import test_util -# pylint: disable=g-importing-member -from mujoco.mjx._src.types import DisableBit -from mujoco.mjx._src.types import SolverType -# pylint: enable=g-importing-member import numpy as np -def _assert_eq(a, b, name, step, fname, atol=5e-3, rtol=5e-3): - err_msg = f'mismatch: {name} at step {step} in {fname}' - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX constraint calculations, +# mostly due to float precision +_TOLERANCE = 5e-5 -class ConstraintTest(parameterized.TestCase): +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - @parameterized.parameters(enumerate(test_util.TEST_FILES)) - def test_constraints(self, seed, fname): + +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) + + +class ConstraintTest(absltest.TestCase): + + def test_constraints(self): """Test constraints.""" - np.random.seed(seed) - - # exclude convex.xml since convex contacts are not exactly equivalent - if fname == 'convex.xml': - return - - m = test_util.load_test_file(fname) + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mx = mjx.device_put(m) - dx = mjx.make_data(mx) + mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) - forward_jit_fn = jax.jit(mjx.forward) - - # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.random(m.nv) - for i in range(100): - dx = dx.replace(qpos=jax.device_put(d.qpos), qvel=jax.device_put(d.qvel)) - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx) - - nnz_filter = dx.efc_J.any(axis=1) - - mj_efc_j = d.efc_J.reshape((-1, m.nv)) - mjx_efc_j = dx.efc_J[nnz_filter] - _assert_eq(mj_efc_j, mjx_efc_j, 'efc_J', i, fname) - - mjx_efc_d = dx.efc_D[nnz_filter] - _assert_eq(d.efc_D, mjx_efc_d, 'efc_D', i, fname) - - mjx_efc_aref = dx.efc_aref[nnz_filter] - _assert_eq(d.efc_aref, mjx_efc_aref, 'efc_aref', i, fname) - - mjx_efc_frictionloss = dx.efc_frictionloss[nnz_filter] - _assert_eq( - d.efc_frictionloss, - mjx_efc_frictionloss, - 'efc_frictionloss', - i, - fname, - ) - - _JNT_RANGE = """ - - - - - - - - - - - - - """ - - def test_jnt_range(self): - """Tests that mixed joint ranges are respected.""" - # TODO(robotics-simulation): also test ball - m = mujoco.MjModel.from_xml_string(self._JNT_RANGE) - m.opt.solver = SolverType.CG.value - d = mujoco.MjData(m) - d.qpos = np.array([2.0, 15.0]) - - mx = mjx.device_put(m) - dx = mjx.device_put(d) - efc = jax.jit(constraint._instantiate_limit_slide_hinge)(mx, dx) - - # first joint is outside the joint range - np.testing.assert_array_almost_equal(efc.J[0, 0], -1.0) - - # second joint has no range, so only one efc row - self.assertEqual(efc.J.shape[0], 1) + dx = mjx.make_constraint(mx, dx) + nnz = dx.efc_J.any(axis=1) + _assert_eq(d.efc_J, dx.efc_J[nnz].reshape(-1), 'efc_J') + _assert_eq(d.efc_D, dx.efc_D[nnz], 'efc_D') + _assert_eq(d.efc_aref, dx.efc_aref[nnz], 'efc_aref') + _assert_eq(d.efc_frictionloss, dx.efc_frictionloss[nnz], 'efc_frictionloss') def test_disable_refsafe(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('constraints.xml') timeconst = m.opt.timestep / 4.0 # timeconst < 2 * timestep solimp = jp.array([timeconst, 1.0]) solref = jp.array([0.8, 0.99, 0.001, 0.2, 2]) pos = jp.ones(3) - m.opt.disableflags = m.opt.disableflags | DisableBit.REFSAFE + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.REFSAFE mx = mjx.device_put(m) k, *_ = constraint._kbi(mx, solimp, solref, pos) self.assertEqual(k, 1 / (0.99**2 * timeconst**2)) - m.opt.disableflags = m.opt.disableflags & ~DisableBit.REFSAFE - mx = mjx.device_put(m) - k, *_ = constraint._kbi(mx, solimp, solref, pos) - self.assertEqual(k, 1 / (0.99**2 * (2 * m.opt.timestep) ** 2)) - - def test_disableconstraint(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - - m.opt.disableflags = m.opt.disableflags | DisableBit.CONSTRAINT - mx, dx = mjx.device_put(m), mjx.device_put(d) - dx = constraint.make_constraint(mx, dx) + def test_disable_constraint(self): + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONSTRAINT + ne, nf, nl, nc = mjx.count_constraints(m) + self.assertEqual(ne, 0) + self.assertEqual(nf, 0) + self.assertEqual(nl, 0) + self.assertEqual(nc, 0) + dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) self.assertEqual(dx.efc_J.shape[0], 0) def test_disable_equality(self): - m = test_util.load_test_file('equality.xml') - d = mujoco.MjData(m) - - m.opt.disableflags = m.opt.disableflags | DisableBit.EQUALITY - mx, dx = mjx.device_put(m), mjx.device_put(d) - dx = constraint.make_constraint(mx, dx) - self.assertEqual(dx.efc_J.shape[0], 0) + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.EQUALITY + ne, nf, nl, nc = mjx.count_constraints(m) + self.assertEqual(ne, 0) + self.assertEqual(nf, 0) + self.assertEqual(nl, 2) + self.assertEqual(nc, 64) + dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) + self.assertEqual(dx.efc_J.shape[0], 66) # only joint range, contact def test_disable_contact(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - d.qpos[2] = 0.0 - mujoco.mj_forward(m, d) - - m.opt.disableflags = m.opt.disableflags & ~DisableBit.CONTACT - mx, dx = mjx.device_put(m), mjx.device_put(d) - dx = dx.tree_replace( - {'contact.frame': dx.contact.frame.reshape((-1, 3, 3))} - ) - efc = constraint._instantiate_contact(mx, dx) - self.assertIsNotNone(efc) - - m.opt.disableflags = m.opt.disableflags | DisableBit.CONTACT - mx, dx = mjx.device_put(m), mjx.device_put(d) - efc = constraint._instantiate_contact(mx, dx) - self.assertIsNone(efc) + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONTACT + ne, nf, nl, nc = mjx.count_constraints(m) + self.assertEqual(ne, 10) + self.assertEqual(nf, 0) + self.assertEqual(nl, 2) + self.assertEqual(nc, 0) + dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) + self.assertEqual(dx.efc_J.shape[0], 12) # only joint range, limit if __name__ == '__main__': diff --git a/mjx/mujoco/mjx/_src/device.py b/mjx/mujoco/mjx/_src/device.py index 327b89de..2819d9ea 100644 --- a/mjx/mujoco/mjx/_src/device.py +++ b/mjx/mujoco/mjx/_src/device.py @@ -184,6 +184,11 @@ def device_put(value): Returns: on-device MJX struct reflecting the input value """ + warnings.warn( + 'device_put is deprecated, use put_model and put_data instead', + category=DeprecationWarning, + ) + clz = _TYPE_MAP.get(type(value)) if clz is None: raise NotImplementedError(f'{type(value)} is not supported for device_put.') @@ -242,6 +247,10 @@ def device_get_into(result, value): Raises: RuntimeError: if result length doesn't match data batch size """ + warnings.warn( + 'device_get_into is deprecated, use get_data instead', + category=DeprecationWarning, + ) value = jax.device_get(value) diff --git a/mjx/mujoco/mjx/_src/device_test.py b/mjx/mujoco/mjx/_src/device_test.py index e6eb7562..9f518d91 100644 --- a/mjx/mujoco/mjx/_src/device_test.py +++ b/mjx/mujoco/mjx/_src/device_test.py @@ -130,31 +130,31 @@ class ValidateInputTest(absltest.TestCase): mjx.device_put(m) def test_trn(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_trntype[0] = mujoco.mjtTrn.mjTRN_SITE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_dyn(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_dyntype[0] = mujoco.mjtDyn.mjDYN_MUSCLE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_gain(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_gaintype[0] = mujoco.mjtGain.mjGAIN_MUSCLE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_bias(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_gaintype[0] = mujoco.mjtGain.mjGAIN_MUSCLE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_condim(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('constraints.xml') for i in [1, 4, 6]: m.geom_condim[0] = i with self.assertRaises(NotImplementedError): diff --git a/mjx/mujoco/mjx/_src/forward.py b/mjx/mujoco/mjx/_src/forward.py index 06bec894..827c8f31 100644 --- a/mjx/mujoco/mjx/_src/forward.py +++ b/mjx/mujoco/mjx/_src/forward.py @@ -60,7 +60,7 @@ def named_scope(fn, name: str = ''): @named_scope -def _position(m: Model, d: Data) -> Data: +def fwd_position(m: Model, d: Data) -> Data: """Position-dependent computations.""" # TODO(robotics-simulation): tendon d = smooth.kinematics(m, d) @@ -74,7 +74,7 @@ def _position(m: Model, d: Data) -> Data: @named_scope -def _velocity(m: Model, d: Data) -> Data: +def fwd_velocity(m: Model, d: Data) -> Data: """Velocity-dependent computations.""" d = d.replace(actuator_velocity=d.actuator_moment @ d.qvel) d = smooth.com_vel(m, d) @@ -84,7 +84,7 @@ def _velocity(m: Model, d: Data) -> Data: @named_scope -def _actuation(m: Model, d: Data) -> Data: +def fwd_actuation(m: Model, d: Data) -> Data: """Actuation-dependent computations.""" if not m.nu or m.opt.disableflags & DisableBit.ACTUATION: return d.replace( @@ -190,7 +190,7 @@ def _actuation(m: Model, d: Data) -> Data: @named_scope -def _acceleration(m: Model, d: Data) -> Data: +def fwd_acceleration(m: Model, d: Data) -> Data: """Add up all non-constraint forces, compute qacc_smooth.""" qfrc_applied = d.qfrc_applied + support.xfrc_accumulate(m, d) qfrc_smooth = d.qfrc_passive - d.qfrc_bias + d.qfrc_actuator + qfrc_applied @@ -263,7 +263,7 @@ def _advance( @named_scope -def _euler(m: Model, d: Data) -> Data: +def euler(m: Model, d: Data) -> Data: """Euler integrator, semi-implicit in velocity.""" # integrate damping implicitly qacc = d.qacc @@ -277,7 +277,7 @@ def _euler(m: Model, d: Data) -> Data: @named_scope -def _rungekutta4(m: Model, d: Data) -> Data: +def rungekutta4(m: Model, d: Data) -> Data: """Runge-Kutta explicit order 4 integrator.""" d_t0 = d # pylint: disable=invalid-name @@ -323,10 +323,10 @@ def _rungekutta4(m: Model, d: Data) -> Data: @named_scope def forward(m: Model, d: Data) -> Data: """Forward dynamics.""" - d = _position(m, d) - d = _velocity(m, d) - d = _actuation(m, d) - d = _acceleration(m, d) + d = fwd_position(m, d) + d = fwd_velocity(m, d) + d = fwd_actuation(m, d) + d = fwd_acceleration(m, d) if d.efc_J.size == 0: d = d.replace(qacc=d.qacc_smooth) @@ -343,9 +343,9 @@ def step(m: Model, d: Data) -> Data: d = forward(m, d) if m.opt.integrator == IntegratorType.EULER: - d = _euler(m, d) + d = euler(m, d) elif m.opt.integrator == IntegratorType.RK4: - d = _rungekutta4(m, d) + d = rungekutta4(m, d) else: raise NotImplementedError(f'integrator {m.opt.integrator} not implemented.') diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index 40da667c..fbe28c9d 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -15,77 +15,75 @@ """Tests for forward functions.""" from absl.testing import absltest -from absl.testing import parameterized import jax -from jax import numpy as jp import mujoco from mujoco import mjx -from mujoco.mjx._src import forward from mujoco.mjx._src import test_util -# pylint: disable=g-importing-member -from mujoco.mjx._src.types import DisableBit -# pylint: enable=g-importing-member import numpy as np -def _assert_attr_eq(a, b, attr, step, fname, atol=1e-3, rtol=1e-3): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX forward calculations - mostly +# due to float precision +_TOLERANCE = 1e-5 -class ForwardTest(parameterized.TestCase): +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - @parameterized.parameters( - filter(lambda s: s not in ('equality.xml',), test_util.TEST_FILES) - ) - def test_forward(self, fname): - """Test mujoco mj forward function matches mujoco_mjx forward function.""" - np.random.seed(test_util.TEST_FILES.index(fname)) - m = test_util.load_test_file(fname) +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) + + +class ForwardTest(absltest.TestCase): + + def test_forward(self): + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mx = mjx.device_put(m) - dx = mjx.make_data(mx) - forward_jit_fn = jax.jit(mjx.forward) + # apply some control and xfrc input + d.ctrl = np.array([-18, 0.59, 0.47]) + d.xfrc_applied[0, 2] = 0.1 # torque + d.xfrc_applied[1, 4] = 0.3 # linear force + mujoco.mj_step(m, d, 100) # get some dynamics going + mujoco.mj_forward(m, d) - # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.random(m.nv) * 0.05 - for i in range(100): - qpos, qvel = d.qpos.copy(), d.qvel.copy() - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx.replace(qpos=qpos, qvel=qvel)) + mx = mjx.put_model(m) - _assert_attr_eq(d, dx, 'qfrc_smooth', i, fname) - _assert_attr_eq(d, dx, 'qacc_smooth', i, fname) + # fwd_actuation + dx = jax.jit(mjx.fwd_actuation)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'act_dot') + _assert_attr_eq(d, dx, 'qfrc_actuator') - @parameterized.parameters( - filter(lambda s: s not in ('equality.xml',), test_util.TEST_FILES) - ) - def test_step(self, fname): - """Test mujoco mj step matches mujoco_mjx step.""" - np.random.seed(test_util.TEST_FILES.index(fname)) - m = test_util.load_test_file(fname) - step_jit_fn = jax.jit(forward.step) + # fwd_accleration (fwd_position and fwd_velocity already tested elsewhere) + dx = jax.jit(mjx.fwd_acceleration)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_smooth') + _assert_attr_eq(d, dx, 'qacc_smooth') - mx = mjx.device_put(m) + # euler + dx = jax.jit(mjx.euler)(mx, mjx.put_data(m, d)) + mujoco.mj_Euler(m, d) + _assert_attr_eq(d, dx, 'act') + _assert_attr_eq(d, dx, 'qpos') + _assert_attr_eq(d, dx, 'time') + + def test_step(self): + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.normal(m.nv) * 0.05 - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - qpos, qvel = d.qpos, d.qvel - d = mujoco.MjData(m) - d.qpos, d.qvel = qpos, qvel - dx = mjx.device_put(d) + # apply some control and xfrc input + d.ctrl = np.array([-18, 0.59, 0.47]) + d.xfrc_applied[0, 2] = 0.1 # torque + d.xfrc_applied[1, 4] = 0.3 # linear force + mujoco.mj_step(m, d, 100) # get some dynamics going - mujoco.mj_step(m, d) - dx = step_jit_fn(mx, dx) - - _assert_attr_eq(d, dx, 'qvel', i, fname, atol=1e-2) - _assert_attr_eq(d, dx, 'qpos', i, fname, atol=1e-2) - _assert_attr_eq(d, dx, 'act', i, fname) - _assert_attr_eq(d, dx, 'time', i, fname) + mx = mjx.put_model(m) + dx = jax.jit(mjx.step)(mx, mjx.put_data(m, d)) + mujoco.mj_step(m, d) + _assert_attr_eq(d, dx, 'act') + _assert_attr_eq(d, dx, 'time') + _assert_attr_eq(d, dx, 'qvel') + _assert_attr_eq(d, dx, 'qpos') def test_rk4(self): m = mujoco.MjModel.from_xml_string(""" @@ -94,7 +92,6 @@ class ForwardTest(parameterized.TestCase): - @@ -107,39 +104,33 @@ class ForwardTest(parameterized.TestCase): """) - step_jit_fn = jax.jit(forward.step) - mx = mjx.device_put(m) d = mujoco.MjData(m) # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.normal(m.nv) * 0.05 - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - qpos, qvel = d.qpos, d.qvel - d = mujoco.MjData(m) - d.qpos, d.qvel = qpos, qvel - dx = mjx.device_put(d) + d.qvel = np.array([0.2, -0.1]) + mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero + mujoco.mj_forward(m, d) - mujoco.mj_step(m, d) - dx = step_jit_fn(mx, dx) + mx = mjx.put_model(m) + dx = jax.jit(mjx.rungekutta4)(mx, mjx.put_data(m, d)) + mujoco.mj_RungeKutta(m, d, 4) - _assert_attr_eq(d, dx, 'qvel', i, 'test_rk4', atol=1e-2) - _assert_attr_eq(d, dx, 'qpos', i, 'test_rk4', atol=1e-2) - _assert_attr_eq(d, dx, 'act', i, 'test_rk4') - _assert_attr_eq(d, dx, 'time', i, 'test_rk4') + _assert_attr_eq(d, dx, 'qvel') + _assert_attr_eq(d, dx, 'qpos') + _assert_attr_eq(d, dx, 'act') + _assert_attr_eq(d, dx, 'time') def test_disable_eulerdamp(self): - m = test_util.load_test_file('ant.xml') - m.opt.disableflags = m.opt.disableflags | DisableBit.EULERDAMP + m = test_util.load_test_file('pendula.xml') + self.assertTrue((m.dof_damping > 0).any()) + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.EULERDAMP d = mujoco.MjData(m) - mx = mjx.device_put(m) - self.assertTrue((mx.dof_damping > 0).any()) - dx = mjx.device_put(d) - dx = jax.jit(forward.forward)(mx, dx) + d.qvel[:] = 1.0 + d.qacc[:] = 1.0 + mx = mjx.put_model(m) + dx = jax.jit(mjx.euler)(mx, mjx.put_data(m, d)) - dx = dx.replace(qvel=jp.ones_like(dx.qvel), qacc=jp.ones_like(dx.qacc)) - dx = jax.jit(forward._euler)(mx, dx) np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 060ffa50..9658ad53 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -77,6 +77,7 @@ def _put_statistic(s: mujoco.MjStatistic, device=None) -> types.Statistic: def put_model(m: mujoco.MjModel, device=None) -> types.Model: """Puts mujoco.MjModel onto a device, resulting in mjx.Model.""" + if m.ntendon: raise NotImplementedError('tendons are not supported') @@ -150,7 +151,7 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data: d = types.Data( solver_niter=jp.array(0, dtype=jp.int32), time=jp.array(0.0), - qpos=m.qpos0, + qpos=jp.array(m.qpos0), qvel=zero_nv, act=zero_na, qacc_warmstart=zero_nv, diff --git a/mjx/mujoco/mjx/_src/passive.py b/mjx/mujoco/mjx/_src/passive.py index 9b0b41f0..268de126 100644 --- a/mjx/mujoco/mjx/_src/passive.py +++ b/mjx/mujoco/mjx/_src/passive.py @@ -76,7 +76,7 @@ def _inertia_box_fluid_model( def passive(m: Model, d: Data) -> Data: """Adds all passive forces.""" if m.opt.disableflags & DisableBit.PASSIVE: - return d + return d.replace(qfrc_passive=jp.zeros(m.nv)) # joint-level springs def fn(jnt_typs, stiffness, qpos_spring, qpos): diff --git a/mjx/mujoco/mjx/_src/passive_test.py b/mjx/mujoco/mjx/_src/passive_test.py index 4a5026c8..6ff5a596 100644 --- a/mjx/mujoco/mjx/_src/passive_test.py +++ b/mjx/mujoco/mjx/_src/passive_test.py @@ -14,100 +14,65 @@ # ============================================================================== """Tests passive forces.""" -import itertools - from absl.testing import absltest -from absl.testing import parameterized -from etils import epath import jax -import jax.numpy as jp import mujoco from mujoco import mjx +from mujoco.mjx._src import test_util import numpy as np - -def _assert_attr_eq(a, b, attr, step, fname, atol=1e-4, rtol=1e-4): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX passive calculations - mostly +# due to float precision +_TOLERANCE = 1e-7 -class PassiveTest(parameterized.TestCase): +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - @parameterized.parameters(enumerate(('ant.xml', 'pendula.xml'))) - def test_stiffness_damping(self, seed, fname): - """Tests stiffness and damping on Ant.""" - np.random.seed(seed) - path = epath.resource_path('mujoco.mjx') / 'test_data' - path /= fname - m = mujoco.MjModel.from_xml_string(path.read_text()) - # set stiffness/damping - m.jnt_stiffness = np.random.uniform(size=m.njnt) - m.dof_damping = np.random.uniform(size=m.nv) +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) + + +class PassiveTest(absltest.TestCase): + + def test_passive(self): + m = test_util.load_test_file('pendula.xml') d = mujoco.MjData(m) - d.qvel = np.random.random(m.nv) # random kick + # 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]) + mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) - mx = mjx.device_put(m) - dx = mjx.make_data(mx) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - passive_jit_fn = jax.jit(mjx.passive) + # test with fluid forces + m.opt.density = 0.01 + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - for i in range(100): - qpos, qvel = d.qpos.copy(), d.qvel.copy() - mujoco.mj_step(m, d) - dx = passive_jit_fn(mx, dx.replace(qpos=qpos, qvel=qvel)) - _assert_attr_eq(d, dx, 'qfrc_passive', i, fname) + m.opt.viscosity = 0.02 + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - @parameterized.parameters( - itertools.product(range(3), ('pendula.xml',)) - ) - def test_fluid(self, seed, fname): - np.random.seed(seed) - path = epath.resource_path('mujoco.mjx') / 'test_data' - path /= fname - m = mujoco.MjModel.from_xml_string(path.read_text()) + m.opt.wind = np.array([0.03, 0.04, 0.05]) + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - # set density/viscosity/wind - m.opt.density = np.random.uniform() - m.opt.viscosity = np.random.uniform() - m.opt.wind = np.random.uniform() - - passive_jit_fn = jax.jit(mjx.passive) - - mx = mjx.device_put(m) - d = mujoco.MjData(m) - d.qvel = np.random.random(m.nv) # random kick - - for i in range(100): - mujoco.mj_step(m, d) - dx = mjx.device_put(d) - mujoco.mj_passive(m, d) - dx = passive_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'qfrc_passive', i, fname) - - def test_disable_passive(self): - m = mujoco.MjModel.from_xml_string(""" - - - - - - - - - - """) - mx = mjx.device_put(m) - d = mujoco.MjData(m) - dx = mjx.device_put(d) - dx = dx.replace(qvel=jp.ones(mx.nv)) - - passive_jit_fn = jax.jit(mjx.passive) - dx = passive_jit_fn(mx, dx) - np.testing.assert_equal(dx.qfrc_passive, np.zeros(mx.nv)) + # test disable passive + mx = mx.tree_replace({'opt.disableflags': mjx.DisableBit.PASSIVE}) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + np.testing.assert_allclose(dx.qfrc_passive, 0) if __name__ == '__main__': diff --git a/mjx/mujoco/mjx/_src/scan_test.py b/mjx/mujoco/mjx/_src/scan_test.py index fe4448ca..456067d6 100644 --- a/mjx/mujoco/mjx/_src/scan_test.py +++ b/mjx/mujoco/mjx/_src/scan_test.py @@ -193,7 +193,7 @@ class ScanTest(absltest.TestCase): """ - def testscan_actuators(self): + def test_scan_actuators(self): """Tests scanning over actuators.""" m = mujoco.MjModel.from_xml_string(self._MULTI_ACT_XML) m = mjx.device_put(m) diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index 624291d8..ba470bed 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -435,40 +435,38 @@ def transmission(m: Model, d: Data) -> Data: if not m.nu: return d - def fn(gear, jnt_typ, m_i, m_j, qpos): + def fn(gear, jnt_typ, m_j, qpos): # handles joint transmissions only if jnt_typ == JointType.FREE: length = jp.zeros(1) moment = gear - m_i = jp.repeat(m_i, 6) m_j = m_j + jp.arange(6) elif jnt_typ == JointType.BALL: - axis, _ = math.quat_to_axis_angle(qpos) - length = jp.dot(axis, gear[:3])[None] + axis, angle = math.quat_to_axis_angle(qpos) + length = jp.dot(axis * angle, gear[:3])[None] moment = gear[:3] - m_i = jp.repeat(m_i, 3) m_j = m_j + jp.arange(3) elif jnt_typ in (JointType.SLIDE, JointType.HINGE): length = qpos * gear[0] moment = gear[:1] - m_i, m_j = m_i[None], m_j[None] + m_j = m_j[None] else: raise RuntimeError(f'unrecognized joint type: {jnt_typ}') - return length, moment, m_i, m_j + moment = jp.zeros((m.nv,)).at[m_j].set(moment) + return length, moment - length, m_val, m_i, m_j = scan.flat( + length, moment = scan.flat( m, fn, - 'ujujq', - 'uvvv', + 'ujjq', + 'uuuu', m.actuator_gear, m.jnt_type, - jp.arange(m.nu), jp.array(m.jnt_dofadr), d.qpos, group_by='u', ) - moment = jp.zeros((m.nu, m.nv)).at[m_i, m_j].set(m_val) length = length.reshape((m.nu,)) + moment = moment.reshape((m.nu, m.nv)) d = d.replace(actuator_length=length, actuator_moment=moment) return d diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index bd9348bb..203cb654 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -15,122 +15,107 @@ """Tests for smooth dynamics functions.""" from absl.testing import absltest -from absl.testing import parameterized import jax from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import test_util -# pylint: disable=g-importing-member -from mujoco.mjx._src.types import DisableBit -# pylint: enable=g-importing-member import numpy as np - -def _assert_eq(a, b, name, step, fname, atol=5e-4, rtol=5e-4): - err_msg = f'mismatch: {name} at step {step} in {fname}' - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX smooth calculations - mostly +# due to float precision +_TOLERANCE = 5e-5 -def _assert_attr_eq(a, b, attr, step, fname, atol=5e-4, rtol=5e-4): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) -class SmoothTest(parameterized.TestCase): +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) - @parameterized.parameters(enumerate(test_util.TEST_FILES)) - def test_smooth(self, seed, fname): - """Tests mujoco mj smooth functions match mujoco_mjx smooth functions.""" - if fname in ('convex.xml', 'equality.xml'): - return - np.random.seed(seed) +class SmoothTest(absltest.TestCase): - m = test_util.load_test_file(fname) + def setUp(self): + super().setUp() + # although we already have generous padding of thresholds, it doesn't hurt + # to also fix the seed to reduce test flakiness + np.random.seed(0) + + def test_smooth(self): + """Tests MJX smooth functions match MuJoCo smooth functions.""" + + m = test_util.load_test_file('pendula.xml') d = mujoco.MjData(m) - - kinematics_jit_fn = jax.jit(mjx.kinematics) - com_pos_jit_fn = jax.jit(mjx.com_pos) - crb_jit_fn = jax.jit(mjx.crb) - factor_m_fn = jax.jit(mjx.factor_m) - com_vel_jit_fn = jax.jit(mjx.com_vel) - rne_jit_fn = jax.jit(mjx.rne) - mul_m_jit_fn = jax.jit(mjx.mul_m) - transmission_jit_fn = jax.jit(mjx.transmission) - - mx = mjx.device_put(m) - dx = mjx.make_data(mx) - # give the system a little kick to ensure we have non-identity rotations d.qvel = np.random.random(m.nv) - for i in range(100): - qpos, qvel = d.qpos.copy(), d.qvel.copy() - mujoco.mj_step(m, d) + mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) - # kinematics - dx = kinematics_jit_fn(mx, dx.replace(qpos=qpos, qvel=qvel)) - _assert_attr_eq(d, dx, 'xanchor', i, fname) - _assert_attr_eq(d, dx, 'xaxis', i, fname) - _assert_attr_eq(d, dx, 'xpos', i, fname) - _assert_attr_eq(d, dx, 'xquat', i, fname) - _assert_eq(d.xmat.reshape((-1, 3, 3)), dx.xmat, 'xmat', i, fname) - _assert_attr_eq(d, dx, 'xipos', i, fname) - _assert_eq(d.ximat.reshape((-1, 3, 3)), dx.ximat, 'ximat', i, fname) - _assert_attr_eq(d, dx, 'geom_xpos', i, fname) - _assert_eq( - d.geom_xmat.reshape((-1, 3, 3)), - dx.geom_xmat, - 'geom_xmat', - i, - fname, - ) + # kinematics + dx = jax.jit(mjx.kinematics)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'xanchor') + _assert_attr_eq(d, dx, 'xaxis') + _assert_attr_eq(d, dx, 'xpos') + _assert_attr_eq(d, dx, 'xquat') + _assert_eq(d.xmat.reshape((-1, 3, 3)), dx.xmat, 'xmat') + _assert_attr_eq(d, dx, 'xipos') + _assert_eq(d.ximat.reshape((-1, 3, 3)), dx.ximat, 'ximat') + _assert_attr_eq(d, dx, 'geom_xpos') + _assert_eq(d.geom_xmat.reshape((-1, 3, 3)), dx.geom_xmat, 'geom_xmat') + _assert_attr_eq(d, dx, 'site_xpos') + _assert_eq(d.site_xmat.reshape((-1, 3, 3)), dx.site_xmat, 'site_xmat') + # com_pos + dx = jax.jit(mjx.com_pos)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'subtree_com') + _assert_attr_eq(d, dx, 'cinert') + _assert_attr_eq(d, dx, 'cdof') + # crb + dx = jax.jit(mjx.crb)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'crb') + _assert_attr_eq(d, dx, 'qM') + # factor_m + dx = mjx.put_data(m, d) + dx = jax.jit(mjx.factor_m)(mx, dx, dx.qM) + _assert_attr_eq(d, dx, 'qLD') + _assert_attr_eq(d, dx, 'qLDiagInv') + # com_vel + dx = jax.jit(mjx.com_vel)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'cvel') + _assert_attr_eq(d, dx, 'cdof_dot') + # rne + dx = jax.jit(mjx.rne)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_bias') + # transmission + dx = jax.jit(mjx.transmission)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'actuator_length') + _assert_attr_eq(d, dx, 'actuator_moment') - # com_pos - dx = com_pos_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'subtree_com', i, fname) - _assert_attr_eq(d, dx, 'cinert', i, fname) - _assert_attr_eq(d, dx, 'cdof', i, fname) + def test_mul_m(self): + m = test_util.load_test_file('pendula.xml') + 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) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + vec = np.random.random(m.nv) + mjx_vec = jax.jit(mjx.mul_m)(mx, dx, jp.array(vec)) + mj_vec = np.zeros(m.nv) + mujoco.mj_mulM(m, d, mj_vec, vec) + _assert_eq(mj_vec, mjx_vec, 'mul_m') - # crb - dx = crb_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'crb', i, fname) - _assert_attr_eq(d, dx, 'qM', i, fname) - - # factor_m - dx = factor_m_fn(mx, dx, dx.qM) - _assert_attr_eq(d, dx, 'qLD', i, fname, atol=1e-3) - _assert_attr_eq(d, dx, 'qLDiagInv', i, fname, atol=1e-3) - - # com_vel - dx = com_vel_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'cvel', i, fname) - _assert_attr_eq(d, dx, 'cdof_dot', i, fname) - - # rne - dx = rne_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'qfrc_bias', i, fname) - - # mul_m (auxilliary function, not part of smooth step) - vec = np.random.random(m.nv) - mjx_vec = mul_m_jit_fn(mx, dx, jp.array(vec)) - mj_vec = np.zeros(m.nv) - mujoco.mj_mulM(m, d, mj_vec, vec) - _assert_eq(mj_vec, mjx_vec, 'mul_m', i, fname) - - # transmission - dx = transmission_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'actuator_length', i, fname) - _assert_attr_eq(d, dx, 'actuator_moment', i, fname) - - -class DisableGravityTest(absltest.TestCase): - - def test_disabled(self): + def test_disable_gravity(self): m = mujoco.MjModel.from_xml_string(""" - @@ -139,63 +124,13 @@ class DisableGravityTest(absltest.TestCase): """) - mx = mjx.device_put(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) - - # test with gravity - step_jit_fn = jax.jit(mjx.step) - dx = step_jit_fn(mx, dx) - np.testing.assert_array_almost_equal( - dx.qpos, np.array([0.0, 0.0, -9.81e-4, 1.0, 0.0, 0.0, 0.0]), decimal=7 - ) - - # test with gravity disabled - mx = mx.tree_replace( - {'opt.disableflags': mx.opt.disableflags | DisableBit.GRAVITY} - ) - dx = mjx.device_put(d) - step_jit_fn = jax.jit(mjx.step) - dx = step_jit_fn(mx, dx) - np.testing.assert_equal( - dx.qpos, np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) - ) - - -class SiteTest(absltest.TestCase): - - def test_site(self): - """Tests that site positions and orientations match MuJoCo.""" - m = mujoco.MjModel.from_xml_string(""" - - - - - - - - - - - - - - - - """) - d = mujoco.MjData(m) - - mx = mjx.device_put(m) - dx = mjx.device_put(d) - mujoco.mj_forward(m, d) - dx = mjx.forward(mx, dx) - - np.testing.assert_array_almost_equal(dx.site_xpos, d.site_xpos) - np.testing.assert_array_almost_equal( - dx.site_xmat, d.site_xmat.reshape((-1, 3, 3)) - ) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + dx = jax.jit(mjx.rne)(mx, dx) + np.testing.assert_allclose(dx.qfrc_bias, 0) if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/solver_test.py b/mjx/mujoco/mjx/_src/solver_test.py index 6a4f7792..d1677ff9 100644 --- a/mjx/mujoco/mjx/_src/solver_test.py +++ b/mjx/mujoco/mjx/_src/solver_test.py @@ -12,119 +12,64 @@ # See the License for the specific language governing permissions and # limitations under the License. # ============================================================================== -"""Tests for forward functions.""" +"""Tests for constraint functions.""" from absl.testing import absltest -from absl.testing import parameterized -from etils import epath import jax import mujoco from mujoco import mjx +from mujoco.mjx._src import test_util import numpy as np -def _assert_attr_eq(a, b, attr, step, fname, atol=1e-2, rtol=1e-2): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX constraint calculations, +# mostly due to float precision +_TOLERANCE = 5e-5 -class Solver64Test(parameterized.TestCase): - """Tests solvers at 64 bit precision.""" +def _assert_eq(a, b, name, tol=_TOLERANCE): + tol = tol * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - def setUp(self): - super().setUp() - jax.config.update('jax_enable_x64', True) - def tearDown(self): - super().tearDown() - jax.config.update('jax_enable_x64', False) +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) - @parameterized.parameters(enumerate(('ant.xml', 'humanoid.xml'))) - def test_cg(self, seed, fname): - """Test mjx cg solver matches mujoco cg solver at 64 bit precision.""" - f = epath.resource_path('mujoco.mjx') / 'test_data' / fname - m = mujoco.MjModel.from_xml_string(f.read_text()) + +class SolverTest(absltest.TestCase): + + def test_solver(self): + """Test solver.""" + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mx = mjx.device_put(m) + mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) - jax.config.update('jax_enable_x64', True) - forward_jit_fn = jax.jit(mjx.forward) + dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qacc_warmstart') + _assert_attr_eq(d, dx, 'qacc') + _assert_attr_eq(d, dx, 'qfrc_constraint') + nnz = dx.efc_J.any(axis=1) + _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') - # give the system a little kick to ensure we have non-identity rotations - np.random.seed(seed) - d.qvel = 0.01 * np.random.random(m.nv) - - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - save = d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth - d = mujoco.MjData(m) - d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth = save - dx = mjx.device_put(d) - - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx) - - # at 64 bits the solutions returned by the two solvers are quite close - self.assertLessEqual(dx.solver_niter[0], d.solver_niter[0]) - _assert_attr_eq(d, dx, 'qfrc_constraint', i, fname) - _assert_attr_eq(d, dx, 'qacc', i, fname) - - -class SolverTest(parameterized.TestCase): - - @parameterized.parameters(enumerate(('ant.xml', 'humanoid.xml'))) - def test_cg(self, seed, fname): - """Test mjx cg solver is close to mj at 32 bit precision. - - Args: - seed: int - fname: file to test - - At lower float resolution there's wiggle room in valid forces that satisfy - constraints. So instead let's mainly validate that mjx is finding solutions - with as good cost as mujoco, even if the resulting forces/accelerations - are not quite the same. - """ - f = epath.resource_path('mujoco.mjx') / 'test_data' / fname - m = mujoco.MjModel.from_xml_string(f.read_text()) - d = mujoco.MjData(m) - mx = mjx.device_put(m) - - forward_jit_fn = jax.jit(mjx.forward) - - # give the system a little kick to ensure we have non-identity rotations - np.random.seed(seed) - d.qvel = 0.01 * np.random.random(m.nv) - - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - save = d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth - d = mujoco.MjData(m) - d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth = save - dx = mjx.device_put(d) - - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx) - - def cost(qacc): - jaref = np.zeros(d.nefc) - mujoco.mj_mulJacVec(m, d, jaref, qacc) - jaref -= d.efc_aref - cost = np.array([0.0]) - mujoco.mj_constraintUpdate(m, d, jaref, cost, 0) - return cost[0] - - cost_mj, cost_mjx = cost(d.qacc), cost(dx.qacc) - - self.assertLessEqual( - cost_mjx, - cost_mj * 1.01, - msg=f'mismatch: {fname} at step {i}, cost too high', - ) - _assert_attr_eq(d, dx, 'qfrc_constraint', i, fname, atol=1e-1, rtol=1e-1) - _assert_attr_eq(d, dx, 'qacc', i, fname, atol=1e-1, rtol=1e-1) + # also test normal CG + m.opt.solver = mujoco.mjtSolver.mjSOL_CG + mujoco.mj_forward(m, d) + dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qacc_warmstart') + _assert_attr_eq(d, dx, 'qacc') + _assert_attr_eq(d, dx, 'qfrc_constraint') + _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') + # without warmstart, the solution is not as close + m.opt.solver = mujoco.mjtSolver.mjSOL_NEWTON + m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_WARMSTART + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=2e-2) if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index fe88fc84..fb3a5389 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -34,8 +34,8 @@ class SupportTest(parameterized.TestCase): m = test_util.load_test_file(fname) d = mujoco.MjData(m) mujoco.mj_step(m, d) - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) point = np.random.randn(3) body = np.random.choice(m.nbody) jacp, jacr = jax.jit(support.jac)(mx, dx, point, body) @@ -49,11 +49,11 @@ class SupportTest(parameterized.TestCase): """Tests that xfrc_accumulate ouput matches mj_xfrcAccumulate.""" np.random.seed(0) - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') d = mujoco.MjData(m) mujoco.mj_step(m, d) - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) self.assertFalse((dx.xipos == 0.0).all()) xfrc = np.random.rand(*dx.xfrc_applied.shape) diff --git a/mjx/mujoco/mjx/_src/test_util.py b/mjx/mujoco/mjx/_src/test_util.py index 8d765644..870621f7 100644 --- a/mjx/mujoco/mjx/_src/test_util.py +++ b/mjx/mujoco/mjx/_src/test_util.py @@ -23,10 +23,8 @@ import mujoco import numpy as np TEST_FILES: List[str] = [ - 'ant.xml', + 'constraints.xml', 'convex.xml', - 'equality.xml', - 'humanoid.xml', 'pendula.xml', ] diff --git a/mjx/mujoco/mjx/integration_test/collision_driver_test.py b/mjx/mujoco/mjx/integration_test/collision_driver_test.py index 1e28b053..a9891656 100644 --- a/mjx/mujoco/mjx/integration_test/collision_driver_test.py +++ b/mjx/mujoco/mjx/integration_test/collision_driver_test.py @@ -58,9 +58,9 @@ class CollisionDriverIntegrationTest(parameterized.TestCase): ) m = mujoco.MjModel.from_xml_string(mjcf) - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) diff --git a/mjx/mujoco/mjx/integration_test/forward_test.py b/mjx/mujoco/mjx/integration_test/forward_test.py index 6fa913c4..67f20371 100644 --- a/mjx/mujoco/mjx/integration_test/forward_test.py +++ b/mjx/mujoco/mjx/integration_test/forward_test.py @@ -19,7 +19,6 @@ from absl.testing import parameterized import jax import mujoco from mujoco import mjx -from mujoco.mjx._src import forward from mujoco.mjx._src import test_util import numpy as np @@ -46,7 +45,7 @@ class ActuationIntegrationTest(parameterized.TestCase): enable_contact=False, ) m = mujoco.MjModel.from_xml_string(mjcf) - actuation_jit_fn = jax.jit(forward._actuation) + actuation_jit_fn = jax.jit(mjx.fwd_actuation) # init d = mujoco.MjData(m) @@ -57,8 +56,8 @@ class ActuationIntegrationTest(parameterized.TestCase): mujoco.mj_fwdVelocity(m, d) # put on device - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) mujoco.mj_fwdActuation(m, d) dx = actuation_jit_fn(mx, dx) diff --git a/mjx/mujoco/mjx/integration_test/smooth_test.py b/mjx/mujoco/mjx/integration_test/smooth_test.py index c924e005..d032be79 100644 --- a/mjx/mujoco/mjx/integration_test/smooth_test.py +++ b/mjx/mujoco/mjx/integration_test/smooth_test.py @@ -60,8 +60,8 @@ class TransmissionIntegrationTest(parameterized.TestCase): d.qvel = np.random.random(m.nv) # put on device - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) mujoco.mj_transmission(m, d) dx = transmission_jit_fn(mx, dx) diff --git a/mjx/mujoco/mjx/test_data/ant.xml b/mjx/mujoco/mjx/test_data/ant.xml deleted file mode 100644 index 7417c3ae..00000000 --- a/mjx/mujoco/mjx/test_data/ant.xml +++ /dev/null @@ -1,82 +0,0 @@ - - - diff --git a/mjx/mujoco/mjx/test_data/constraints.xml b/mjx/mujoco/mjx/test_data/constraints.xml new file mode 100644 index 00000000..52eb95d8 --- /dev/null +++ b/mjx/mujoco/mjx/test_data/constraints.xml @@ -0,0 +1,53 @@ + + + diff --git a/mjx/mujoco/mjx/test_data/equality.xml b/mjx/mujoco/mjx/test_data/equality.xml deleted file mode 100644 index e5c9d184..00000000 --- a/mjx/mujoco/mjx/test_data/equality.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mjx/mujoco/mjx/test_data/humanoid.xml b/mjx/mujoco/mjx/test_data/humanoid.xml deleted file mode 100644 index 2d7158ee..00000000 --- a/mjx/mujoco/mjx/test_data/humanoid.xml +++ /dev/null @@ -1,109 +0,0 @@ - - - - - - - - diff --git a/mjx/mujoco/mjx/test_data/pendula.xml b/mjx/mujoco/mjx/test_data/pendula.xml index 2363a3f8..0dc476ab 100644 --- a/mjx/mujoco/mjx/test_data/pendula.xml +++ b/mjx/mujoco/mjx/test_data/pendula.xml @@ -18,6 +18,8 @@ + + @@ -26,45 +28,49 @@ - + + - + + - + + - - + + - + - + - + - + + @@ -72,14 +78,14 @@ - + - + - + @@ -89,14 +95,21 @@ - + - + - + + + + + + + +