Allow for different MJX backend implementations.

PiperOrigin-RevId: 755935704
Change-Id: Ic135cd00137c2857c73c683ed9fdc5ac4418715d
This commit is contained in:
Baruch Tabanpour
2025-05-07 11:27:57 -07:00
committed by Copybara-Service
parent 421c487d9d
commit 6cfea71985
24 changed files with 2182 additions and 1504 deletions
+6 -1
View File
@@ -28,8 +28,10 @@ from mujoco.mjx._src.collision_types import FunctionKey
from mujoco.mjx._src.collision_types import GeomInfo from mujoco.mjx._src.collision_types import GeomInfo
from mujoco.mjx._src.collision_types import HFieldInfo from mujoco.mjx._src.collision_types import HFieldInfo
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import GeomType from mujoco.mjx._src.types import GeomType
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
_GeomInfo = Union[GeomInfo, ConvexInfo] _GeomInfo = Union[GeomInfo, ConvexInfo]
@@ -42,6 +44,9 @@ def collider(ncon: int):
def collide( def collide(
m: Model, d: Data, key: FunctionKey, geom: jax.Array m: Model, d: Data, key: FunctionKey, geom: jax.Array
) -> Collision: ) -> Collision:
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('collider requires JAX backend implementation.')
g1, g2 = geom.T g1, g2 = geom.T
infos = [ infos = [
GeomInfo(d.geom_xpos[g1], d.geom_xmat[g1], m.geom_size[g1]), GeomInfo(d.geom_xpos[g1], d.geom_xmat[g1], m.geom_size[g1]),
@@ -56,7 +61,7 @@ def collider(ncon: int):
pos=0, mat=0, size=0, face=0, vert=0 pos=0, mat=0, size=0, face=0, vert=0
) )
elif key.types[i] == GeomType.MESH: elif key.types[i] == GeomType.MESH:
c, cm = infos[i], m.mesh_convex[key.data_ids[i]] c, cm = infos[i], m._impl.mesh_convex[key.data_ids[i]]
infos[i] = ConvexInfo(**vars(c), **vars(cm)) infos[i] = ConvexInfo(**vars(c), **vars(cm))
in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace( in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace(
pos=0, mat=0, size=0 pos=0, mat=0, size=0
+10 -5
View File
@@ -71,9 +71,11 @@ from mujoco.mjx._src.collision_sdf import sphere_ellipsoid
from mujoco.mjx._src.collision_types import FunctionKey from mujoco.mjx._src.collision_types import FunctionKey
from mujoco.mjx._src.types import Contact from mujoco.mjx._src.types import Contact
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import DisableBit
from mujoco.mjx._src.types import GeomType from mujoco.mjx._src.types import GeomType
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
import numpy as np import numpy as np
@@ -227,7 +229,7 @@ def _geom_groups(
if types[0] == mujoco.mjtGeom.mjGEOM_HFIELD: if types[0] == mujoco.mjtGeom.mjGEOM_HFIELD:
# add static grid bounds to the grouping key for hfield collisions # add static grid bounds to the grouping key for hfield collisions
geom_rbound_hfield = ( geom_rbound_hfield = (
m.geom_rbound_hfield if isinstance(m, Model) else m.geom_rbound m._impl.geom_rbound_hfield if isinstance(m, Model) else m.geom_rbound # pytype: disable=attribute-error
) )
nrow, ncol = m.hfield_nrow[data_ids[0]], m.hfield_ncol[data_ids[0]] nrow, ncol = m.hfield_nrow[data_ids[0]], m.hfield_ncol[data_ids[0]]
xsize, ysize = m.hfield_size[data_ids[0]][:2] xsize, ysize = m.hfield_size[data_ids[0]][:2]
@@ -323,11 +325,11 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]:
solref=solref, solref=solref,
solreffriction=solreffriction, solreffriction=solreffriction,
solimp=solimp, solimp=solimp,
dim=d.contact.dim, dim=d._impl.contact.dim, # pytype: disable=attribute-error
geom1=jp.array(geom[:, 0]), geom1=jp.array(geom[:, 0]),
geom2=jp.array(geom[:, 1]), geom2=jp.array(geom[:, 1]),
geom=jp.array(geom[:, :2]), geom=jp.array(geom[:, :2]),
efc_address=d.contact.efc_address, efc_address=d._impl.contact.efc_address, # pytype: disable=attribute-error
) )
return groups return groups
@@ -374,7 +376,10 @@ def make_condim(m: Union[Model, mujoco.MjModel]) -> np.ndarray:
def collision(m: Model, d: Data) -> Data: def collision(m: Model, d: Data) -> Data:
"""Collides geometries.""" """Collides geometries."""
if d.ncon == 0: if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('collision requires JAX backend implementation.')
if d._impl.ncon == 0: # pytype: disable=attribute-error
return d return d
max_geom_pairs = _numeric(m, 'max_geom_pairs') max_geom_pairs = _numeric(m, 'max_geom_pairs')
@@ -424,4 +429,4 @@ def collision(m: Model, d: Data) -> Data:
contacts = sum([condim_groups[k] for k in sorted(condim_groups)], []) contacts = sum([condim_groups[k] for k in sorted(condim_groups)], [])
contact = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *contacts) contact = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *contacts)
return d.replace(contact=contact) return d.replace(_impl=d._impl.replace(contact=contact)) # pytype: disable=attribute-error
+99 -87
View File
@@ -119,7 +119,7 @@ class SphereCollisionTest(parameterized.TestCase):
def test_sphere(self, name, mjcf): def test_sphere(self, name, mjcf):
d, dx = _collide(mjcf) d, dx = _collide(mjcf)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, name, 1e-5) _assert_attr_eq(dx._impl.contact, d.contact, field.name, name, 1e-5)
_SPHERE_CONVEX = """ _SPHERE_CONVEX = """
<mujoco> <mujoco>
@@ -143,7 +143,7 @@ class SphereCollisionTest(parameterized.TestCase):
) )
d, dx = _collide(xml) d, dx = _collide(xml)
self.assertEmpty(d.contact.dist) self.assertEmpty(d.contact.dist)
self.assertGreater(dx.contact.dist, 0) self.assertGreater(dx._impl.contact.dist, 0)
# face contact # face contact
xml = self._SPHERE_CONVEX.replace( xml = self._SPHERE_CONVEX.replace(
@@ -151,38 +151,38 @@ class SphereCollisionTest(parameterized.TestCase):
) )
d, dx = _collide(xml) d, dx = _collide(xml)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'face', 1e-4) _assert_attr_eq(dx._impl.contact, d.contact, field.name, 'face', 1e-4)
# deep face contact # deep face contact
xml = self._SPHERE_CONVEX.replace( xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.48 0 0.47">' '<body pos="0.52 0 0.52">', '<body pos="0.48 0 0.47">'
) )
d, dx = _collide(xml) d, dx = _collide(xml)
self.assertTrue((dx.contact.dist < 0).all()) self.assertTrue((dx._impl.contact.dist < 0).all())
self.assertTrue((d.contact.dist < 0).all()) self.assertTrue((d.contact.dist < 0).all())
np.testing.assert_allclose(dx.contact.dist, [-0.07], atol=1e-5) np.testing.assert_allclose(dx._impl.contact.dist, [-0.07], atol=1e-5)
np.testing.assert_array_almost_equal(dx.contact.pos, d.contact.pos) np.testing.assert_array_almost_equal(dx._impl.contact.pos, d.contact.pos)
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.frame, d.contact.frame.reshape((-1, 3, 3)) dx._impl.contact.frame, d.contact.frame.reshape((-1, 3, 3))
) )
def test_sphere_convex_edge(self): def test_sphere_convex_edge(self):
# edge contact # edge contact
d, dx = _collide(self._SPHERE_CONVEX) d, dx = _collide(self._SPHERE_CONVEX)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'edge', 1e-4) _assert_attr_eq(dx._impl.contact, d.contact, field.name, 'edge', 1e-4)
# deep edge penetration # deep edge penetration
xml = self._SPHERE_CONVEX.replace( xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.49 0 0.49">' '<body pos="0.52 0 0.52">', '<body pos="0.49 0 0.49">'
) )
d, dx = _collide(xml) d, dx = _collide(xml)
self.assertTrue((dx.contact.dist < 0).all()) self.assertTrue((dx._impl.contact.dist < 0).all())
self.assertTrue((d.contact.dist < 0).all()) self.assertTrue((d.contact.dist < 0).all())
np.testing.assert_allclose(dx.contact.dist, [-0.06], atol=1e-5) np.testing.assert_allclose(dx._impl.contact.dist, [-0.06], atol=1e-5)
np.testing.assert_array_almost_equal(dx.contact.pos, d.contact.pos) np.testing.assert_array_almost_equal(dx._impl.contact.pos, d.contact.pos)
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.frame, d.contact.frame.reshape((-1, 3, 3)) dx._impl.contact.frame, d.contact.frame.reshape((-1, 3, 3))
) )
# vertex contact # vertex contact
@@ -191,7 +191,7 @@ class SphereCollisionTest(parameterized.TestCase):
) )
d, dx = _collide(xml) d, dx = _collide(xml)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'vertex', 1e-4) _assert_attr_eq(dx._impl.contact, d.contact, field.name, 'vertex', 1e-4)
# sphere center on vertex # sphere center on vertex
xml = self._SPHERE_CONVEX.replace( xml = self._SPHERE_CONVEX.replace(
@@ -199,7 +199,9 @@ class SphereCollisionTest(parameterized.TestCase):
) )
d, dx = _collide(xml) d, dx = _collide(xml)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'vertex_center', 1e-4) _assert_attr_eq(
dx._impl.contact, d.contact, field.name, 'vertex_center', 1e-4
)
class EllipsoidCollisionTest(parameterized.TestCase): class EllipsoidCollisionTest(parameterized.TestCase):
@@ -219,10 +221,10 @@ class EllipsoidCollisionTest(parameterized.TestCase):
def test_plane_ellipsoid(self): def test_plane_ellipsoid(self):
"""Tests ellipsoid plane contact.""" """Tests ellipsoid plane contact."""
d, dx = _collide(self._ELLIPSOID_PLANE) d, dx = _collide(self._ELLIPSOID_PLANE)
self.assertLess(dx.contact.dist[0], 0) self.assertLess(dx._impl.contact.dist[0], 0)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq( _assert_attr_eq(
dx.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5 dx._impl.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5
) )
_ELLIPSOID_ELLIPSOID = """ _ELLIPSOID_ELLIPSOID = """
@@ -242,10 +244,10 @@ class EllipsoidCollisionTest(parameterized.TestCase):
def test_ellipsoid_ellipsoid(self): def test_ellipsoid_ellipsoid(self):
"""Tests ellipsoid ellipsoid contact.""" """Tests ellipsoid ellipsoid contact."""
d, dx = _collide(self._ELLIPSOID_ELLIPSOID) d, dx = _collide(self._ELLIPSOID_ELLIPSOID)
self.assertLess(dx.contact.dist[0], 0) self.assertLess(dx._impl.contact.dist[0], 0)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq( _assert_attr_eq(
dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-2 dx._impl.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-2
) )
_ELLIPSOID_SPHERE = """ _ELLIPSOID_SPHERE = """
@@ -266,10 +268,10 @@ class EllipsoidCollisionTest(parameterized.TestCase):
"""Tests ellipsoid capsule contact.""" """Tests ellipsoid capsule contact."""
d, dx = _collide(self._ELLIPSOID_SPHERE) d, dx = _collide(self._ELLIPSOID_SPHERE)
d.contact.pos[0][2] = 0.03 # MJX finds the point on the surface d.contact.pos[0][2] = 0.03 # MJX finds the point on the surface
self.assertLess(dx.contact.dist[0], 0) self.assertLess(dx._impl.contact.dist[0], 0)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq( _assert_attr_eq(
dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-4 dx._impl.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-4
) )
_ELLIPSOID_CAPSULE = """ _ELLIPSOID_CAPSULE = """
@@ -289,10 +291,10 @@ class EllipsoidCollisionTest(parameterized.TestCase):
def test_capsule_ellipsoid(self): def test_capsule_ellipsoid(self):
"""Tests ellipsoid capsule contact.""" """Tests ellipsoid capsule contact."""
d, dx = _collide(self._ELLIPSOID_CAPSULE) d, dx = _collide(self._ELLIPSOID_CAPSULE)
self.assertLess(dx.contact.dist[0], 0) self.assertLess(dx._impl.contact.dist[0], 0)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq( _assert_attr_eq(
dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-5 dx._impl.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-5
) )
_ELLIPSOID_CYLINDER = """ _ELLIPSOID_CYLINDER = """
@@ -313,10 +315,10 @@ class EllipsoidCollisionTest(parameterized.TestCase):
"""Tests ellipsoid cylinder contact.""" """Tests ellipsoid cylinder contact."""
d, dx = _collide(self._ELLIPSOID_CYLINDER) d, dx = _collide(self._ELLIPSOID_CYLINDER)
d.contact.pos[0][2] = 0.04 # MJX finds the deepest point on the surface d.contact.pos[0][2] = 0.04 # MJX finds the deepest point on the surface
self.assertLess(dx.contact.dist[0], 0) self.assertLess(dx._impl.contact.dist[0], 0)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq( _assert_attr_eq(
dx.contact, d.contact, field.name, 'ellipsoid-cylinder', 1e-4 dx._impl.contact, d.contact, field.name, 'ellipsoid-cylinder', 1e-4
) )
@@ -357,7 +359,7 @@ class CapsuleCollisionTest(parameterized.TestCase):
def test_capsule(self, name, mjcf): def test_capsule(self, name, mjcf):
d, dx = _collide(mjcf) d, dx = _collide(mjcf)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, name, 1e-4) _assert_attr_eq(dx._impl.contact, d.contact, field.name, name, 1e-4)
_PARALLEL_CAP = """ _PARALLEL_CAP = """
<mujoco> <mujoco>
@@ -378,14 +380,14 @@ class CapsuleCollisionTest(parameterized.TestCase):
"""Tests that two parallel capsules are colliding at the midpoint.""" """Tests that two parallel capsules are colliding at the midpoint."""
_, dx = _collide(self._PARALLEL_CAP) _, dx = _collide(self._PARALLEL_CAP)
np.testing.assert_allclose(dx.contact.dist, -0.05) np.testing.assert_allclose(dx._impl.contact.dist, -0.05)
np.testing.assert_allclose( np.testing.assert_allclose(
dx.contact.pos[0], dx._impl.contact.pos[0],
np.array([0.0, 0.1, (0.15 + 0.2) / 2.0]), np.array([0.0, 0.1, (0.15 + 0.2) / 2.0]),
atol=1e-5, atol=1e-5,
) )
np.testing.assert_allclose( np.testing.assert_allclose(
dx.contact.frame[0, 0, :], np.array([0, 0.0, -1.0]), atol=1e-5 dx._impl.contact.frame[0, 0, :], np.array([0, 0.0, -1.0]), atol=1e-5
) )
_CAP_BOX = """ _CAP_BOX = """
@@ -408,30 +410,32 @@ class CapsuleCollisionTest(parameterized.TestCase):
d, dx = _collide(self._CAP_BOX) d, dx = _collide(self._CAP_BOX)
# sort positions for comparison # sort positions for comparison
idx = np.lexsort((dx.contact.pos[:, 0], dx.contact.pos[:, 1])) idx = np.lexsort((dx._impl.contact.pos[:, 0], dx._impl.contact.pos[:, 1]))
dx = dx.tree_replace({'contact.pos': dx.contact.pos[idx]}) dx = dx.tree_replace({'_impl.contact.pos': dx._impl.contact.pos[idx]})
idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1])) idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1]))
d.contact.pos[:] = d.contact.pos[idx] d.contact.pos[:] = d.contact.pos[idx]
d.contact.frame[:] = d.contact.frame[idx] d.contact.frame[:] = d.contact.frame[idx]
d.contact.dist[:] = d.contact.dist[idx] d.contact.dist[:] = d.contact.dist[idx]
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'capsule_convex', 1e-4) _assert_attr_eq(
dx._impl.contact, d.contact, field.name, 'capsule_convex', 1e-4
)
def test_capsule_convex_face_deep(self): def test_capsule_convex_face_deep(self):
"""Tests deep face penetration.""" """Tests deep face penetration."""
xml = self._CAP_BOX.replace('<body pos="0 0 0.54">', '<body pos="0 0 0.4">') xml = self._CAP_BOX.replace('<body pos="0 0 0.54">', '<body pos="0 0 0.4">')
_, dx = _collide(xml) _, dx = _collide(xml)
self.assertTrue((dx.contact.dist < 0).all()) self.assertTrue((dx._impl.contact.dist < 0).all())
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.pos, np.array([[0.5, 0, 0.425], [-0.4, 0, 0.425]]) dx._impl.contact.pos, np.array([[0.5, 0, 0.425], [-0.4, 0, 0.425]])
) )
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.dist, np.array([-0.15, -0.15]) dx._impl.contact.dist, np.array([-0.15, -0.15])
) )
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.frame[:, 0], np.array([[0, 0, -1]] * 2) dx._impl.contact.frame[:, 0], np.array([[0, 0, -1]] * 2)
) )
_CAP_EDGE_BOX = """ _CAP_EDGE_BOX = """
@@ -453,11 +457,11 @@ class CapsuleCollisionTest(parameterized.TestCase):
"""Tests edge contact.""" """Tests edge contact."""
d, dx = _collide(self._CAP_EDGE_BOX) d, dx = _collide(self._CAP_EDGE_BOX)
c = dx.contact c = dx._impl.contact
self.assertEqual(c.pos.shape[0], 2) self.assertEqual(c.pos.shape[0], 2)
self.assertGreater(c.dist[1], 0) self.assertGreater(c.dist[1], 0)
# extract the contact point with penetration # extract the contact point with penetration
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact) c = jax.tree_util.tree_map(lambda x: x[:1], dx._impl.contact)
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1]) c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'capsule_convex_edge', 1e-4) _assert_attr_eq(c, d.contact, field.name, 'capsule_convex_edge', 1e-4)
@@ -469,13 +473,17 @@ class CapsuleCollisionTest(parameterized.TestCase):
) )
_, dx = _collide(xml) _, dx = _collide(xml)
np.testing.assert_array_equal(dx.contact.dist < 0, np.array([True, False])) np.testing.assert_array_equal(
np.testing.assert_array_almost_equal(dx.contact.dist[0], np.array([-0.13])) dx._impl.contact.dist < 0, np.array([True, False])
np.testing.assert_array_almost_equal(
dx.contact.pos[0], np.array([0.5, 0, 0.435]), decimal=3
) )
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.frame[0, 0], np.array([0, 0, -1]), decimal=3 dx._impl.contact.dist[0], np.array([-0.13])
)
np.testing.assert_array_almost_equal(
dx._impl.contact.pos[0], np.array([0.5, 0, 0.435]), decimal=3
)
np.testing.assert_array_almost_equal(
dx._impl.contact.frame[0, 0], np.array([0, 0, -1]), decimal=3
) )
def test_capsule_convex_edge_shallow_tip(self): def test_capsule_convex_edge_shallow_tip(self):
@@ -489,16 +497,16 @@ class CapsuleCollisionTest(parameterized.TestCase):
xml = xml.replace('<body pos="0.5 0 0.55"', '<body pos="0.58 0 0.55"') xml = xml.replace('<body pos="0.5 0 0.55"', '<body pos="0.58 0 0.55"')
d, dx = _collide(xml) d, dx = _collide(xml)
c = dx.contact c = dx._impl.contact
self.assertEqual(c.pos.shape[0], 2) self.assertEqual(c.pos.shape[0], 2)
self.assertGreater(c.dist[1], 0) self.assertGreater(c.dist[1], 0)
# extract the contact point with penetration # extract the contact point with penetration
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact) c = jax.tree_util.tree_map(lambda x: x[:1], dx._impl.contact)
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1]) c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip1', 1e-4) _assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip1', 1e-4)
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.frame[0][0, :3], np.array([-0.43952, 0.0, -0.898233]) dx._impl.contact.frame[0][0, :3], np.array([-0.43952, 0.0, -0.898233])
) )
# the capsule sphere is outside the edge voronoi region, so there is a # the capsule sphere is outside the edge voronoi region, so there is a
@@ -510,16 +518,16 @@ class CapsuleCollisionTest(parameterized.TestCase):
xml = xml.replace('<body pos="0.5 0 0.55"', '<body pos="0.5 0 0.52"') xml = xml.replace('<body pos="0.5 0 0.55"', '<body pos="0.5 0 0.52"')
d, dx = _collide(xml) d, dx = _collide(xml)
c = dx.contact c = dx._impl.contact
self.assertEqual(c.pos.shape[0], 2) self.assertEqual(c.pos.shape[0], 2)
self.assertGreater(c.dist[1], 0) self.assertGreater(c.dist[1], 0)
# extract the contact point with penetration # extract the contact point with penetration
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact) c = jax.tree_util.tree_map(lambda x: x[:1], dx._impl.contact)
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1]) c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip2', 1e-4) _assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip2', 1e-4)
np.testing.assert_array_almost_equal( np.testing.assert_array_almost_equal(
dx.contact.frame[0][0, :3], np.array([0.0, 0.0, -1.0]) dx._impl.contact.frame[0][0, :3], np.array([0.0, 0.0, -1.0])
) )
@@ -542,17 +550,17 @@ class CylinderTest(absltest.TestCase):
d, dx = _collide(self._CYLINDER_PLANE) d, dx = _collide(self._CYLINDER_PLANE)
# cylinder is lying flat # cylinder is lying flat
np.testing.assert_array_less(dx.contact.dist[:2], 0) np.testing.assert_array_less(dx._impl.contact.dist[:2], 0)
np.testing.assert_array_less(-dx.contact.dist[2:], 0) np.testing.assert_array_less(-dx._impl.contact.dist[2:], 0)
# sort position for comparison # sort position for comparison
idx = np.lexsort((dx.contact.pos[:, 0], dx.contact.pos[:, 1])) idx = np.lexsort((dx._impl.contact.pos[:, 0], dx._impl.contact.pos[:, 1]))
dx = dx.tree_replace({'contact.pos': dx.contact.pos[idx]}) dx = dx.tree_replace({'_impl.contact.pos': dx._impl.contact.pos[idx]})
idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1])) idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1]))
d.contact.pos[:] = d.contact.pos[idx] d.contact.pos[:] = d.contact.pos[idx]
# extract the contact points with penetration # extract the contact points with penetration
c = jax.tree_util.tree_map(lambda x: x[:2], dx.contact) c = jax.tree_util.tree_map(lambda x: x[:2], dx._impl.contact)
c = c.replace(dim=c.dim[:2], efc_address=c.efc_address[:2]) c = c.replace(dim=c.dim[:2], efc_address=c.efc_address[:2])
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'cylinder_plane', 1e-5) _assert_attr_eq(c, d.contact, field.name, 'cylinder_plane', 1e-5)
@@ -564,9 +572,11 @@ class CylinderTest(absltest.TestCase):
xml = xml.replace('pos="0 0 0.04"', 'pos="0 0 0.095"') xml = xml.replace('pos="0 0 0.04"', 'pos="0 0 0.095"')
d, dx = _collide(xml) d, dx = _collide(xml)
np.testing.assert_array_less(dx.contact.dist, 0) np.testing.assert_array_less(dx._impl.contact.dist, 0)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'cylinder_plane', 1e-5) _assert_attr_eq(
dx._impl.contact, d.contact, field.name, 'cylinder_plane', 1e-5
)
_SPHERE_CYLINDER = """ _SPHERE_CYLINDER = """
<mujoco> <mujoco>
@@ -586,10 +596,10 @@ class CylinderTest(absltest.TestCase):
"""Tests sphere cylinder contact.""" """Tests sphere cylinder contact."""
d, dx = _collide(self._SPHERE_CYLINDER) d, dx = _collide(self._SPHERE_CYLINDER)
d.contact.pos[0][2] = 0.05 # MJX finds the deepest point on the surface d.contact.pos[0][2] = 0.05 # MJX finds the deepest point on the surface
self.assertLess(dx.contact.dist[0], 0) self.assertLess(dx._impl.contact.dist[0], 0)
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq( _assert_attr_eq(
dx.contact, d.contact, field.name, 'sphere-cylinder', 1e-4 dx._impl.contact, d.contact, field.name, 'sphere-cylinder', 1e-4
) )
@@ -612,11 +622,11 @@ class ConvexTest(absltest.TestCase):
"""Tests box collision with a plane.""" """Tests box collision with a plane."""
d, dx = _collide(self._BOX_PLANE) d, dx = _collide(self._BOX_PLANE)
np.testing.assert_array_less(dx.contact.dist[:2], 0) np.testing.assert_array_less(dx._impl.contact.dist[:2], 0)
np.testing.assert_array_less(-dx.contact.dist[2:], 0) np.testing.assert_array_less(-dx._impl.contact.dist[2:], 0)
# extract the contact points with penetration # extract the contact points with penetration
c = jax.tree_util.tree_map( c = jax.tree_util.tree_map(
lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx._impl.contact
) )
c = c.replace(dim=c.dim[[0, 1]], efc_address=c.efc_address[[0, 1]]) c = c.replace(dim=c.dim[[0, 1]], efc_address=c.efc_address[[0, 1]])
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
@@ -638,16 +648,18 @@ class ConvexTest(absltest.TestCase):
"""Tests box collision with a plane.""" """Tests box collision with a plane."""
d, dx = _collide(self._FLAT_BOX_PLANE) d, dx = _collide(self._FLAT_BOX_PLANE)
np.testing.assert_array_less(dx.contact.dist, 0) np.testing.assert_array_less(dx._impl.contact.dist, 0)
# sort positions for comparison # sort positions for comparison
idx = np.lexsort((dx.contact.pos[:, 0], dx.contact.pos[:, 1])) idx = np.lexsort((dx._impl.contact.pos[:, 0], dx._impl.contact.pos[:, 1]))
dx = dx.tree_replace({'contact.pos': dx.contact.pos[idx]}) dx = dx.tree_replace({'_impl.contact.pos': dx._impl.contact.pos[idx]})
idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1])) idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1]))
d.contact.pos[:] = d.contact.pos[idx] d.contact.pos[:] = d.contact.pos[idx]
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'flat_box_plane', 1e-5) _assert_attr_eq(
dx._impl.contact, d.contact, field.name, 'flat_box_plane', 1e-5
)
_BOX_BOX = """ _BOX_BOX = """
<mujoco> <mujoco>
@@ -669,7 +681,7 @@ class ConvexTest(absltest.TestCase):
def test_box_box(self): def test_box_box(self):
"""Tests a face contact for a box-box collision.""" """Tests a face contact for a box-box collision."""
d, dx = _collide(self._BOX_BOX, keyframe=0) d, dx = _collide(self._BOX_BOX, keyframe=0)
c = dx.contact c = dx._impl.contact
self.assertEqual(c.pos.shape[0], 4) self.assertEqual(c.pos.shape[0], 4)
np.testing.assert_array_less(c.dist, 0) np.testing.assert_array_less(c.dist, 0)
@@ -701,10 +713,10 @@ class ConvexTest(absltest.TestCase):
d, dx = _collide(self._BOX_BOX_EDGE) d, dx = _collide(self._BOX_BOX_EDGE)
# Only one contact point. # Only one contact point.
np.testing.assert_array_less(dx.contact.dist[:1], 0) np.testing.assert_array_less(dx._impl.contact.dist[:1], 0)
np.testing.assert_array_less(-dx.contact.dist[1:], 0) np.testing.assert_array_less(-dx._impl.contact.dist[1:], 0)
# extract the contact point with penetration # extract the contact point with penetration
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact) c = jax.tree_util.tree_map(lambda x: x[:1], dx._impl.contact)
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1]) c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
for field in dataclasses.fields(Contact): for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'box_box_edge', 1e-2) _assert_attr_eq(c, d.contact, field.name, 'box_box_edge', 1e-2)
@@ -736,7 +748,7 @@ class ConvexTest(absltest.TestCase):
).read_bytes(), ).read_bytes(),
} }
_, dx = _collide(self._CONVEX_CONVEX, assets=assets) _, dx = _collide(self._CONVEX_CONVEX, assets=assets)
c = dx.contact c = dx._impl.contact
# Only one contact point for an edge contact. # Only one contact point for an edge contact.
self.assertLess(c.dist[0], 0) self.assertLess(c.dist[0], 0)
@@ -766,7 +778,7 @@ class ConvexTest(absltest.TestCase):
def test_convex_convex_edge(self): def test_convex_convex_edge(self):
"""Tests convex-convex collisions with edge contact via _sat_gaussmap.""" """Tests convex-convex collisions with edge contact via _sat_gaussmap."""
_, dx = _collide(self._CONVEX_CONVEX_THIN) _, dx = _collide(self._CONVEX_CONVEX_THIN)
c = dx.contact c = dx._impl.contact
# Only one contact point for an edge contact. # Only one contact point for an edge contact.
self.assertLess(c.dist[0], 0) self.assertLess(c.dist[0], 0)
@@ -781,7 +793,7 @@ class ConvexTest(absltest.TestCase):
'pos="0.0 2.0 0.35"', 'pos="0.0 2.0 0"' 'pos="0.0 2.0 0.35"', 'pos="0.0 2.0 0"'
) )
) )
c = dx.contact c = dx._impl.contact
self.assertTrue((c.dist > 0).all()) self.assertTrue((c.dist > 0).all())
@@ -837,8 +849,8 @@ class HFieldTest(absltest.TestCase):
# check that all geoms are colliding with the hfield # check that all geoms are colliding with the hfield
for geom_id in [1, 2, 3]: for geom_id in [1, 2, 3]:
mask = (dx.contact.geom == np.array([0, geom_id])).all(axis=1) mask = (dx._impl.contact.geom == np.array([0, geom_id])).all(axis=1)
c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx.contact) c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx._impl.contact)
self.assertTrue((c.dist < 0).any()) self.assertTrue((c.dist < 0).any())
self.assertTrue((c.dist > -1e-3).any()) self.assertTrue((c.dist > -1e-3).any())
# all contact normals are roughly pointing in the right direction # all contact normals are roughly pointing in the right direction
@@ -850,7 +862,7 @@ class HFieldTest(absltest.TestCase):
for p in positions: for p in positions:
xml = self._HFIELD.replace('<body pos="0 0', f'<body pos="{p}') xml = self._HFIELD.replace('<body pos="0 0', f'<body pos="{p}')
_, dx = _collide(xml) _, dx = _collide(xml)
self.assertTrue((dx.contact.dist >= 0).all()) self.assertTrue((dx._impl.contact.dist >= 0).all())
def test_hfield_deep(self): def test_hfield_deep(self):
"""Tests that objects with deep penetration do not get stuck.""" """Tests that objects with deep penetration do not get stuck."""
@@ -868,8 +880,8 @@ class HFieldTest(absltest.TestCase):
# check that all geoms are colliding with the hfield # check that all geoms are colliding with the hfield
for geom_id in [1, 2, 3]: for geom_id in [1, 2, 3]:
mask = (dx.contact.geom == np.array([0, geom_id])).all(axis=1) mask = (dx._impl.contact.geom == np.array([0, geom_id])).all(axis=1)
c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx.contact) c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx._impl.contact)
# all contact normals are in the top half-face of the hfield # all contact normals are in the top half-face of the hfield
self.assertTrue((c.frame[:, 0].dot(np.array([0, 0, 1])) > 0.7).all()) self.assertTrue((c.frame[:, 0].dot(np.array([0, 0, 1])) > 0.7).all())
@@ -892,8 +904,8 @@ class BodyPairFilterTest(absltest.TestCase):
def test_filter_self_collision(self): def test_filter_self_collision(self):
"""Tests that self collisions get filtered.""" """Tests that self collisions get filtered."""
d, dx = _collide(self._SELF_COLLISION) d, dx = _collide(self._SELF_COLLISION)
self.assertEqual(dx.contact.pos.shape[0], d.contact.pos.shape[0]) self.assertEqual(dx._impl.contact.pos.shape[0], d.contact.pos.shape[0])
self.assertEqual(dx.contact.pos.shape[0], 0) self.assertEqual(dx._impl.contact.pos.shape[0], 0)
_PARENT_CHILD = """ _PARENT_CHILD = """
<mujoco> <mujoco>
@@ -923,8 +935,8 @@ class BodyPairFilterTest(absltest.TestCase):
dx = kinematics_jit_fn(mx, dx) dx = kinematics_jit_fn(mx, dx)
dx = collision_jit_fn(mx, dx) dx = collision_jit_fn(mx, dx)
self.assertEqual(dx.contact.pos.shape[0], d.contact.pos.shape[0]) self.assertEqual(dx._impl.contact.pos.shape[0], d.contact.pos.shape[0])
self.assertEqual(dx.contact.pos.shape[0], 0) self.assertEqual(dx._impl.contact.pos.shape[0], 0)
def test_disable_filter_parent_child(self): def test_disable_filter_parent_child(self):
"""Tests that filterparent flag disables parent-child filtering.""" """Tests that filterparent flag disables parent-child filtering."""
@@ -941,8 +953,8 @@ class BodyPairFilterTest(absltest.TestCase):
dx = collision_jit_fn(mx, dx) dx = collision_jit_fn(mx, dx)
# one collision between parent-child spheres # one collision between parent-child spheres
self.assertEqual(dx.contact.pos.shape[0], d.contact.pos.shape[0]) self.assertEqual(dx._impl.contact.pos.shape[0], d.contact.pos.shape[0])
self.assertEqual(dx.contact.pos.shape[0], 1) self.assertEqual(dx._impl.contact.pos.shape[0], 1)
class DimTest(parameterized.TestCase): class DimTest(parameterized.TestCase):
@@ -1027,8 +1039,8 @@ class TopKContactTest(absltest.TestCase):
dx_all = collision_jit_fn(mx_all, dx) dx_all = collision_jit_fn(mx_all, dx)
dx_top_k = collision_jit_fn(mx_top_k, dx) dx_top_k = collision_jit_fn(mx_top_k, dx)
self.assertEqual(dx_all.contact.dist.shape, (3,)) self.assertEqual(dx_all._impl.contact.dist.shape, (3,))
self.assertEqual(dx_top_k.contact.dist.shape, (2,)) self.assertEqual(dx_top_k._impl.contact.dist.shape, (2,))
_CAPSULES_MAX_PAIR = """ _CAPSULES_MAX_PAIR = """
<mujoco> <mujoco>
@@ -1073,9 +1085,9 @@ class TopKContactTest(absltest.TestCase):
dx_all = collision_jit_fn(mx_all, dx) dx_all = collision_jit_fn(mx_all, dx)
dx_top_k = collision_jit_fn(mx_top_k, dx) dx_top_k = collision_jit_fn(mx_top_k, dx)
self.assertEqual(dx_all.contact.dist.shape, (6,)) self.assertEqual(dx_all._impl.contact.dist.shape, (6,))
self.assertEqual(dx_top_k.contact.dist.shape, (2,)) self.assertEqual(dx_top_k._impl.contact.dist.shape, (2,))
self.assertTrue((dx_top_k.contact.dist < 0).all()) self.assertTrue((dx_top_k._impl.contact.dist < 0).all())
if __name__ == '__main__': if __name__ == '__main__':
+2
View File
@@ -32,7 +32,9 @@ from mujoco.mjx._src.collision_types import Collision
from mujoco.mjx._src.collision_types import GeomInfo from mujoco.mjx._src.collision_types import GeomInfo
from mujoco.mjx._src.dataclasses import PyTreeNode from mujoco.mjx._src.dataclasses import PyTreeNode
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
# the SDF function takes position in, and returns a distance or objective # the SDF function takes position in, and returns a distance or objective
+58 -26
View File
@@ -28,10 +28,12 @@ from mujoco.mjx._src.types import ConeType
from mujoco.mjx._src.types import ConstraintType from mujoco.mjx._src.types import ConstraintType
from mujoco.mjx._src.types import Contact from mujoco.mjx._src.types import Contact
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import DisableBit
from mujoco.mjx._src.types import EqType from mujoco.mjx._src.types import EqType
from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import JointType
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
from mujoco.mjx._src.types import ObjType from mujoco.mjx._src.types import ObjType
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
import numpy as np import numpy as np
@@ -281,6 +283,10 @@ def _efc_equality_joint(m: Model, d: Data) -> Optional[_Efc]:
def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]: def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]:
"""Calculates constraint rows for tendon equality constraints.""" """Calculates constraint rows for tendon equality constraints."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError(
'_efc_equality_tendon requires JAX backend implementation.'
)
eq_id = np.nonzero(m.eq_type == EqType.TENDON)[0] eq_id = np.nonzero(m.eq_type == EqType.TENDON)[0]
@@ -314,9 +320,9 @@ def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]:
return jax.tree_util.tree_map(lambda x: x * active, efc) return jax.tree_util.tree_map(lambda x: x * active, efc)
inv1, inv2 = m.tendon_invweight0[obj1id], m.tendon_invweight0[obj2id] inv1, inv2 = m.tendon_invweight0[obj1id], m.tendon_invweight0[obj2id]
jac1, jac2 = d.ten_J[obj1id], d.ten_J[obj2id] jac1, jac2 = d._impl.ten_J[obj1id], d._impl.ten_J[obj2id]
pos1 = d.ten_length[obj1id] - m.tendon_length0[obj1id] pos1 = d._impl.ten_length[obj1id] - m.tendon_length0[obj1id]
pos2 = d.ten_length[obj2id] - m.tendon_length0[obj2id] pos2 = d._impl.ten_length[obj2id] - m.tendon_length0[obj2id]
invweight = inv1 + inv2 * (obj2id > -1) invweight = inv1 + inv2 * (obj2id > -1)
return rows( return rows(
@@ -326,8 +332,11 @@ def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]:
def _efc_friction(m: Model, d: Data) -> Optional[_Efc]: def _efc_friction(m: Model, d: Data) -> Optional[_Efc]:
"""Calculates constraint rows for dof frictionloss.""" """Calculates constraint rows for dof frictionloss."""
dof_id = np.nonzero(m.dof_hasfrictionloss)[0] if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
tendon_id = np.nonzero(m.tendon_hasfrictionloss)[0] raise ValueError('_efc_friction requires JAX backend implementation.')
dof_id = np.nonzero(m._impl.dof_hasfrictionloss)[0]
tendon_id = np.nonzero(m._impl.tendon_hasfrictionloss)[0]
size = dof_id.size + tendon_id.size size = dof_id.size + tendon_id.size
if (m.opt.disableflags & DisableBit.FRICTIONLOSS) or (size == 0): if (m.opt.disableflags & DisableBit.FRICTIONLOSS) or (size == 0):
@@ -337,7 +346,7 @@ def _efc_friction(m: Model, d: Data) -> Optional[_Efc]:
args_dof += (m.dof_solimp,) args_dof += (m.dof_solimp,)
args_dof = jax.tree_util.tree_map(lambda x: x[dof_id], args_dof) args_dof = jax.tree_util.tree_map(lambda x: x[dof_id], args_dof)
args_ten = (d.ten_J, m.tendon_frictionloss, m.tendon_invweight0) args_ten = (d._impl.ten_J, m.tendon_frictionloss, m.tendon_invweight0)
args_ten += (m.tendon_solref_fri, m.tendon_solimp_fri) args_ten += (m.tendon_solref_fri, m.tendon_solimp_fri)
args_ten = jax.tree_util.tree_map(lambda x: x[tendon_id], args_ten) args_ten = jax.tree_util.tree_map(lambda x: x[tendon_id], args_ten)
@@ -415,6 +424,9 @@ def _efc_limit_slide_hinge(m: Model, d: Data) -> Optional[_Efc]:
def _efc_limit_tendon(m: Model, d: Data) -> Optional[_Efc]: def _efc_limit_tendon(m: Model, d: Data) -> Optional[_Efc]:
"""Calculates constraint rows for tendon limits.""" """Calculates constraint rows for tendon limits."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('_efc_limit_tendon requires JAX backend implementation.')
tendon_id = np.nonzero(m.tendon_limited)[0] tendon_id = np.nonzero(m.tendon_limited)[0]
if (m.opt.disableflags & DisableBit.LIMIT) or tendon_id.size == 0: if (m.opt.disableflags & DisableBit.LIMIT) or tendon_id.size == 0:
@@ -423,8 +435,8 @@ def _efc_limit_tendon(m: Model, d: Data) -> Optional[_Efc]:
length, j, range_, margin, invweight, solref, solimp = jax.tree_util.tree_map( length, j, range_, margin, invweight, solref, solimp = jax.tree_util.tree_map(
lambda x: x[tendon_id], lambda x: x[tendon_id],
( (
d.ten_length, d._impl.ten_length,
d.ten_J, d._impl.ten_J,
m.tendon_range, m.tendon_range,
m.tendon_margin, m.tendon_margin,
m.tendon_invweight0, m.tendon_invweight0,
@@ -446,8 +458,12 @@ def _efc_limit_tendon(m: Model, d: Data) -> Optional[_Efc]:
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."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError(
'_efc_contact_frictionless requires JAX backend implementation.'
)
con_id = np.nonzero(d.contact.dim == 1)[0] con_id = np.nonzero(d._impl.contact.dim == 1)[0]
if con_id.size == 0: if con_id.size == 0:
return None return None
@@ -473,15 +489,19 @@ def _efc_contact_frictionless(m: Model, d: Data) -> Optional[_Efc]:
jp.zeros_like(pos), jp.zeros_like(pos),
) )
contact = jax.tree_util.tree_map(lambda x: x[con_id], d.contact) contact = jax.tree_util.tree_map(lambda x: x[con_id], d._impl.contact)
return rows(contact) return rows(contact)
def _efc_contact_pyramidal(m: Model, d: Data, condim: int) -> Optional[_Efc]: def _efc_contact_pyramidal(m: Model, d: Data, condim: int) -> Optional[_Efc]:
"""Calculates constraint rows for frictional pyramidal contacts.""" """Calculates constraint rows for frictional pyramidal contacts."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError(
'_efc_contact_pyramidal requires JAX backend implementation.'
)
con_id = np.nonzero(d.contact.dim == condim)[0] con_id = np.nonzero(d._impl.contact.dim == condim)[0]
if con_id.size == 0: if con_id.size == 0:
return None return None
@@ -518,15 +538,19 @@ def _efc_contact_pyramidal(m: Model, d: Data, condim: int) -> Optional[_Efc]:
jp.zeros_like(pos), jp.zeros_like(pos),
) )
contact = jax.tree_util.tree_map(lambda x: x[con_id], d.contact) contact = jax.tree_util.tree_map(lambda x: x[con_id], d._impl.contact)
# concatenate to drop row grouping # concatenate to drop row grouping
return jax.tree_util.tree_map(jp.concatenate, rows(contact)) return jax.tree_util.tree_map(jp.concatenate, rows(contact))
def _efc_contact_elliptic(m: Model, d: Data, condim: int) -> Optional[_Efc]: def _efc_contact_elliptic(m: Model, d: Data, condim: int) -> Optional[_Efc]:
"""Calculates constraint rows for frictional elliptic contacts.""" """Calculates constraint rows for frictional elliptic contacts."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError(
'_efc_contact_elliptic requires JAX backend implementation.'
)
con_id = np.nonzero(d.contact.dim == condim)[0] con_id = np.nonzero(d._impl.contact.dim == condim)[0]
if con_id.size == 0: if con_id.size == 0:
return None return None
@@ -563,7 +587,7 @@ def _efc_contact_elliptic(m: Model, d: Data, condim: int) -> Optional[_Efc]:
jp.zeros_like(pos), jp.zeros_like(pos),
) )
contact = jax.tree_util.tree_map(lambda x: x[con_id], d.contact) contact = jax.tree_util.tree_map(lambda x: x[con_id], d._impl.contact)
# concatenate to drop row grouping # concatenate to drop row grouping
return jax.tree_util.tree_map(jp.concatenate, rows(contact)) return jax.tree_util.tree_map(jp.concatenate, rows(contact))
@@ -602,14 +626,14 @@ def make_efc_type(
if not m.opt.disableflags & DisableBit.FRICTIONLOSS: if not m.opt.disableflags & DisableBit.FRICTIONLOSS:
nf_dof = ( nf_dof = (
m.dof_hasfrictionloss.sum() m._impl.dof_hasfrictionloss.sum()
if isinstance(m, Model) if isinstance(m, Model) and isinstance(m._impl, ModelJAX)
else (m.dof_frictionloss > 0).sum() else (m.dof_frictionloss > 0).sum()
) )
efc_types += [ConstraintType.FRICTION_DOF] * nf_dof efc_types += [ConstraintType.FRICTION_DOF] * nf_dof
nf_tendon = ( nf_tendon = (
m.tendon_hasfrictionloss.sum() m._impl.tendon_hasfrictionloss.sum()
if isinstance(m, Model) if isinstance(m, Model) and isinstance(m._impl, ModelJAX)
else (m.tendon_frictionloss > 0).sum() else (m.tendon_frictionloss > 0).sum()
) )
efc_types += [ConstraintType.FRICTION_TENDON] * nf_tendon efc_types += [ConstraintType.FRICTION_TENDON] * nf_tendon
@@ -683,10 +707,14 @@ def make_constraint(m: Model, d: Data) -> Data:
if not efcs: if not efcs:
z = jp.empty(0) z = jp.empty(0)
d = d.replace(efc_J=jp.empty((0, m.nv))) d = d.tree_replace({'_impl.efc_J': jp.empty((0, m.nv))})
d = d.replace( d = d.tree_replace({
efc_D=z, efc_aref=z, efc_frictionloss=z, efc_pos=z, efc_margin=z '_impl.efc_D': z,
) '_impl.efc_aref': z,
'_impl.efc_frictionloss': z,
'_impl.efc_pos': z,
'_impl.efc_margin': z,
})
return d return d
efc = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *efcs) efc = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *efcs)
@@ -699,9 +727,13 @@ def make_constraint(m: Model, d: Data) -> Data:
return aref, r, efc.pos_aref + efc.margin, efc.margin, efc.frictionloss return aref, r, efc.pos_aref + efc.margin, efc.margin, efc.frictionloss
aref, r, pos, margin, frictionloss = fn(efc) aref, r, pos, margin, frictionloss = fn(efc)
d = d.replace( d = d.tree_replace({
efc_J=efc.J, efc_D=1 / r, efc_aref=aref, efc_pos=pos, efc_margin=margin '_impl.efc_J': efc.J,
) '_impl.efc_D': 1 / r,
d = d.replace(efc_frictionloss=frictionloss) '_impl.efc_aref': aref,
'_impl.efc_pos': pos,
'_impl.efc_margin': margin,
})
d = d.tree_replace({'_impl.efc_frictionloss': frictionloss})
return d return d
+18 -14
View File
@@ -69,16 +69,16 @@ class ConstraintTest(parameterized.TestCase):
order = test_util.efc_order(m, d, dx) order = test_util.efc_order(m, d, dx)
d_efc_j = d.efc_J.reshape((-1, m.nv)) d_efc_j = d.efc_J.reshape((-1, m.nv))
_assert_eq(d_efc_j, dx.efc_J[order][: d.nefc], 'efc_J') _assert_eq(d_efc_j, dx._impl.efc_J[order][: d.nefc], 'efc_J')
_assert_eq(0, dx.efc_J[order][d.nefc :], 'efc_J') _assert_eq(0, dx._impl.efc_J[order][d.nefc :], 'efc_J')
_assert_eq(d.efc_aref, dx.efc_aref[order][: d.nefc], 'efc_aref') _assert_eq(d.efc_aref, dx._impl.efc_aref[order][: d.nefc], 'efc_aref')
_assert_eq(0, dx.efc_aref[order][d.nefc :], 'efc_aref') _assert_eq(0, dx._impl.efc_aref[order][d.nefc :], 'efc_aref')
_assert_eq(d.efc_D, dx.efc_D[order][: d.nefc], 'efc_D') _assert_eq(d.efc_D, dx._impl.efc_D[order][: d.nefc], 'efc_D')
_assert_eq(d.efc_pos, dx.efc_pos[order][: d.nefc], 'efc_pos') _assert_eq(d.efc_pos, dx._impl.efc_pos[order][: d.nefc], 'efc_pos')
_assert_eq(dx.efc_pos[order][d.nefc :], 0, 'efc_pos') _assert_eq(dx._impl.efc_pos[order][d.nefc :], 0, 'efc_pos')
_assert_eq( _assert_eq(
d.efc_frictionloss, d.efc_frictionloss,
dx.efc_frictionloss[order][: d.nefc], dx._impl.efc_frictionloss[order][: d.nefc],
'efc_frictionloss', 'efc_frictionloss',
) )
@@ -104,7 +104,7 @@ class ConstraintTest(parameterized.TestCase):
self.assertEqual(nl, 0) self.assertEqual(nl, 0)
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], 0) self.assertEqual(dx._impl.efc_J.shape[0], 0)
def test_disable_equality(self): def test_disable_equality(self):
m = test_util.load_test_file('constraints.xml') m = test_util.load_test_file('constraints.xml')
@@ -115,7 +115,9 @@ class ConstraintTest(parameterized.TestCase):
self.assertEqual(nl, 5) self.assertEqual(nl, 5)
self.assertEqual(nc, 180) self.assertEqual(nc, 180)
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], 187) # only joint/tendon limit, contact self.assertEqual(
dx._impl.efc_J.shape[0], 187
) # 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')
@@ -126,7 +128,9 @@ class ConstraintTest(parameterized.TestCase):
self.assertEqual(nl, 5) 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], 27) # only equality, joint/tendon limit self.assertEqual(
dx._impl.efc_J.shape[0], 27
) # only equality, joint/tendon limit
def test_disable_frictionloss(self): def test_disable_frictionloss(self):
m = test_util.load_test_file('constraints.xml') m = test_util.load_test_file('constraints.xml')
@@ -137,7 +141,7 @@ class ConstraintTest(parameterized.TestCase):
self.assertEqual(nl, 5) self.assertEqual(nl, 5)
self.assertEqual(nc, 180) self.assertEqual(nc, 180)
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], 205) self.assertEqual(dx._impl.efc_J.shape[0], 205)
def test_margin(self): def test_margin(self):
"""Test margin.""" """Test margin."""
@@ -163,8 +167,8 @@ class ConstraintTest(parameterized.TestCase):
dx = mjx.make_constraint(mx, dx) dx = mjx.make_constraint(mx, dx)
order = test_util.efc_order(m, d, dx) order = test_util.efc_order(m, d, dx)
_assert_eq(d.efc_pos, dx.efc_pos[order][: d.nefc], 'efc_pos') _assert_eq(d.efc_pos, dx._impl.efc_pos[order][: d.nefc], 'efc_pos')
_assert_eq(d.efc_margin, dx.efc_margin[order][: d.nefc], 'efc_margin') _assert_eq(d.efc_margin, dx._impl.efc_margin[order][: d.nefc], 'efc_margin')
if __name__ == '__main__': if __name__ == '__main__':
+31
View File
@@ -18,6 +18,7 @@ import copy
import dataclasses import dataclasses
import typing import typing
from typing import Dict, Optional, Sequence, Tuple, TypeVar, Union from typing import Dict, Optional, Sequence, Tuple, TypeVar, Union
import warnings
import jax import jax
import numpy as np import numpy as np
@@ -49,6 +50,36 @@ def dataclass(clz: _T, register_as_pytree: bool) -> _T:
""" """
data_clz = dataclasses.dataclass(frozen=True)(clz) data_clz = dataclasses.dataclass(frozen=True)(clz)
data_clz.replace = dataclasses.replace data_clz.replace = dataclasses.replace
# def replace(self, **updates):
# """Returns a new object replacing the specified fields with new values."""
# if not hasattr(self, '_impl'):
# return dataclasses.replace(self, **updates)
# # Private fields under `_impl` are allowed to be replaced directly as if
# # they were on the base class. This logic will be removed in a future
# # release.
# impl_updates = {}
# for k in tuple(updates.keys()):
# # Recall that getattr is overridden for '_impl' fields.
# hasattr_ = k in self.__annotations__
# if not hasattr_ and hasattr(self._impl, k): # pylint: disable=protected-access
# impl_updates[k] = updates[k]
# del updates[k]
# if impl_updates:
# updates['_impl'] = self._impl.replace(**impl_updates)
# warnings.warn(
# f'Accessing/replacing fields `{tuple(impl_updates.keys())}` directly'
# f' from `{self.__class__.__name__}` will be deprecated. Refrain from'
# ' using private fields that were moved to'
# f' `{self.__class__.__name__}`._impl.',
# DeprecationWarning,
# stacklevel=2,
# )
# return dataclasses.replace(self, **updates)
# data_clz.replace = replace
if register_as_pytree: if register_as_pytree:
meta_fields, data_fields = [], [] meta_fields, data_fields = [], []
+5 -3
View File
@@ -40,7 +40,9 @@ def deriv_smooth_vel(m: Model, d: Data) -> Optional[jax.Array]:
gain_vel = m.actuator_gainprm[:, 2] * affine_gain gain_vel = m.actuator_gainprm[:, 2] * affine_gain
ctrl = d.ctrl.at[m.actuator_dyntype != DynType.NONE].set(d.act) ctrl = d.ctrl.at[m.actuator_dyntype != DynType.NONE].set(d.act)
vel = bias_vel + gain_vel * ctrl vel = bias_vel + gain_vel * ctrl
qderiv = d.actuator_moment.T @ jax.vmap(jp.multiply)(d.actuator_moment, vel) qderiv = d._impl.actuator_moment.T @ jax.vmap(jp.multiply)(
d._impl.actuator_moment, vel
)
# qDeriv += d qfrc_passive / d qvel # qDeriv += d qfrc_passive / d qvel
if not m.opt.disableflags & DisableBit.PASSIVE: if not m.opt.disableflags & DisableBit.PASSIVE:
@@ -49,9 +51,9 @@ def deriv_smooth_vel(m: Model, d: Data) -> Optional[jax.Array]:
else: else:
qderiv -= jp.diag(m.dof_damping) qderiv -= jp.diag(m.dof_damping)
if m.ntendon: if m.ntendon:
qderiv -= d.ten_J.T @ jp.diag(m.tendon_damping) @ d.ten_J qderiv -= d._impl.ten_J.T @ jp.diag(m.tendon_damping) @ d._impl.ten_J
# TODO(robotics-simulation): fluid drag model # TODO(robotics-simulation): fluid drag model
if m.opt.has_fluid_params: if m.opt.has_fluid_params: # pytype: disable=attribute-error
raise NotImplementedError('fluid drag not supported for implicitfast') raise NotImplementedError('fluid drag not supported for implicitfast')
# TODO(team): rne derivative # TODO(team): rne derivative
+25 -11
View File
@@ -33,12 +33,14 @@ from mujoco.mjx._src import support
# pylint: disable=g-importing-member # pylint: disable=g-importing-member
from mujoco.mjx._src.types import BiasType from mujoco.mjx._src.types import BiasType
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import DisableBit
from mujoco.mjx._src.types import DynType from mujoco.mjx._src.types import DynType
from mujoco.mjx._src.types import GainType from mujoco.mjx._src.types import GainType
from mujoco.mjx._src.types import IntegratorType from mujoco.mjx._src.types import IntegratorType
from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import JointType
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
from mujoco.mjx._src.types import TrnType from mujoco.mjx._src.types import TrnType
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
import numpy as np import numpy as np
@@ -81,10 +83,13 @@ 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( if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
actuator_velocity=d.actuator_moment @ d.qvel, raise ValueError('fwd_velocity requires JAX backend implementation.')
ten_velocity=d.ten_J @ d.qvel,
) d = d.tree_replace({
'_impl.actuator_velocity': d._impl.actuator_moment @ d.qvel,
'_impl.ten_velocity': d._impl.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)
@@ -173,8 +178,8 @@ def fwd_actuation(m: Model, d: Data) -> Data:
m.actuator_gainprm, m.actuator_gainprm,
m.actuator_biastype, m.actuator_biastype,
m.actuator_biasprm, m.actuator_biasprm,
d.actuator_length, d._impl.actuator_length,
d.actuator_velocity, d._impl.actuator_velocity,
ctrl_act, ctrl_act,
jp.array(m.actuator_lengthrange), jp.array(m.actuator_lengthrange),
jp.array(m.actuator_acc0), jp.array(m.actuator_acc0),
@@ -215,7 +220,7 @@ def fwd_actuation(m: Model, d: Data) -> Data:
) )
force = jp.clip(force, forcerange[:, 0], forcerange[:, 1]) force = jp.clip(force, forcerange[:, 0], forcerange[:, 1])
qfrc_actuator = d.actuator_moment.T @ force qfrc_actuator = d._impl.actuator_moment.T @ force
if m.ngravcomp: if m.ngravcomp:
# actuator-level gravity compensation, skip if added as passive force # actuator-level gravity compensation, skip if added as passive force
@@ -331,13 +336,17 @@ def _advance(
@named_scope @named_scope
def euler(m: Model, d: Data) -> Data: def euler(m: Model, d: Data) -> Data:
"""Euler integrator, semi-implicit in velocity.""" """Euler integrator, semi-implicit in velocity."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('euler requires JAX backend implementation.')
# integrate damping implicitly # integrate damping implicitly
qacc = d.qacc qacc = d.qacc
if not m.opt.disableflags & DisableBit.EULERDAMP: if not m.opt.disableflags & DisableBit.EULERDAMP:
if support.is_sparse(m): if support.is_sparse(m):
dh = d.replace(qM=d.qM.at[m.dof_Madr].add(m.opt.timestep * m.dof_damping)) qM = d._impl.qM.at[m.dof_Madr].add(m.opt.timestep * m.dof_damping)
else: else:
dh = d.replace(qM=d.qM + jp.diag(m.opt.timestep * m.dof_damping)) qM = d._impl.qM + jp.diag(m.opt.timestep * m.dof_damping)
dh = d.tree_replace({'_impl.qM': qM})
dh = smooth.factor_m(m, dh) dh = smooth.factor_m(m, dh)
qfrc = d.qfrc_smooth + d.qfrc_constraint qfrc = d.qfrc_smooth + d.qfrc_constraint
qacc = smooth.solve_m(m, dh, qfrc) qacc = smooth.solve_m(m, dh, qfrc)
@@ -392,13 +401,15 @@ def rungekutta4(m: Model, d: Data) -> Data:
@named_scope @named_scope
def implicit(m: Model, d: Data) -> Data: def implicit(m: Model, d: Data) -> Data:
"""Integrates fully implicit in velocity.""" """Integrates fully implicit in velocity."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('implicit requires JAX backend implementation.')
qderiv = derivative.deriv_smooth_vel(m, d) qderiv = derivative.deriv_smooth_vel(m, d)
qacc = d.qacc qacc = d.qacc
if qderiv is not None: if qderiv is not None:
# TODO(robotics-simulation): use smooth.factor_m / solve_m here: # TODO(robotics-simulation): use smooth.factor_m / solve_m here:
qm = support.full_m(m, d) if support.is_sparse(m) else d.qM qm = support.full_m(m, d) if support.is_sparse(m) else d._impl.qM
qm -= m.opt.timestep * qderiv qm -= m.opt.timestep * qderiv
qh, _ = jax.scipy.linalg.cho_factor(qm) qh, _ = jax.scipy.linalg.cho_factor(qm)
qfrc = d.qfrc_smooth + d.qfrc_constraint qfrc = d.qfrc_smooth + d.qfrc_constraint
@@ -410,6 +421,9 @@ def implicit(m: Model, d: Data) -> Data:
@named_scope @named_scope
def forward(m: Model, d: Data) -> Data: def forward(m: Model, d: Data) -> Data:
"""Forward dynamics.""" """Forward dynamics."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('forward requires JAX backend implementation.')
d = fwd_position(m, d) d = fwd_position(m, d)
d = sensor.sensor_pos(m, d) d = sensor.sensor_pos(m, d)
d = fwd_velocity(m, d) d = fwd_velocity(m, d)
@@ -418,7 +432,7 @@ def forward(m: Model, d: Data) -> Data:
d = fwd_acceleration(m, d) d = fwd_acceleration(m, d)
d = sensor.sensor_acc(m, d) d = sensor.sensor_acc(m, d)
if d.efc_J.size == 0: if d._impl.efc_J.size == 0:
d = d.replace(qacc=d.qacc_smooth) d = d.replace(qacc=d.qacc_smooth)
return d return d
+5 -5
View File
@@ -69,16 +69,16 @@ def inv_constraint(m: Model, d: Data) -> Data:
"""Inverse constraint solver.""" """Inverse constraint solver."""
# no constraints # no constraints
if d.efc_J.size == 0: if d._impl.efc_J.size == 0: # pytype: disable=attribute-error
return d.replace(qfrc_constraint=jp.zeros(m.nv)) return d.replace(qfrc_constraint=jp.zeros(m.nv))
# update # update
ctx = solver.Context.create(m, d, grad=False) ctx = solver.Context.create(m, d, grad=False)
return d.replace( return d.tree_replace({
qfrc_constraint=ctx.qfrc_constraint, 'qfrc_constraint': ctx.qfrc_constraint,
efc_force=ctx.efc_force, '_impl.efc_force': ctx.efc_force,
) })
def inverse(m: Model, d: Data) -> Data: def inverse(m: Model, d: Data) -> Data:
-2
View File
@@ -15,12 +15,10 @@
"""Tests for inverse dynamics functions.""" """Tests for inverse dynamics functions."""
from absl.testing import absltest from absl.testing import absltest
from absl.testing import parameterized from absl.testing import parameterized
import jax
from jax import numpy as jp from jax import numpy as jp
import mujoco import mujoco
from mujoco import mjx from mujoco import mjx
from mujoco.mjx._src import support from mujoco.mjx._src import support
from mujoco.mjx._src import test_util
import numpy as np import numpy as np
# tolerance for difference between MuJoCo and MJX calculations - mostly # tolerance for difference between MuJoCo and MJX calculations - mostly
+854 -345
View File
File diff suppressed because it is too large Load Diff
+469 -97
View File
@@ -14,14 +14,19 @@
# ============================================================================== # ==============================================================================
"""Tests for io functions.""" """Tests for io functions."""
import os
from unittest import mock
from absl.testing import absltest from absl.testing import absltest
from absl.testing import parameterized from absl.testing import parameterized
import jax import jax
from jax import numpy as jp from jax import numpy as jp
import mujoco import mujoco
from mujoco import mjx from mujoco import mjx
from mujoco.mjx._src import io as mjx_io
from mujoco.mjx._src import test_util from mujoco.mjx._src import test_util
# pylint: disable=g-importing-member # pylint: disable=g-importing-member
from mujoco.mjx._src.types import BackendImpl
from mujoco.mjx._src.types import ConeType from mujoco.mjx._src.types import ConeType
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
import numpy as np import numpy as np
@@ -95,10 +100,13 @@ _MULTIPLE_CONSTRAINTS = """
class ModelIOTest(parameterized.TestCase): class ModelIOTest(parameterized.TestCase):
"""IO tests for mjx.Model.""" """IO tests for mjx.Model."""
@parameterized.parameters(_MULTIPLE_CONVEX_OBJECTS, _MULTIPLE_CONSTRAINTS) @parameterized.product(
def test_put_model(self, xml): xml=(_MULTIPLE_CONVEX_OBJECTS, _MULTIPLE_CONSTRAINTS),
backend_impl=('jax', 'c'),
)
def test_put_model(self, xml, backend_impl):
m = mujoco.MjModel.from_xml_string(xml) m = mujoco.MjModel.from_xml_string(xml)
mx = mjx.put_model(m) mx = mjx.put_model(m, backend_impl=backend_impl)
def assert_not_weak_type(x): def assert_not_weak_type(x):
if isinstance(x, jax.Array): if isinstance(x, jax.Array):
@@ -120,8 +128,14 @@ class ModelIOTest(parameterized.TestCase):
self.assertEqual(mx.nM, m.nM) self.assertEqual(mx.nM, m.nM)
self.assertAlmostEqual(mx.opt.timestep, m.opt.timestep) self.assertAlmostEqual(mx.opt.timestep, m.opt.timestep)
# fields restricted to MuJoCo should not be populated if backend_impl == 'jax':
self.assertEqual(mx.bvh_aabb.shape, (0,)) # fields restricted to MuJoCo should not be populated
self.assertFalse(hasattr(mx, 'bvh_aabb'))
elif backend_impl == 'c':
# Options specific to C are populated.
self.assertEqual(mx.opt.apirate, m.opt.apirate)
# Fields private to C backend impl are populated.
self.assertTrue(hasattr(mx._impl, 'bvh_aabb'))
np.testing.assert_allclose(mx.body_parentid, m.body_parentid) np.testing.assert_allclose(mx.body_parentid, m.body_parentid)
np.testing.assert_allclose(mx.geom_type, m.geom_type) np.testing.assert_allclose(mx.geom_type, m.geom_type)
@@ -150,7 +164,8 @@ class ModelIOTest(parameterized.TestCase):
m = mjx.put_model( m = mjx.put_model(
mujoco.MjModel.from_xml_string( mujoco.MjModel.from_xml_string(
'<mujoco><option viscosity="3.0"/><worldbody/></mujoco>' '<mujoco><option viscosity="3.0"/><worldbody/></mujoco>'
) ),
backend_impl='jax',
) )
self.assertTrue(m.opt.has_fluid_params) self.assertTrue(m.opt.has_fluid_params)
@@ -174,7 +189,8 @@ class ModelIOTest(parameterized.TestCase):
def test_margin_gap_mesh_not_implemented(self): def test_margin_gap_mesh_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>
<asset> <asset>
<mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1" scale="1 1 1"/> <mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1" scale="1 1 1"/>
@@ -189,27 +205,32 @@ class ModelIOTest(parameterized.TestCase):
<geom size="0.05"/> <geom size="0.05"/>
</body> </body>
</worldbody> </worldbody>
</mujoco>""")) </mujoco>"""),
backend_impl='jax',
)
def test_implicitfast_fluid_not_implemented(self): def test_implicitfast_fluid_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>
<option viscosity="3.0" integrator="implicitfast"/> <option viscosity="3.0" integrator="implicitfast"/>
<worldbody/> <worldbody/>
</mujoco>""")) </mujoco>"""),
backend_impl='jax',
)
def test_wrap_inside(self): def test_wrap_inside(self):
m = test_util.load_test_file('tendon/wrap_sidesite.xml') m = test_util.load_test_file('tendon/wrap_sidesite.xml')
mx0 = mjx.put_model(m) mx0 = mjx.put_model(m, backend_impl='jax')
np.testing.assert_equal( np.testing.assert_equal(
mx0.is_wrap_inside, mx0._impl.is_wrap_inside,
np.array([1, 0, 1, 0, 1, 1, 0]), np.array([1, 0, 1, 0, 1, 1, 0]),
) )
m.site_pos[2] = m.site_pos[1] m.site_pos[2] = m.site_pos[1]
mx1 = mjx.put_model(m) mx1 = mjx.put_model(m, backend_impl='jax')
np.testing.assert_equal( np.testing.assert_equal(
mx1.is_wrap_inside, mx1._impl.is_wrap_inside,
np.array([0, 0, 1, 0, 1, 0, 0]), np.array([0, 0, 1, 0, 1, 0, 0]),
) )
@@ -217,11 +238,11 @@ class ModelIOTest(parameterized.TestCase):
class DataIOTest(parameterized.TestCase): class DataIOTest(parameterized.TestCase):
"""IO tests for mjx.Data.""" """IO tests for mjx.Data."""
def test_make_data(self): @parameterized.parameters('jax', 'c')
def test_make_data(self, backend_impl: str):
"""Test that make_data returns the correct shapes.""" """Test that make_data returns the correct shapes."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS) m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS)
d = mjx.make_data(m) d = mjx.make_data(m, backend_impl=backend_impl)
nq = 22 nq = 22
nbody = 5 nbody = 5
@@ -230,7 +251,7 @@ class DataIOTest(parameterized.TestCase):
nv = 19 nv = 19
nefc = 185 nefc = 185
self.assertEqual(d.nefc, nefc) self.assertEqual(d._impl.nefc, nefc)
self.assertEqual(d.qpos.shape, (nq,)) self.assertEqual(d.qpos.shape, (nq,))
self.assertEqual(d.qvel.shape, (nv,)) self.assertEqual(d.qvel.shape, (nv,))
self.assertEqual(d.act.shape, (0,)) self.assertEqual(d.act.shape, (0,))
@@ -251,57 +272,68 @@ class DataIOTest(parameterized.TestCase):
self.assertEqual(d.geom_xpos.shape, (6, 3)) self.assertEqual(d.geom_xpos.shape, (6, 3))
self.assertEqual(d.geom_xmat.shape, (6, 3, 3)) self.assertEqual(d.geom_xmat.shape, (6, 3, 3))
self.assertEqual(d.subtree_com.shape, (nbody, 3)) self.assertEqual(d.subtree_com.shape, (nbody, 3))
self.assertEqual(d.cdof.shape, (nv, 6)) self.assertEqual(d._impl.cdof.shape, (nv, 6))
self.assertEqual(d.cinert.shape, (nbody, 10)) self.assertEqual(d._impl.cinert.shape, (nbody, 10))
self.assertEqual(d.crb.shape, (nbody, 10)) self.assertEqual(d._impl.crb.shape, (nbody, 10))
self.assertEqual(d.actuator_length.shape, (1,)) self.assertEqual(d._impl.actuator_length.shape, (1,))
self.assertEqual(d.actuator_moment.shape, (1, nv)) self.assertEqual(d._impl.actuator_moment.shape, (1, nv))
self.assertEqual(d.qM.shape, (nv, nv)) self.assertEqual(d._impl.contact.dist.shape, (ncon,))
self.assertEqual(d.qLD.shape, (nv, nv)) self.assertEqual(d._impl.contact.pos.shape, (ncon, 3))
self.assertEqual(d.qLDiagInv.shape, (0,)) self.assertEqual(d._impl.contact.frame.shape, (ncon, 3, 3))
self.assertEqual(d.contact.dist.shape, (ncon,)) self.assertEqual(d._impl.contact.solref.shape, (ncon, 2))
self.assertEqual(d.contact.pos.shape, (ncon, 3)) self.assertEqual(d._impl.contact.solimp.shape, (ncon, 5))
self.assertEqual(d.contact.frame.shape, (ncon, 3, 3)) self.assertEqual(d._impl.contact.geom1.shape, (ncon,))
self.assertEqual(d.contact.solref.shape, (ncon, 2)) self.assertEqual(d._impl.contact.geom2.shape, (ncon,))
self.assertEqual(d.contact.solimp.shape, (ncon, 5)) self.assertEqual(d._impl.efc_J.shape, (nefc, nv))
self.assertEqual(d.contact.geom1.shape, (ncon,)) self.assertEqual(d._impl.efc_frictionloss.shape, (nefc,))
self.assertEqual(d.contact.geom2.shape, (ncon,)) self.assertEqual(d._impl.efc_D.shape, (nefc,))
self.assertEqual(d.efc_J.shape, (nefc, nv)) self.assertEqual(d._impl.actuator_velocity.shape, (1,))
self.assertEqual(d.efc_frictionloss.shape, (nefc,))
self.assertEqual(d.efc_D.shape, (nefc,))
self.assertEqual(d.actuator_velocity.shape, (1,))
self.assertEqual(d.cvel.shape, (nbody, 6)) self.assertEqual(d.cvel.shape, (nbody, 6))
self.assertEqual(d.cdof_dot.shape, (nv, 6)) self.assertEqual(d._impl.cdof_dot.shape, (nv, 6))
self.assertEqual(d.qfrc_bias.shape, (nv,)) self.assertEqual(d.qfrc_bias.shape, (nv,))
self.assertEqual(d.qfrc_passive.shape, (nv,)) self.assertEqual(d.qfrc_passive.shape, (nv,))
self.assertEqual(d.efc_aref.shape, (nefc,)) self.assertEqual(d._impl.efc_aref.shape, (nefc,))
self.assertEqual(d.qfrc_actuator.shape, (nv,)) self.assertEqual(d.qfrc_actuator.shape, (nv,))
self.assertEqual(d.qfrc_smooth.shape, (nv,)) self.assertEqual(d.qfrc_smooth.shape, (nv,))
self.assertEqual(d.qacc_smooth.shape, (nv,)) self.assertEqual(d.qacc_smooth.shape, (nv,))
self.assertEqual(d.qfrc_constraint.shape, (nv,)) self.assertEqual(d.qfrc_constraint.shape, (nv,))
self.assertEqual(d.qfrc_inverse.shape, (nv,)) self.assertEqual(d.qfrc_inverse.shape, (nv,))
self.assertEqual(d.efc_force.shape, (nefc,)) self.assertEqual(d._impl.efc_force.shape, (nefc,))
if backend_impl == 'jax':
self.assertEqual(d._impl.qM.shape, (nv, nv))
self.assertEqual(d._impl.qLD.shape, (nv, nv))
self.assertEqual(d._impl.qLDiagInv.shape, (0,))
elif backend_impl == 'c':
self.assertEqual(d._impl.qM.shape, (nm,))
self.assertEqual(d._impl.qLD.shape, (nm,))
self.assertEqual(d._impl.qLDiagInv.shape, (nv,))
# test sparse # test sparse
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
d = mjx.make_data(m) d = mjx.make_data(m, backend_impl=backend_impl)
self.assertEqual(d.qM.shape, (nm,)) self.assertEqual(d._impl.qM.shape, (nm,))
self.assertEqual(d.qLD.shape, (nm,)) self.assertEqual(d._impl.qLD.shape, (nm,))
self.assertEqual(d.qLDiagInv.shape, (nv,)) self.assertEqual(d._impl.qLDiagInv.shape, (nv,))
def test_put_data(self): if backend_impl == 'c':
# check C specific fields
self.assertEqual(d._impl.light_xpos.shape, (m.nlight, 3))
self.assertEqual(d._impl.bvh_active.shape, (m.nbvh,))
@parameterized.parameters('jax', 'c')
def test_put_data(self, backend_impl: str):
"""Test that put_data puts the correct data for dense and sparse.""" """Test that put_data puts the correct data for dense and sparse."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS)
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx = mjx.put_data(m, d) dx = mjx.put_data(m, d, backend_impl=backend_impl)
# check a few fields # check a few fields
np.testing.assert_allclose(dx.qpos, d.qpos) np.testing.assert_allclose(dx.qpos, d.qpos)
np.testing.assert_allclose(dx.xpos, d.xpos) np.testing.assert_allclose(dx.xpos, d.xpos)
np.testing.assert_allclose(dx.cvel, d.cvel) np.testing.assert_allclose(dx.cvel, d.cvel)
np.testing.assert_allclose(dx.cdof_dot, d.cdof_dot) np.testing.assert_allclose(dx._impl.cdof_dot, d.cdof_dot)
# check that there are no weak types # check that there are no weak types
self.assertFalse( self.assertFalse(
@@ -312,21 +344,26 @@ class DataIOTest(parameterized.TestCase):
) )
) )
# check that qM is transformed properly if backend_impl == 'jax':
qm = np.zeros((m.nv, m.nv), dtype=np.float64) # check that qM is transformed properly
mujoco.mj_fullM(m, qm, d.qM) qm = np.zeros((m.nv, m.nv), dtype=np.float64)
np.testing.assert_allclose(qm, mjx.full_m(mjx.put_model(m), dx)) mujoco.mj_fullM(m, qm, d.qM)
np.testing.assert_allclose(qm, mjx.full_m(mjx.put_model(m), dx))
elif backend_impl == 'c':
np.testing.assert_allclose(dx._impl.qM, d.qM)
np.testing.assert_allclose(dx._impl.qLD, d.qLD)
np.testing.assert_allclose(dx._impl.qLDiagInv, d.qLDiagInv)
# 4 contacts, 2 for each capsule against the plane # 4 contacts, 2 for each capsule against the plane
self.assertEqual(dx.contact.dist.shape, (4,)) self.assertEqual(dx._impl.contact.dist.shape, (4,))
self.assertEqual(d.ncon, 1) # however only 1 contact in this step self.assertEqual(d.ncon, 1) # however only 1 contact in this step
np.testing.assert_allclose(dx.contact.dist[0], d.contact.dist[0]) np.testing.assert_allclose(dx._impl.contact.dist[0], d.contact.dist[0])
self.assertTrue((dx.contact.dist[1:] > 0).all()) self.assertTrue((dx._impl.contact.dist[1:] > 0).all())
self.assertEqual(dx.contact.frame.shape, (4, 3, 3)) self.assertEqual(dx._impl.contact.frame.shape, (4, 3, 3))
np.testing.assert_allclose( np.testing.assert_allclose(
dx.contact.frame[0].reshape(9), d.contact.frame[0] dx._impl.contact.frame[0].reshape(9), d.contact.frame[0]
) )
np.testing.assert_allclose(dx.contact.frame[1:], 0) np.testing.assert_allclose(dx._impl.contact.frame[1:], 0)
# xmat, ximat, geom_xmat are all shape transformed # xmat, ximat, geom_xmat are all shape transformed
self.assertEqual(dx.xmat.shape, (3, 3, 3)) self.assertEqual(dx.xmat.shape, (3, 3, 3))
@@ -339,57 +376,65 @@ class DataIOTest(parameterized.TestCase):
np.testing.assert_allclose(dx.site_xmat.reshape((1, 9)), d.site_xmat) np.testing.assert_allclose(dx.site_xmat.reshape((1, 9)), d.site_xmat)
# tendon data is correct # tendon data is correct
np.testing.assert_allclose(dx.ten_length, d.ten_length) np.testing.assert_allclose(dx._impl.ten_length, d.ten_length)
np.testing.assert_equal(dx.ten_wrapadr, np.zeros((1,))) np.testing.assert_equal(dx._impl.ten_wrapadr, np.zeros((1,)))
np.testing.assert_equal(dx.ten_wrapnum, np.zeros((1,))) np.testing.assert_equal(dx._impl.ten_wrapnum, np.zeros((1,)))
np.testing.assert_equal(dx.wrap_obj, np.zeros((2, 2))) np.testing.assert_equal(dx._impl.wrap_obj, np.zeros((2, 2)))
np.testing.assert_equal(dx.wrap_xpos, np.zeros((2, 6))) np.testing.assert_equal(dx._impl.wrap_xpos, np.zeros((2, 6)))
# efc_ are also shape transformed and padded # efc_ are also shape transformed and padded
self.assertEqual(dx.efc_J.shape, (45, 8)) # nefc, nv self.assertEqual(dx._impl.efc_J.shape, (45, 8)) # nefc, nv
d_efc_j = d.efc_J.reshape((-1, 8)) d_efc_j = d.efc_J.reshape((-1, 8))
np.testing.assert_allclose(dx.efc_J[:3], d_efc_j[:3]) # connect eq np.testing.assert_allclose(dx._impl.efc_J[:3], d_efc_j[:3]) # connect eq
np.testing.assert_allclose(dx.efc_J[3], d_efc_j[3]) # one active limit np.testing.assert_allclose(
np.testing.assert_allclose(dx.efc_J[4], 0) # one inactive limit dx._impl.efc_J[3], d_efc_j[3]
np.testing.assert_allclose(dx.efc_J[5:15], d_efc_j[4:14]) # contact ) # one active limit
np.testing.assert_allclose(dx.efc_J[15:], 0) # no contact np.testing.assert_allclose(dx._impl.efc_J[4], 0) # one inactive limit
np.testing.assert_allclose(dx._impl.efc_J[5:15], d_efc_j[4:14]) # contact
np.testing.assert_allclose(dx._impl.efc_J[15:], 0) # no contact
# check another efc_ too # check another efc_ too
self.assertEqual(dx.efc_aref.shape, (45,)) # nefc self.assertEqual(dx._impl.efc_aref.shape, (45,)) # nefc
np.testing.assert_allclose(dx.efc_aref[:3], d.efc_aref[:3]) np.testing.assert_allclose(dx._impl.efc_aref[:3], d.efc_aref[:3])
np.testing.assert_allclose(dx.efc_aref[3], d.efc_aref[3]) np.testing.assert_allclose(dx._impl.efc_aref[3], d.efc_aref[3])
np.testing.assert_allclose(dx.efc_aref[4], 0) np.testing.assert_allclose(dx._impl.efc_aref[4], 0)
np.testing.assert_allclose(dx.efc_aref[5:15], d.efc_aref[4:14]) np.testing.assert_allclose(dx._impl.efc_aref[5:15], d.efc_aref[4:14])
np.testing.assert_allclose(dx.efc_aref[15:], 0) np.testing.assert_allclose(dx._impl.efc_aref[15:], 0)
# check sparse transform is correct # check sparse transform is correct
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx_sparse = mjx.put_data(m, d) dx_sparse = mjx.put_data(m, d, backend_impl=backend_impl)
np.testing.assert_allclose(dx_sparse.efc_J, dx.efc_J, atol=1e-8) np.testing.assert_allclose(dx_sparse._impl.efc_J, dx._impl.efc_J, atol=1e-8)
# check sparse mass matrices are correct # check sparse mass matrices are correct
np.testing.assert_allclose(dx_sparse.qM, d.qM, atol=1e-8) np.testing.assert_allclose(dx_sparse._impl.qM, d.qM, atol=1e-8)
np.testing.assert_allclose(dx_sparse.qLD, d.qLD, atol=1e-8) np.testing.assert_allclose(dx_sparse._impl.qLD, d.qLD, atol=1e-8)
np.testing.assert_allclose(dx_sparse.qLDiagInv, d.qLDiagInv, atol=1e-8) np.testing.assert_allclose(
dx_sparse._impl.qLDiagInv, d.qLDiagInv, atol=1e-8
)
# check dense mass matrices are correct # check dense mass matrices are correct
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_DENSE m.opt.jacobian = mujoco.mjtJacobian.mjJAC_DENSE
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx_from_dense = mjx.put_data(m, d) dx_from_dense = mjx.put_data(m, d, backend_impl=backend_impl)
qm = np.zeros((m.nv, m.nv)) if backend_impl == 'jax':
mujoco.mj_fullM(m, qm, d.qM) qm = np.zeros((m.nv, m.nv))
np.testing.assert_allclose(dx_from_dense.qM, qm, atol=1e-8) mujoco.mj_fullM(m, qm, d.qM)
np.testing.assert_allclose(dx_from_dense._impl.qM, qm, atol=1e-8)
elif backend_impl == 'c':
np.testing.assert_allclose(dx_from_dense._impl.qM, d.qM, atol=1e-8)
def test_get_data(self): @parameterized.parameters('jax', 'c')
def test_get_data(self, backend_impl: str):
"""Test that get_data makes correct MjData.""" """Test that get_data makes correct MjData."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS)
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx = mjx.put_data(m, d) dx = mjx.put_data(m, d, backend_impl=backend_impl)
d_2: mujoco.MjData = mjx.get_data(m, dx) d_2: mujoco.MjData = mjx.get_data(m, dx)
# check a few fields # check a few fields
@@ -424,6 +469,10 @@ class DataIOTest(parameterized.TestCase):
np.testing.assert_allclose(d_2.efc_aref, d.efc_aref) np.testing.assert_allclose(d_2.efc_aref, d.efc_aref)
np.testing.assert_allclose(d_2.contact.efc_address, d.contact.efc_address) np.testing.assert_allclose(d_2.contact.efc_address, d.contact.efc_address)
if backend_impl == 'c':
# check fields specific to the C implementation
np.testing.assert_allclose(d_2.M_rownnz, d.M_rownnz)
def test_get_data_runs(self): def test_get_data_runs(self):
xml = """ xml = """
<mujoco> <mujoco>
@@ -444,13 +493,14 @@ class DataIOTest(parameterized.TestCase):
dx = mjx.put_data(m, d) dx = mjx.put_data(m, d)
mjx.get_data(m, dx) mjx.get_data(m, dx)
def test_get_data_batched(self): @parameterized.parameters('jax', 'c')
def test_get_data_batched(self, backend_impl):
"""Test that get_data makes correct List[MjData] for batched Data.""" """Test that get_data makes correct List[MjData] for batched Data."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS)
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx = mjx.put_data(m, d) dx = mjx.put_data(m, d, backend_impl=backend_impl)
# second data in batch has contact dist > 0, disables contact # second data in batch has contact dist > 0, disables contact
dx_b = jax.tree_util.tree_map(lambda x: jp.stack((x, x + 0.05)), dx) dx_b = jax.tree_util.tree_map(lambda x: jp.stack((x, x + 0.05)), dx)
ds = mjx.get_data(m, dx_b) ds = mjx.get_data(m, dx_b)
@@ -460,13 +510,14 @@ class DataIOTest(parameterized.TestCase):
self.assertEqual(ds[0].ncon, 1) self.assertEqual(ds[0].ncon, 1)
self.assertEqual(ds[1].ncon, 0) self.assertEqual(ds[1].ncon, 0)
def test_get_data_into(self): @parameterized.parameters('jax', 'c')
def test_get_data_into(self, backend_impl):
"""Test that get_data_into correctly populates an MjData.""" """Test that get_data_into correctly populates an MjData."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS)
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx = mjx.put_data(m, d) dx = mjx.put_data(m, d, backend_impl=backend_impl)
d_2 = mujoco.MjData(m) d_2 = mujoco.MjData(m)
mjx.get_data_into(d_2, m, dx) mjx.get_data_into(d_2, m, dx)
@@ -482,32 +533,33 @@ class DataIOTest(parameterized.TestCase):
self.assertEqual(d_2.contact.frame.shape, (1, 9)) self.assertEqual(d_2.contact.frame.shape, (1, 9))
np.testing.assert_allclose(d_2.contact.frame, d.contact.frame) np.testing.assert_allclose(d_2.contact.frame, d.contact.frame)
def test_get_data_into_wrong_shape(self): @parameterized.parameters('jax', 'c')
def test_get_data_into_wrong_shape(self, backend_impl):
"""Tests that get_data_into throwsif input and output shapes don't match.""" """Tests that get_data_into throwsif input and output shapes don't match."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS)
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx = mjx.put_data(m, d) dx = mjx.put_data(m, d, backend_impl=backend_impl)
m_2 = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS) m_2 = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS)
d_2 = mujoco.MjData(m_2) d_2 = mujoco.MjData(m_2)
with self.assertRaisesRegex(ValueError, r'Input field.*has shape.*'): with self.assertRaisesRegex(ValueError, r'Input field.*has shape.*'):
mjx.get_data_into(d_2, m, dx) mjx.get_data_into(d_2, m, dx)
def test_make_matches_put(self): @parameterized.parameters('jax', 'c')
def test_make_matches_put(self, backend_impl):
"""Test that make_data produces a pytree that matches put_data.""" """Test that make_data produces a pytree that matches put_data."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS)
d = mujoco.MjData(m) d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2) mujoco.mj_step(m, d, 2)
dx = mjx.put_data(m, d) dx = mjx.put_data(m, d, backend_impl=backend_impl)
step_fn = lambda d: d.replace(time=d.time + 1) step_fn = lambda d: d.replace(time=d.time + 1)
step_fn_jit = jax.jit(step_fn).lower(dx).compile() step_fn_jit = jax.jit(step_fn).lower(dx).compile()
# placing an MjData onto device should yield the same treedef mjx.Data as # placing an MjData onto device should yield the same treedef mjx.Data as
# calling make_data. they should be interchangeable for jax functions: # calling make_data. they should be interchangeable for jax functions:
step_fn_jit(mjx.make_data(m)) step_fn_jit(mjx.make_data(m, backend_impl=backend_impl))
def test_contact_elliptic_condim1(self): def test_contact_elliptic_condim1(self):
"""Test that condim=1 with ConeType.ELLIPTIC is not implemented.""" """Test that condim=1 with ConeType.ELLIPTIC is not implemented."""
@@ -556,7 +608,327 @@ class DataIOTest(parameterized.TestCase):
</mujoco> </mujoco>
""") """)
with self.assertRaises(NotImplementedError): with self.assertRaises(NotImplementedError):
mjx.put_model(m) mjx.put_model(m, backend_impl='jax')
class FullCompatTest(parameterized.TestCase):
"""Tests for the _full_compat flag."""
def test_full_compat_deprecated(self):
"""Tests that _full_compat is deprecated."""
xml = """
<mujoco>
<worldbody>
<body name="box">
<joint name="slide1" type="slide" axis="1 0 0" />
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide1"/>
</actuator>
</mujoco>
"""
m = mujoco.MjModel.from_xml_string(xml)
with self.assertWarns(DeprecationWarning):
out = mjx_io.put_model(m, _full_compat=True)
self.assertEqual(out.backend_impl, BackendImpl.C)
with self.assertWarns(DeprecationWarning):
out = mjx_io.make_data(m, _full_compat=True)
self.assertEqual(out.backend_impl, BackendImpl.C)
# Test cases for `_resolve_backend_impl_and_device` where the device is
# specified by the user and the device is available.
_DEVICE_TEST_CASES = [
# Arguments use the following format:
# (device_type_str, backend_impl_str,
# (expected_device, expected_backend_impl)))
# No backend specified.
('cpu', None, ('cpu', BackendImpl.C)),
('gpu-notnvidia', None, ('gpu', BackendImpl.JAX)),
('gpu-nvidia', None, ('gpu', BackendImpl.WARP)),
('tpu', None, ('tpu', BackendImpl.JAX)),
# JAX backend specified.
('cpu', 'jax', ('cpu', BackendImpl.JAX)),
('gpu-notnvidia', 'jax', ('gpu', BackendImpl.JAX)),
('gpu-nvidia', 'jax', ('gpu', BackendImpl.JAX)),
('tpu', 'jax', ('tpu', BackendImpl.JAX)),
# WARP backend specified.
('cpu', 'warp', ('cpu', 'error')),
('gpu-notnvidia', 'warp', ('cpu', 'error')),
('gpu-nvidia', 'warp', ('gpu', BackendImpl.WARP)),
('tpu', 'warp', ('tpu', 'error')),
# C backend specified.
('cpu', 'c', ('cpu', BackendImpl.C)),
('gpu-notnvidia', 'c', ('cpu', 'error')),
('gpu-nvidia', 'c', ('cpu', 'error')),
('tpu', 'c', ('tpu', 'error')),
]
# Test cases for `_resolve_backend_impl_and_device` where the user does NOT
# specify a device. We mock the JAX default device.
_DEFAULT_DEVICE_TEST_CASES = [
# Arguments use the following format:
# (jax.default_device, backend_impl_str,
# (expected_device, expected_backend_impl))
# No backend impl specified.
('cpu', None, ('cpu', BackendImpl.C)),
('gpu-notnvidia', None, ('gpu', BackendImpl.JAX)),
('gpu-nvidia', None, ('gpu', BackendImpl.WARP)),
('tpu', None, ('tpu', BackendImpl.JAX)),
# JAX backend impl specified.
('cpu', 'jax', ('cpu', BackendImpl.JAX)),
('gpu-notnvidia', 'jax', ('gpu', BackendImpl.JAX)),
('gpu-nvidia', 'jax', ('gpu', BackendImpl.JAX)),
('tpu', 'jax', ('tpu', BackendImpl.JAX)),
# WARP backend impl specified.
('cpu', 'warp', ('cpu', 'error')),
('gpu-notnvidia', 'warp', ('cpu', 'error')),
('gpu-nvidia', 'warp', ('gpu', BackendImpl.WARP)),
('tpu', 'warp', ('tpu', 'error')),
# C backend impl specified, CPU should always be available.
('cpu', 'c', ('cpu', BackendImpl.C)),
('gpu-notnvidia', 'c', ('cpu', BackendImpl.C)),
('gpu-nvidia', 'c', ('cpu', BackendImpl.C)),
('tpu', 'c', ('cpu', BackendImpl.C)),
]
class ResolveBackendImplAndDeviceTest(parameterized.TestCase):
"""Tests for the _resolve_backend_impl_and_device function."""
def setUp(self):
super().setUp()
# Create mock devices
self.mock_cpu = mock.Mock(spec=jax.Device)
self.mock_cpu.platform = 'cpu'
self.mock_cpu.device_kind = 'Mock CPU'
self.mock_cpu.id = 0
self.mock_nvidia_gpu = mock.Mock(spec=jax.Device)
self.mock_nvidia_gpu.platform = 'gpu'
self.mock_nvidia_gpu.device_kind = 'NVIDIA Mocked GPU'
self.mock_nvidia_gpu.id = 0
self.mock_other_gpu = mock.Mock(spec=jax.Device)
self.mock_other_gpu.platform = 'gpu'
self.mock_other_gpu.device_kind = 'Other Mocked GPU'
self.mock_other_gpu.id = 1
self.mock_tpu = mock.Mock(spec=jax.Device)
self.mock_tpu.platform = 'tpu'
self.mock_tpu.device_kind = 'Mock TPU'
self.mock_tpu.id = 0
# Patch jax.devices for the entire test class using enter_context
self.mock_jax_devices = self.enter_context(mock.patch('jax.devices'))
self.mock_default_backend = self.enter_context(
mock.patch('jax.default_backend')
)
@parameterized.named_parameters(
(f'{str(args[0])}_{str(args[1])}', *args) for args in _DEVICE_TEST_CASES
)
@mock.patch.dict(
os.environ, {'MJX_WARP_ENABLED': 'true', 'MJX_C_DEFAULT_ENABLED': 'true'}
)
def test_resolve_with_device(
self,
device_type_str,
backend_impl_str,
expected,
):
"""Tests various combinations of device and backend impls."""
input_device = {
'cpu': self.mock_cpu,
'gpu-nvidia': self.mock_nvidia_gpu,
'gpu-notnvidia': self.mock_other_gpu,
'tpu': self.mock_tpu,
}[device_type_str]
def devices_side_effect(backend=None):
# assume the user-specified device is always available
if backend == 'cpu':
return [self.mock_cpu]
elif backend == 'gpu':
if 'nvidia' in device_type_str:
return [self.mock_nvidia_gpu]
return [self.mock_other_gpu]
elif backend == 'tpu':
return [self.mock_tpu]
elif backend == 'cuda':
return [self.mock_nvidia_gpu]
raise AssertionError('Should not be called.')
self.mock_jax_devices.side_effect = devices_side_effect
expected_device, expected_backend_impl = expected
if expected_backend_impl == 'error':
with self.assertRaises(AssertionError):
mjx_io._resolve_backend_impl_and_device(
backend_impl=backend_impl_str, device=input_device
)
return
actual_backend_impl, actual_device = (
mjx_io._resolve_backend_impl_and_device(
backend_impl=backend_impl_str, device=input_device
)
)
self.assertEqual(actual_backend_impl, expected_backend_impl)
self.assertIsNotNone(actual_device)
self.assertEqual(actual_device.platform, expected_device)
@parameterized.named_parameters(
(f'{str(args[0])}_{str(args[1])}', *args)
for args in _DEFAULT_DEVICE_TEST_CASES
)
@mock.patch.dict(
os.environ, {'MJX_WARP_ENABLED': 'true', 'MJX_C_DEFAULT_ENABLED': 'true'}
)
def test_resolve_without_device(
self,
default_device_str,
backend_impl_str,
expected,
):
"""Tests various combinations of jax.default_device and backend impls."""
default_devices = {
'cpu': [self.mock_cpu],
'gpu-nvidia': [self.mock_nvidia_gpu, self.mock_cpu],
'gpu-notnvidia': [self.mock_other_gpu, self.mock_cpu],
'tpu': [self.mock_tpu, self.mock_cpu],
}[default_device_str]
def devices_side_effect(backend=None):
if backend == 'cpu':
return [self.mock_cpu] # CPU is always available
if backend == 'gpu' and default_device_str == 'gpu-notnvidia':
return [self.mock_other_gpu]
if backend == 'gpu' and default_device_str == 'gpu-nvidia':
return [self.mock_nvidia_gpu]
if backend == 'cuda' and default_device_str == 'gpu-nvidia':
return [self.mock_nvidia_gpu]
if backend == 'tpu' and default_device_str == 'tpu':
return [self.mock_tpu]
if backend is None:
return default_devices
if backend == 'cuda':
raise RuntimeError('cuda backend not supported')
raise AssertionError('jax.devices error')
self.mock_jax_devices.side_effect = devices_side_effect
default_device_side_effect_str = {
'cpu': 'cpu',
'gpu-nvidia': 'gpu',
'gpu-notnvidia': 'gpu',
'tpu': 'tpu',
}[default_device_str]
self.mock_default_backend.side_effect = (
lambda: default_device_side_effect_str
)
expected_device, expected_backend_impl = expected
if (
expected_backend_impl == 'error'
and default_device_str != 'gpu-nvidia'
and backend_impl_str == 'warp'
):
with self.assertRaisesRegex(RuntimeError, 'cuda backend not supported'):
mjx_io._resolve_backend_impl_and_device(
backend_impl=backend_impl_str, device=None
)
return
if expected_backend_impl == 'error':
with self.assertRaises(AssertionError):
mjx_io._resolve_backend_impl_and_device(
backend_impl=backend_impl_str, device=None
)
return
actual_backend_impl, actual_device = (
mjx_io._resolve_backend_impl_and_device(
backend_impl=backend_impl_str, device=None
)
)
self.assertEqual(actual_backend_impl, expected_backend_impl)
self.assertIsNotNone(actual_device)
self.assertEqual(actual_device.platform, expected_device)
@mock.patch.dict(os.environ, {'MJX_WARP_ENABLED': 'false'})
def test_resolve_warp_disabled(self):
"""Tests behavior when MJX_WARP_ENABLED is false."""
self.mock_jax_devices.side_effect = lambda backend=None: (
[self.mock_nvidia_gpu, self.mock_cpu]
if backend is None
else ([self.mock_nvidia_gpu] if backend == 'gpu' else [self.mock_cpu])
)
self.mock_default_backend.side_effect = lambda: 'gpu'
# Default to JAX instead of WARP on NVIDIA GPU.
backend_impl, device = mjx_io._resolve_backend_impl_and_device(
backend_impl=None, device=None
)
self.assertEqual(backend_impl, BackendImpl.JAX)
self.assertEqual(device.platform, 'gpu')
# Specifying an NVIDIA GPU should still choose JAX.
backend_impl, device = mjx_io._resolve_backend_impl_and_device(
backend_impl=None, device=self.mock_nvidia_gpu
)
self.assertEqual(backend_impl, BackendImpl.JAX)
self.assertEqual(device.platform, 'gpu')
# Requesting warp explicitly should fail since it is disabled.
with self.assertRaises(AssertionError):
mjx_io._resolve_backend_impl_and_device(
backend_impl='warp', device=self.mock_nvidia_gpu
)
with self.assertRaises(AssertionError):
mjx_io._resolve_backend_impl_and_device(backend_impl='warp', device=None)
@mock.patch.dict(os.environ, {'MJX_C_DEFAULT_ENABLED': 'false'})
def test_resolve_c_disabled(self):
"""Tests behavior when MJX_C_DEFAULT_ENABLED is false."""
# Users expect that CPU defaults to the JAX impl. But in the future, it will
# default to the C backend implementation. This test checks that
# MJX_C_DEFAULT_ENABLED=false defaults to the old behavior, until the
# migration to MJEP-15 is complete.
self.mock_jax_devices.side_effect = lambda backend=None: ([self.mock_cpu])
self.mock_default_backend.side_effect = lambda: 'cpu'
# Default to JAX instead of C on CPU.
backend_impl, device = mjx_io._resolve_backend_impl_and_device(
backend_impl=None, device=None
)
self.assertEqual(backend_impl, BackendImpl.JAX)
self.assertEqual(device.platform, 'cpu')
# Specifing CPU should still choose JAX.
backend_impl, device = mjx_io._resolve_backend_impl_and_device(
backend_impl=None, device=self.mock_cpu
)
self.assertEqual(backend_impl, BackendImpl.JAX)
self.assertEqual(device.platform, 'cpu')
# Specifying C should choose C!
backend_impl, device = mjx_io._resolve_backend_impl_and_device(
backend_impl='c', device=None
)
self.assertEqual(backend_impl, BackendImpl.C)
self.assertEqual(device.platform, 'cpu')
backend_impl, device = mjx_io._resolve_backend_impl_and_device(
backend_impl='c', device=self.mock_cpu
)
self.assertEqual(backend_impl, BackendImpl.C)
self.assertEqual(device.platform, 'cpu')
if __name__ == '__main__': if __name__ == '__main__':
+10 -4
View File
@@ -23,9 +23,12 @@ from mujoco.mjx._src import scan
from mujoco.mjx._src import support from mujoco.mjx._src import support
# pylint: disable=g-importing-member # pylint: disable=g-importing-member
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import DisableBit
from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import JointType
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
from mujoco.mjx._src.types import OptionJAX
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
@@ -72,11 +75,11 @@ def _spring_damper(m: Model, d: Data) -> jax.Array:
qfrc -= m.dof_damping * d.qvel qfrc -= m.dof_damping * d.qvel
# tendon-level spring-dampers # tendon-level spring-dampers
below, above = m.tendon_lengthspring.T - d.ten_length below, above = m.tendon_lengthspring.T - d._impl.ten_length
frc_spring = jp.where(below > 0, m.tendon_stiffness * below, 0) frc_spring = jp.where(below > 0, m.tendon_stiffness * below, 0)
frc_spring = jp.where(above < 0, m.tendon_stiffness * above, frc_spring) frc_spring = jp.where(above < 0, m.tendon_stiffness * above, frc_spring)
frc_damper = -m.tendon_damping * d.ten_velocity frc_damper = -m.tendon_damping * d._impl.ten_velocity
qfrc += d.ten_J.T @ (frc_spring + frc_damper) qfrc += d._impl.ten_J.T @ (frc_spring + frc_damper)
return qfrc return qfrc
@@ -113,6 +116,9 @@ def _fluid(m: Model, d: Data) -> jax.Array:
def passive(m: Model, d: Data) -> Data: def passive(m: Model, d: Data) -> Data:
"""Adds all passive forces.""" """Adds all passive forces."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('passive requires JAX backend implementation.')
if m.opt.disableflags & DisableBit.PASSIVE: if m.opt.disableflags & DisableBit.PASSIVE:
return d.replace(qfrc_passive=jp.zeros(m.nv), qfrc_gravcomp=jp.zeros(m.nv)) return d.replace(qfrc_passive=jp.zeros(m.nv), qfrc_gravcomp=jp.zeros(m.nv))
@@ -124,7 +130,7 @@ def passive(m: Model, d: Data) -> Data:
# add gravcomp unless added via actuators # add gravcomp unless added via actuators
qfrc_passive += qfrc_gravcomp * (1 - m.jnt_actgravcomp[m.dof_jntid]) qfrc_passive += qfrc_gravcomp * (1 - m.jnt_actgravcomp[m.dof_jntid])
if m.opt.has_fluid_params: if m.opt.has_fluid_params: # pytype: disable=attribute-error
qfrc_passive += _fluid(m, d) qfrc_passive += _fluid(m, d)
d = d.replace(qfrc_passive=qfrc_passive, qfrc_gravcomp=qfrc_gravcomp) d = d.replace(qfrc_passive=qfrc_passive, qfrc_gravcomp=qfrc_gravcomp)
+35 -24
View File
@@ -22,9 +22,12 @@ from mujoco.mjx._src import math
from mujoco.mjx._src import ray from mujoco.mjx._src import ray
from mujoco.mjx._src import smooth from mujoco.mjx._src import smooth
from mujoco.mjx._src import support from mujoco.mjx._src import support
from mujoco.mjx._src.types import BackendImpl
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import DisableBit
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
from mujoco.mjx._src.types import ObjType from mujoco.mjx._src.types import ObjType
from mujoco.mjx._src.types import SensorType from mujoco.mjx._src.types import SensorType
from mujoco.mjx._src.types import TrnType from mujoco.mjx._src.types import TrnType
@@ -32,7 +35,7 @@ from mujoco.mjx._src.types import TrnType
import numpy as np import numpy as np
def apply_cutoff( def _apply_cutoff(
sensor: jax.Array, cutoff: jax.Array, data_type: int sensor: jax.Array, cutoff: jax.Array, data_type: int
) -> jax.Array: ) -> jax.Array:
"""Clip sensor to cutoff value.""" """Clip sensor to cutoff value."""
@@ -51,6 +54,8 @@ def apply_cutoff(
def sensor_pos(m: Model, d: Data) -> Data: def sensor_pos(m: Model, d: Data) -> Data:
"""Compute position-dependent sensors values.""" """Compute position-dependent sensors values."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('sensor_pos requires JAX backend implementation.')
if m.opt.disableflags & DisableBit.SENSOR: if m.opt.disableflags & DisableBit.SENSOR:
return d return d
@@ -163,15 +168,15 @@ def sensor_pos(m: Model, d: Data) -> Data:
sensor, _ = jax.vmap( sensor, _ = jax.vmap(
ray.ray, in_axes=(None, None, 0, 0, None, None, None) ray.ray, in_axes=(None, None, 0, 0, None, None, None)
)(m, d, site_xpos, site_mat, (), True, sid) )(m, d, site_xpos, site_mat, (), True, sid)
sensors.append(apply_cutoff(sensor, cutoffs, data_type[0])) sensors.append(_apply_cutoff(sensor, cutoffs, data_type[0]))
adrs.append(adr[idxs]) adrs.append(adr[idxs])
continue # avoid adding to sensors/adrs list a second time continue # avoid adding to sensors/adrs list a second time
elif sensor_type == SensorType.JOINTPOS: elif sensor_type == SensorType.JOINTPOS:
sensor = d.qpos[m.jnt_qposadr[objid]] sensor = d.qpos[m.jnt_qposadr[objid]]
elif sensor_type == SensorType.TENDONPOS: elif sensor_type == SensorType.TENDONPOS:
sensor = d.ten_length[objid] sensor = d._impl.ten_length[objid]
elif sensor_type == SensorType.ACTUATORPOS: elif sensor_type == SensorType.ACTUATORPOS:
sensor = d.actuator_length[objid] sensor = d._impl.actuator_length[objid]
elif sensor_type == SensorType.BALLQUAT: elif sensor_type == SensorType.BALLQUAT:
jnt_qposadr = m.jnt_qposadr[objid, None] + np.arange(4)[None] jnt_qposadr = m.jnt_qposadr[objid, None] + np.arange(4)[None]
quat = d.qpos[jnt_qposadr] quat = d.qpos[jnt_qposadr]
@@ -194,7 +199,7 @@ def sensor_pos(m: Model, d: Data) -> Data:
cutofft = cutoff[idxt] cutofft = cutoff[idxt]
sensor = jax.vmap(_framepos)(xpos, xpos_ref, xmat_ref, refidt) sensor = jax.vmap(_framepos)(xpos, xpos_ref, xmat_ref, refidt)
adrt = adr[idxt, None] + np.arange(3)[None] adrt = adr[idxt, None] + np.arange(3)[None]
sensors.append(apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1)) sensors.append(_apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1))
adrs.append(adrt.reshape(-1)) adrs.append(adrt.reshape(-1))
continue # avoid adding to sensors/adrs list a second time continue # avoid adding to sensors/adrs list a second time
elif sensor_type in frame_axis: elif sensor_type in frame_axis:
@@ -214,7 +219,7 @@ def sensor_pos(m: Model, d: Data) -> Data:
cutofft = cutoff[idxt] cutofft = cutoff[idxt]
sensor = jax.vmap(_frameaxis)(xmat, xmat_ref, refidt) sensor = jax.vmap(_frameaxis)(xmat, xmat_ref, refidt)
adrt = adr[idxt, None] + np.arange(3)[None] adrt = adr[idxt, None] + np.arange(3)[None]
sensors.append(apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1)) sensors.append(_apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1))
adrs.append(adrt.reshape(-1)) adrs.append(adrt.reshape(-1))
continue # avoid adding to sensors/adrs list a second time continue # avoid adding to sensors/adrs list a second time
elif sensor_type == SensorType.FRAMEQUAT: elif sensor_type == SensorType.FRAMEQUAT:
@@ -255,7 +260,7 @@ def sensor_pos(m: Model, d: Data) -> Data:
) )
)(quat, refquat, refidt) )(quat, refquat, refidt)
adrt = adr[idxt, None] + np.arange(4)[None] adrt = adr[idxt, None] + np.arange(4)[None]
sensors.append(apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1)) sensors.append(_apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1))
adrs.append(adrt.reshape(-1)) adrs.append(adrt.reshape(-1))
continue # avoid adding to sensors/adrs list a second time continue # avoid adding to sensors/adrs list a second time
elif sensor_type == SensorType.SUBTREECOM: elif sensor_type == SensorType.SUBTREECOM:
@@ -267,7 +272,7 @@ def sensor_pos(m: Model, d: Data) -> Data:
# TODO(taylorhowell): raise error after adding sensor check to io.py # TODO(taylorhowell): raise error after adding sensor check to io.py
continue # unsupported sensor type continue # unsupported sensor type
sensors.append(apply_cutoff(sensor, cutoff, data_type[0]).reshape(-1)) sensors.append(_apply_cutoff(sensor, cutoff, data_type[0]).reshape(-1))
adrs.append(adr) adrs.append(adr)
if not adrs: if not adrs:
@@ -282,6 +287,8 @@ def sensor_pos(m: Model, d: Data) -> Data:
def sensor_vel(m: Model, d: Data) -> Data: def sensor_vel(m: Model, d: Data) -> Data:
"""Compute velocity-dependent sensors values.""" """Compute velocity-dependent sensors values."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('sensor_vel requires JAX backend implementation.')
if m.opt.disableflags & DisableBit.SENSOR: if m.opt.disableflags & DisableBit.SENSOR:
return d return d
@@ -333,9 +340,9 @@ def sensor_vel(m: Model, d: Data) -> Data:
elif sensor_type == SensorType.JOINTVEL: elif sensor_type == SensorType.JOINTVEL:
sensor = d.qvel[m.jnt_dofadr[objid]] sensor = d.qvel[m.jnt_dofadr[objid]]
elif sensor_type == SensorType.TENDONVEL: elif sensor_type == SensorType.TENDONVEL:
sensor = d.ten_velocity[objid] sensor = d._impl.ten_velocity[objid]
elif sensor_type == SensorType.ACTUATORVEL: elif sensor_type == SensorType.ACTUATORVEL:
sensor = d.actuator_velocity[objid] sensor = d._impl.actuator_velocity[objid]
elif sensor_type == SensorType.BALLANGVEL: elif sensor_type == SensorType.BALLANGVEL:
jnt_dotadr = m.jnt_dofadr[objid, None] + np.arange(3)[None] jnt_dotadr = m.jnt_dofadr[objid, None] + np.arange(3)[None]
sensor = d.qvel[jnt_dotadr] sensor = d.qvel[jnt_dotadr]
@@ -393,20 +400,20 @@ def sensor_vel(m: Model, d: Data) -> Data:
adrt = adr[idxt, None] + np.arange(3)[None] adrt = adr[idxt, None] + np.arange(3)[None]
sensors.append(apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1)) sensors.append(_apply_cutoff(sensor, cutofft, data_type[0]).reshape(-1))
adrs.append(adrt.reshape(-1)) adrs.append(adrt.reshape(-1))
continue # avoid adding to sensors/adrs list a second time continue # avoid adding to sensors/adrs list a second time
elif sensor_type == SensorType.SUBTREELINVEL: elif sensor_type == SensorType.SUBTREELINVEL:
sensor = d.subtree_linvel[objid] sensor = d._impl.subtree_linvel[objid]
adr = (adr[:, None] + np.arange(3)[None]).reshape(-1) adr = (adr[:, None] + np.arange(3)[None]).reshape(-1)
elif sensor_type == SensorType.SUBTREEANGMOM: elif sensor_type == SensorType.SUBTREEANGMOM:
sensor = d.subtree_angmom[objid] sensor = d._impl.subtree_angmom[objid]
adr = (adr[:, None] + np.arange(3)[None]).reshape(-1) adr = (adr[:, None] + np.arange(3)[None]).reshape(-1)
else: else:
# TODO(taylorhowell): raise error after adding sensor check to io.py # TODO(taylorhowell): raise error after adding sensor check to io.py
continue # unsupported sensor type continue # unsupported sensor type
sensors.append(apply_cutoff(sensor, cutoff, data_type[0]).reshape(-1)) sensors.append(_apply_cutoff(sensor, cutoff, data_type[0]).reshape(-1))
adrs.append(adr) adrs.append(adr)
if not adrs: if not adrs:
@@ -421,6 +428,8 @@ def sensor_vel(m: Model, d: Data) -> Data:
def sensor_acc(m: Model, d: Data) -> Data: def sensor_acc(m: Model, d: Data) -> Data:
"""Compute acceleration/force-dependent sensors values.""" """Compute acceleration/force-dependent sensors values."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('sensor_acc requires JAX backend implementation.')
if m.opt.disableflags & DisableBit.SENSOR: if m.opt.disableflags & DisableBit.SENSOR:
return d return d
@@ -460,14 +469,14 @@ def sensor_acc(m: Model, d: Data) -> Data:
# compute contact forces # compute contact forces
forces = [] forces = []
condim_ids = [] condim_ids = []
for dim in set(d.contact.dim): for dim in set(d._impl.contact.dim):
force, condim_id = support.contact_force_dim(m, d, dim) force, condim_id = support.contact_force_dim(m, d, dim)
forces.append(force) forces.append(force)
condim_ids.append(condim_id) condim_ids.append(condim_id)
forces = jp.concatenate(forces)[np.argsort(np.concatenate(condim_ids))] forces = jp.concatenate(forces)[np.argsort(np.concatenate(condim_ids))]
# get bodies of contact geoms # get bodies of contact geoms
conbody = jp.array(m.geom_bodyid)[d.contact.geom] conbody = jp.array(m.geom_bodyid)[d._impl.contact.geom]
# get site information # get site information
site_bodyid = m.site_bodyid[objid] site_bodyid = m.site_bodyid[objid]
@@ -477,12 +486,14 @@ def sensor_acc(m: Model, d: Data) -> Data:
site_type = m.site_type[objid] site_type = m.site_type[objid]
conbody0 = site_bodyid[:, None] == conbody[:, 0] conbody0 = site_bodyid[:, None] == conbody[:, 0]
conbody1 = site_bodyid[:, None] == conbody[:, 1] conbody1 = site_bodyid[:, None] == conbody[:, 1]
contacts = (d.contact.efc_address >= 0)[None] & (conbody0 | conbody1) contacts = (d._impl.contact.efc_address >= 0)[None] & (
conbody0 | conbody1
)
# compute conray, flip if second body # compute conray, flip if second body
conray = jax.vmap( conray = jax.vmap(
lambda frame, force: math.normalize(frame[0] * force[0]) lambda frame, force: math.normalize(frame[0] * force[0])
)(d.contact.frame, forces) )(d._impl.contact.frame, forces)
conray = jp.where(conbody1[..., None], -conray, conray) conray = jp.where(conbody1[..., None], -conray, conray)
# compute distance, mapping over sites and contacts # compute distance, mapping over sites and contacts
@@ -504,7 +515,7 @@ def sensor_acc(m: Model, d: Data) -> Data:
site_xpos[dist_id_site], site_xpos[dist_id_site],
site_xmat[dist_id_site], site_xmat[dist_id_site],
st, st,
d.contact.pos, d._impl.contact.pos,
conray[dist_id_site], conray[dist_id_site],
) )
dist.append(jp.where(jp.isinf(dist_site), 0, dist_site)) dist.append(jp.where(jp.isinf(dist_site), 0, dist_site))
@@ -526,14 +537,14 @@ def sensor_acc(m: Model, d: Data) -> Data:
bodyid = m.site_bodyid[objid] bodyid = m.site_bodyid[objid]
rot = d.site_xmat[objid] rot = d.site_xmat[objid]
cvel = d.cvel[bodyid] cvel = d.cvel[bodyid]
cacc = d.cacc[bodyid] cacc = d._impl.cacc[bodyid]
dif = d.site_xpos[objid] - d.subtree_com[m.body_rootid[bodyid]] dif = d.site_xpos[objid] - d.subtree_com[m.body_rootid[bodyid]]
sensor = _accelerometer(cvel, cacc, dif, rot) sensor = _accelerometer(cvel, cacc, dif, rot)
adr = (adr[:, None] + np.arange(3)[None]).reshape(-1) adr = (adr[:, None] + np.arange(3)[None]).reshape(-1)
elif sensor_type == SensorType.FORCE: elif sensor_type == SensorType.FORCE:
bodyid = m.site_bodyid[objid] bodyid = m.site_bodyid[objid]
cfrc_int = d.cfrc_int[bodyid] cfrc_int = d._impl.cfrc_int[bodyid]
site_xmat = d.site_xmat[objid] site_xmat = d.site_xmat[objid]
sensor = jax.vmap(lambda mat, vec: mat.T @ vec)( sensor = jax.vmap(lambda mat, vec: mat.T @ vec)(
site_xmat, cfrc_int[:, 3:] site_xmat, cfrc_int[:, 3:]
@@ -542,7 +553,7 @@ def sensor_acc(m: Model, d: Data) -> Data:
elif sensor_type == SensorType.TORQUE: elif sensor_type == SensorType.TORQUE:
bodyid = m.site_bodyid[objid] bodyid = m.site_bodyid[objid]
rootid = m.body_rootid[bodyid] rootid = m.body_rootid[bodyid]
cfrc_int = d.cfrc_int[bodyid] cfrc_int = d._impl.cfrc_int[bodyid]
site_xmat = d.site_xmat[objid] site_xmat = d.site_xmat[objid]
dif = d.site_xpos[objid] - d.subtree_com[rootid] dif = d.site_xpos[objid] - d.subtree_com[rootid]
sensor = jax.vmap( sensor = jax.vmap(
@@ -571,7 +582,7 @@ def sensor_acc(m: Model, d: Data) -> Data:
pos, bodyid = objtype_data[ot] pos, bodyid = objtype_data[ot]
pos = pos[objidt] pos = pos[objidt]
bodyid = bodyid[objidt] bodyid = bodyid[objidt]
cacc = d.cacc[bodyid] cacc = d._impl.cacc[bodyid]
if sensor_type == SensorType.FRAMELINACC: if sensor_type == SensorType.FRAMELINACC:
@@ -601,7 +612,7 @@ def sensor_acc(m: Model, d: Data) -> Data:
# TODO(taylorhowell): raise error after adding sensor check to io.py # TODO(taylorhowell): raise error after adding sensor check to io.py
continue # unsupported sensor type continue # unsupported sensor type
sensors.append(apply_cutoff(sensor, cutoff, data_type[0]).reshape(-1)) sensors.append(_apply_cutoff(sensor, cutoff, data_type[0]).reshape(-1))
adrs.append(adr) adrs.append(adr)
if not adrs: if not adrs:
+8 -8
View File
@@ -64,14 +64,14 @@ class SensorTest(parameterized.TestCase):
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
mx = mjx.put_model(m) mx = mjx.put_model(m)
dx = mjx.put_data(m, d).replace( dx = mjx.put_data(m, d).tree_replace({
sensordata=jp.zeros_like(d.sensordata), 'sensordata': jp.zeros_like(d.sensordata),
subtree_linvel=jp.zeros_like(d.subtree_linvel), '_impl.subtree_linvel': jp.zeros_like(d.subtree_linvel),
subtree_angmom=jp.zeros_like(d.subtree_angmom), '_impl.subtree_angmom': jp.zeros_like(d.subtree_angmom),
cacc=jp.zeros_like(d.cacc), '_impl.cacc': jp.zeros_like(d.cacc),
cfrc_int=jp.zeros_like(d.cfrc_int), '_impl.cfrc_int': jp.zeros_like(d.cfrc_int),
cfrc_ext=jp.zeros_like(d.cfrc_ext), '_impl.cfrc_ext': jp.zeros_like(d.cfrc_ext),
) })
dx = jax.jit(mjx.sensor_pos)(mx, dx) dx = jax.jit(mjx.sensor_pos)(mx, dx)
dx = jax.jit(mjx.sensor_vel)(mx, dx) dx = jax.jit(mjx.sensor_vel)(mx, dx)
dx = jax.jit(mjx.sensor_acc)(mx, dx) dx = jax.jit(mjx.sensor_acc)(mx, dx)
+89 -60
View File
@@ -23,10 +23,12 @@ from mujoco.mjx._src import support
# pylint: disable=g-importing-member # pylint: disable=g-importing-member
from mujoco.mjx._src.types import CamLightType from mujoco.mjx._src.types import CamLightType
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import DisableBit
from mujoco.mjx._src.types import EqType from mujoco.mjx._src.types import EqType
from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import JointType
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
from mujoco.mjx._src.types import TrnType from mujoco.mjx._src.types import TrnType
from mujoco.mjx._src.types import WrapType from mujoco.mjx._src.types import WrapType
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
@@ -35,7 +37,6 @@ import numpy as np
def kinematics(m: Model, d: Data) -> Data: def kinematics(m: Model, d: Data) -> Data:
"""Converts position/velocity from generalized coordinates to maximal.""" """Converts position/velocity from generalized coordinates to maximal."""
def fn(carry, jnt_typs, jnt_pos, jnt_axis, qpos, qpos0, pos, quat): def fn(carry, jnt_typs, jnt_pos, jnt_axis, qpos, qpos0, pos, quat):
# calculate joint anchors, axes, body pos and quat in global frame # calculate joint anchors, axes, body pos and quat in global frame
# also normalize qpos while we're at it # also normalize qpos while we're at it
@@ -131,6 +132,8 @@ def kinematics(m: Model, d: Data) -> Data:
def com_pos(m: Model, d: Data) -> Data: def com_pos(m: Model, d: Data) -> Data:
"""Maps inertias and motion dofs to global frame centered at subtree-CoM.""" """Maps inertias and motion dofs to global frame centered at subtree-CoM."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('com_pos requires JAX backend implementation.')
# calculate center of mass of each subtree # calculate center of mass of each subtree
def subtree_sum(carry, xipos, body_mass): def subtree_sum(carry, xipos, body_mass):
@@ -162,7 +165,7 @@ def com_pos(m: Model, d: Data) -> Data:
root_com = subtree_com[m.body_rootid] root_com = subtree_com[m.body_rootid]
offset = d.xipos - root_com offset = d.xipos - root_com
cinert = inert_com(m.body_inertia, d.ximat, offset, m.body_mass) cinert = inert_com(m.body_inertia, d.ximat, offset, m.body_mass)
d = d.replace(cinert=cinert) d = d.tree_replace({'_impl.cinert': cinert})
# map motion dofs to global frame centered at subtree_com # map motion dofs to global frame centered at subtree_com
def cdof_fn(jnt_typs, root_com, xmat, xanchor, xaxis): def cdof_fn(jnt_typs, root_com, xmat, xanchor, xaxis):
@@ -201,13 +204,16 @@ def com_pos(m: Model, d: Data) -> Data:
d.xanchor, d.xanchor,
d.xaxis, d.xaxis,
) )
d = d.replace(cdof=cdof) d = d.tree_replace({'_impl.cdof': cdof})
return d return d
def camlight(m: Model, d: Data) -> Data: def camlight(m: Model, d: Data) -> Data:
"""Computes camera and light positions and orientations.""" """Computes camera and light positions and orientations."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('camlight requires JAX backend implementation.')
if m.ncam == 0: if m.ncam == 0:
return d.replace(cam_xpos=jp.zeros((0, 3)), cam_xmat=jp.zeros((0, 3, 3))) return d.replace(cam_xpos=jp.zeros((0, 3)), cam_xmat=jp.zeros((0, 3, 3)))
@@ -278,32 +284,33 @@ def camlight(m: Model, d: Data) -> Data:
def crb(m: Model, d: Data) -> Data: def crb(m: Model, d: Data) -> Data:
"""Runs composite rigid body inertia algorithm.""" """Runs composite rigid body inertia algorithm."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('crb requires JAX backend implementation.')
def crb_fn(crb_child, crb_body): def crb_fn(crb_child, crb_body):
if crb_child is not None: if crb_child is not None:
crb_body += crb_child crb_body += crb_child
return crb_body return crb_body
crb_body = scan.body_tree(m, crb_fn, 'b', 'b', d.cinert, reverse=True) crb_body = scan.body_tree(m, crb_fn, 'b', 'b', d._impl.cinert, reverse=True)
crb_body = crb_body.at[0].set(0.0) crb_body = crb_body.at[0].set(0.0)
d = d.replace(crb=crb_body) d = d.tree_replace({'_impl.crb': crb_body})
crb_dof = jp.take(crb_body, jp.array(m.dof_bodyid), axis=0) crb_dof = jp.take(crb_body, jp.array(m.dof_bodyid), axis=0)
crb_cdof = jax.vmap(math.inert_mul)(crb_dof, d.cdof) crb_cdof = jax.vmap(math.inert_mul)(crb_dof, d._impl.cdof)
qm = support.make_m(m, crb_cdof, d.cdof, m.dof_armature) qm = support.make_m(m, crb_cdof, d._impl.cdof, m.dof_armature)
d = d.replace(qM=qm) d = d.tree_replace({'_impl.qM': qm})
if support.is_sparse(m) and d._qM_sparse.size > 0: # pylint: disable=protected-access
d = d.replace(_qM_sparse=qm)
return d return d
def factor_m(m: Model, d: Data) -> Data: def factor_m(m: Model, d: Data) -> Data:
"""Gets factorizaton of inertia-like matrix M, assumed spd.""" """Gets factorizaton of inertia-like matrix M, assumed spd."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('factor_m requires JAX backend implementation.')
if not support.is_sparse(m): if not support.is_sparse(m):
qh, _ = jax.scipy.linalg.cho_factor(d.qM) qh, _ = jax.scipy.linalg.cho_factor(d._impl.qM)
d = d.replace(qLD=qh) d = d.tree_replace({'_impl.qLD': qh})
return d return d
# build up indices for where we will do backwards updates over qLD # build up indices for where we will do backwards updates over qLD
@@ -325,7 +332,7 @@ def factor_m(m: Model, d: Data) -> Data:
(out_beg, out_end, madr_d, madr_ij) (out_beg, out_end, madr_d, madr_ij)
) )
qld = d.qM qld = d._impl.qM
for _, updates in sorted(updates.items(), reverse=True): for _, updates in sorted(updates.items(), reverse=True):
# combine the updates into one update batch (per depth level) # combine the updates into one update batch (per depth level)
@@ -353,20 +360,17 @@ def factor_m(m: Model, d: Data) -> Data:
qld_diag = qld[m.dof_Madr] qld_diag = qld[m.dof_Madr]
qld = (qld / qld[jp.array(madr_ds)]).at[m.dof_Madr].set(qld_diag) qld = (qld / qld[jp.array(madr_ds)]).at[m.dof_Madr].set(qld_diag)
d = d.replace(qLD=qld, qLDiagInv=1 / qld_diag) d = d.tree_replace({'_impl.qLD': qld, '_impl.qLDiagInv': 1 / qld_diag})
if d._qLD_sparse.size > 0: # pylint: disable=protected-access
d = d.replace(_qLD_sparse=d.qLD)
if d._qLDiagInv_sparse.size > 0: # pylint: disable=protected-access
d = d.replace(_qLDiagInv_sparse=d.qLDiagInv)
return d return d
def solve_m(m: Model, d: Data, x: jax.Array) -> jax.Array: def solve_m(m: Model, d: Data, x: jax.Array) -> jax.Array:
"""Computes sparse backsubstitution: x = inv(L'*D*L)*y .""" """Computes sparse backsubstitution: x = inv(L'*D*L)*y ."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('solve_m requires JAX backend implementation.')
if not support.is_sparse(m): if not support.is_sparse(m):
return jax.scipy.linalg.cho_solve((d.qLD, False), x) return jax.scipy.linalg.cho_solve((d._impl.qLD, False), x)
depth = [] depth = []
for i in range(m.nv): for i in range(m.nv):
@@ -385,21 +389,23 @@ def solve_m(m: Model, d: Data, x: jax.Array) -> jax.Array:
# x <- inv(L') * x # x <- inv(L') * x
for _, vals in sorted(updates_j.items(), reverse=True): for _, vals in sorted(updates_j.items(), reverse=True):
j, madr_ij, i = np.array(vals).T j, madr_ij, i = np.array(vals).T
x = x.at[j].add(-d.qLD[madr_ij] * x[i]) x = x.at[j].add(-d._impl.qLD[madr_ij] * x[i])
# x <- inv(D) * x # x <- inv(D) * x
x = x * d.qLDiagInv x = x * d._impl.qLDiagInv
# x <- inv(L) * x # x <- inv(L) * x
for _, vals in sorted(updates_i.items()): for _, vals in sorted(updates_i.items()):
i, madr_ij, j = np.array(vals).T i, madr_ij, j = np.array(vals).T
x = x.at[i].add(-d.qLD[madr_ij] * x[j]) x = x.at[i].add(-d._impl.qLD[madr_ij] * x[j])
return x return x
def com_vel(m: Model, d: Data) -> Data: def com_vel(m: Model, d: Data) -> Data:
"""Computes cvel, cdof_dot.""" """Computes cvel, cdof_dot."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('com_vel requires JAX backend implementation.')
# forward scan down tree: accumulate link center of mass velocity # forward scan down tree: accumulate link center of mass velocity
def fn(parent, jnt_typs, cdof, qvel): def fn(parent, jnt_typs, cdof, qvel):
@@ -431,17 +437,19 @@ def com_vel(m: Model, d: Data) -> Data:
'jvv', 'jvv',
'bv', 'bv',
m.jnt_type, m.jnt_type,
d.cdof, d._impl.cdof,
d.qvel, d.qvel,
) )
d = d.replace(cvel=cvel, cdof_dot=cdof_dot) d = d.tree_replace({'cvel': cvel, '_impl.cdof_dot': cdof_dot})
return d return d
def subtree_vel(m: Model, d: Data) -> Data: def subtree_vel(m: Model, d: Data) -> Data:
"""Subtree linear velocity and angular momentum.""" """Subtree linear velocity and angular momentum."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('subtree_vel requires JAX backend implementation.')
# bodywise quantities # bodywise quantities
def _forward(cvel, xipos, ximat, subtree_com_root, mass, inertia): def _forward(cvel, xipos, ximat, subtree_com_root, mass, inertia):
@@ -529,7 +537,10 @@ def subtree_vel(m: Model, d: Data) -> Data:
reverse=True, reverse=True,
) )
return d.replace(subtree_linvel=subtree_linvel, subtree_angmom=subtree_angmom) return d.tree_replace({
'_impl.subtree_linvel': subtree_linvel,
'_impl.subtree_angmom': subtree_angmom,
})
def rne(m: Model, d: Data, flg_acc: bool = False) -> Data: def rne(m: Model, d: Data, flg_acc: bool = False) -> Data:
@@ -537,6 +548,8 @@ def rne(m: Model, d: Data, flg_acc: bool = False) -> Data:
flg_acc=False removes inertial term. flg_acc=False removes inertial term.
""" """
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('rne requires JAX backend implementation.')
# forward scan over tree: accumulate link center of mass acceleration # forward scan over tree: accumulate link center of mass acceleration
def cacc_fn(cacc, cdof_dot, qvel, cdof, qacc): def cacc_fn(cacc, cdof_dot, qvel, cdof, qacc):
@@ -555,7 +568,7 @@ def rne(m: Model, d: Data, flg_acc: bool = False) -> Data:
return cacc return cacc
cacc = scan.body_tree( cacc = scan.body_tree(
m, cacc_fn, 'vvvv', 'b', d.cdof_dot, d.qvel, d.cdof, d.qacc m, cacc_fn, 'vvvv', 'b', d._impl.cdof_dot, d.qvel, d._impl.cdof, d.qacc
) )
def frc(cinert, cacc, cvel): def frc(cinert, cacc, cvel):
@@ -564,7 +577,7 @@ def rne(m: Model, d: Data, flg_acc: bool = False) -> Data:
return frc return frc
loc_cfrc = jax.vmap(frc)(d.cinert, cacc, d.cvel) loc_cfrc = jax.vmap(frc)(d._impl.cinert, cacc, d.cvel)
# backward scan up tree: accumulate body forces # backward scan up tree: accumulate body forces
def cfrc_fn(cfrc_child, cfrc): def cfrc_fn(cfrc_child, cfrc):
@@ -573,7 +586,7 @@ def rne(m: Model, d: Data, flg_acc: bool = False) -> Data:
return cfrc return cfrc
cfrc = scan.body_tree(m, cfrc_fn, 'b', 'b', loc_cfrc, reverse=True) cfrc = scan.body_tree(m, cfrc_fn, 'b', 'b', loc_cfrc, reverse=True)
qfrc_bias = jax.vmap(jp.dot)(d.cdof, cfrc[jp.array(m.dof_bodyid)]) qfrc_bias = jax.vmap(jp.dot)(d._impl.cdof, cfrc[jp.array(m.dof_bodyid)])
d = d.replace(qfrc_bias=qfrc_bias) d = d.replace(qfrc_bias=qfrc_bias)
@@ -582,6 +595,8 @@ def rne(m: Model, d: Data, flg_acc: bool = False) -> Data:
def rne_postconstraint(m: Model, d: Data) -> Data: def rne_postconstraint(m: Model, d: Data) -> Data:
"""RNE with complete data: compute cacc, cfrc_ext, cfrc_int.""" """RNE with complete data: compute cacc, cfrc_ext, cfrc_int."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('rne_postconstraint requires JAX backend implementation.')
def _transform_force(frc, offset): def _transform_force(frc, offset):
force, torque = jp.split(frc, 2) force, torque = jp.split(frc, 2)
@@ -602,7 +617,7 @@ def rne_postconstraint(m: Model, d: Data) -> Data:
# compute contact forces for each condim # compute contact forces for each condim
forces = [] forces = []
condim_idx = [] condim_idx = []
for dim in set(d.contact.dim): for dim in set(d._impl.contact.dim):
force, idx = support.contact_force_dim(m, d, dim) force, idx = support.contact_force_dim(m, d, dim)
forces.append(force) forces.append(force)
condim_idx.append(idx) condim_idx.append(idx)
@@ -629,10 +644,10 @@ def rne_postconstraint(m: Model, d: Data) -> Data:
) )
condim_idx = jp.concatenate(condim_idx) condim_idx = jp.concatenate(condim_idx)
frame = d.contact.frame[condim_idx] frame = d._impl.contact.frame[condim_idx]
pos = d.contact.pos[condim_idx] pos = d._impl.contact.pos[condim_idx]
id1 = jp.array(m.geom_bodyid)[d.contact.geom[condim_idx, 0]] id1 = jp.array(m.geom_bodyid)[d._impl.contact.geom[condim_idx, 0]]
id2 = jp.array(m.geom_bodyid)[d.contact.geom[condim_idx, 1]] id2 = jp.array(m.geom_bodyid)[d._impl.contact.geom[condim_idx, 1]]
com1 = d.subtree_com[jp.array(m.body_rootid)][id1] com1 = d.subtree_com[jp.array(m.body_rootid)][id1]
com2 = d.subtree_com[jp.array(m.body_rootid)][id2] com2 = d.subtree_com[jp.array(m.body_rootid)][id2]
@@ -668,8 +683,8 @@ def rne_postconstraint(m: Model, d: Data) -> Data:
) )
# cacc = cacc_parent + cdofdot * qvel + cdof * qacc # cacc = cacc_parent + cdofdot * qvel + cdof * qacc
cacc_vel = d.cdof_dot.T @ (mask * d.qvel) cacc_vel = d._impl.cdof_dot.T @ (mask * d.qvel)
cacc_acc = d.cdof.T @ (mask * d.qacc) cacc_acc = d._impl.cdof.T @ (mask * d.qacc)
cacc = cacc_parent + cacc_vel + cacc_acc cacc = cacc_parent + cacc_vel + cacc_acc
# cfrc_body = cinert * cacc + cvel x (cinert * cvel) # cfrc_body = cinert * cacc + cvel x (cinert * cvel)
@@ -687,7 +702,7 @@ def rne_postconstraint(m: Model, d: Data) -> Data:
'bbbbb', 'bbbbb',
'bb', 'bb',
cfrc_ext, cfrc_ext,
d.cinert, d._impl.cinert,
d.cvel, d.cvel,
jp.array(m.body_dofadr), jp.array(m.body_dofadr),
jp.array(m.body_dofnum), jp.array(m.body_dofnum),
@@ -704,11 +719,18 @@ def rne_postconstraint(m: Model, d: Data) -> Data:
) )
# update data # update data
return d.replace(cacc=cacc, cfrc_int=cfrc_int, cfrc_ext=cfrc_ext) return d.tree_replace({
'_impl.cacc': cacc,
'_impl.cfrc_int': cfrc_int,
'_impl.cfrc_ext': cfrc_ext,
})
def tendon(m: Model, d: Data) -> Data: def tendon(m: Model, d: Data) -> Data:
"""Computes tendon lengths and moments.""" """Computes tendon lengths and moments."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('tendon requires JAX backend implementation.')
if not m.ntendon: if not m.ntendon:
return d return d
@@ -836,8 +858,8 @@ def tendon(m: Model, d: Data) -> Data:
# wrap inside # wrap inside
# TODO(taylorhowell): check that is_wrap_inside is consistent with # TODO(taylorhowell): check that is_wrap_inside is consistent with
# site and geom relative positions # site and geom relative positions
(wrap_inside_id,) = np.nonzero(m.is_wrap_inside) (wrap_inside_id,) = np.nonzero(m._impl.is_wrap_inside)
(wrap_outside_id,) = np.nonzero(~m.is_wrap_inside) (wrap_outside_id,) = np.nonzero(~m._impl.is_wrap_inside)
# compute geom wrap length and connect points (if wrap occurs) # compute geom wrap length and connect points (if wrap occurs)
v_wrap = jax.vmap( v_wrap = jax.vmap(
@@ -853,9 +875,9 @@ def tendon(m: Model, d: Data) -> Data:
has_sidesite[wrap_inside_id], has_sidesite[wrap_inside_id],
is_sphere[wrap_inside_id], is_sphere[wrap_inside_id],
True, True,
m.wrap_inside_maxiter, m._impl.wrap_inside_maxiter,
m.wrap_inside_tolerance, m._impl.wrap_inside_tolerance,
m.wrap_inside_z_init, m._impl.wrap_inside_z_init,
) )
lengths_outside, pnt0_outside, pnt1_outside = v_wrap( lengths_outside, pnt0_outside, pnt1_outside = v_wrap(
@@ -868,9 +890,9 @@ def tendon(m: Model, d: Data) -> Data:
has_sidesite[wrap_outside_id], has_sidesite[wrap_outside_id],
is_sphere[wrap_outside_id], is_sphere[wrap_outside_id],
False, False,
m.wrap_inside_maxiter, m._impl.wrap_inside_maxiter,
m.wrap_inside_tolerance, m._impl.wrap_inside_tolerance,
m.wrap_inside_z_init, m._impl.wrap_inside_z_init,
) )
wrap_id = np.argsort(np.concatenate([wrap_inside_id, wrap_outside_id])) wrap_id = np.argsort(np.concatenate([wrap_inside_id, wrap_outside_id]))
@@ -952,12 +974,14 @@ def tendon(m: Model, d: Data) -> Data:
) )
# assemble length and moment # assemble length and moment
ten_length = jp.zeros_like(d.ten_length).at[tendon_id_jnt].set(length_jnt) ten_length = (
jp.zeros_like(d._impl.ten_length).at[tendon_id_jnt].set(length_jnt)
)
ten_length = ten_length.at[tendon_id_site].add(length_site) ten_length = ten_length.at[tendon_id_site].add(length_site)
ten_length = ten_length.at[tendon_id_geom].add(length_geom) ten_length = ten_length.at[tendon_id_geom].add(length_geom)
ten_moment = ( ten_moment = (
jp.zeros_like(d.ten_J) jp.zeros_like(d._impl.ten_J)
.at[adr_moment_jnt, dofadr_moment_jnt] .at[adr_moment_jnt, dofadr_moment_jnt]
.set(moment_jnt) .set(moment_jnt)
) )
@@ -1020,14 +1044,14 @@ def tendon(m: Model, d: Data) -> Data:
[wrap_obj[sort], jp.zeros(2 * m.nwrap - count, dtype=int)] [wrap_obj[sort], jp.zeros(2 * m.nwrap - count, dtype=int)]
).reshape((m.nwrap, 2)) ).reshape((m.nwrap, 2))
return d.replace( return d.tree_replace({
ten_length=ten_length, '_impl.ten_length': ten_length,
ten_J=ten_moment, '_impl.ten_J': ten_moment,
ten_wrapadr=jp.array(ten_wrapadr, dtype=int), '_impl.ten_wrapadr': jp.array(ten_wrapadr, dtype=int),
ten_wrapnum=jp.array(ten_wrapnum, dtype=int), '_impl.ten_wrapnum': jp.array(ten_wrapnum, dtype=int),
wrap_xpos=wrap_xpos, '_impl.wrap_xpos': wrap_xpos,
wrap_obj=jp.array(wrap_obj, dtype=int), '_impl.wrap_obj': jp.array(wrap_obj, dtype=int),
) })
def _site_dof_mask(m: Model) -> np.ndarray: def _site_dof_mask(m: Model) -> np.ndarray:
@@ -1061,6 +1085,9 @@ 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."""
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('transmission requires JAX backend implementation.')
if not m.nu: if not m.nu:
return d return d
@@ -1120,8 +1147,8 @@ def transmission(m: Model, d: Data) -> Data:
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: elif trntype == TrnType.TENDON:
length = d.ten_length[trnid[0]] * gear[:1] length = d._impl.ten_length[trnid[0]] * gear[:1]
moment = d.ten_J[trnid[0]] * gear[0] moment = d._impl.ten_J[trnid[0]] * gear[0]
else: else:
raise RuntimeError(f'unrecognized trntype: {TrnType(trntype)}') raise RuntimeError(f'unrecognized trntype: {TrnType(trntype)}')
@@ -1153,5 +1180,7 @@ def transmission(m: Model, d: Data) -> Data:
length = length.reshape((m.nu,)) length = length.reshape((m.nu,))
moment = moment.reshape((m.nu, m.nv)) moment = moment.reshape((m.nu, m.nv))
d = d.replace(actuator_length=length, actuator_moment=moment) d = d.tree_replace(
{'_impl.actuator_length': length, '_impl.actuator_moment': moment}
)
return d return d
+25 -28
View File
@@ -20,7 +20,7 @@ import jax
import mujoco import mujoco
from mujoco import mjx from mujoco import mjx
from mujoco.mjx._src import test_util from mujoco.mjx._src import test_util
from mujoco.mjx._src.types import ConeType from mujoco.mjx._src.types import ConeType # pylint: disable=g-importing-member
import numpy as np import numpy as np
# tolerance for difference between MuJoCo and MJX smooth calculations - mostly # tolerance for difference between MuJoCo and MJX smooth calculations - mostly
@@ -78,30 +78,27 @@ class SmoothTest(absltest.TestCase):
# com_pos # com_pos
dx = jax.jit(mjx.com_pos)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.com_pos)(mx, mjx.put_data(m, d))
_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._impl, 'cinert')
_assert_attr_eq(d, dx, 'cdof') _assert_attr_eq(d, dx._impl, 'cdof')
# camlight # camlight
dx = jax.jit(mjx.camlight)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.camlight)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'cam_xpos') _assert_attr_eq(d, dx, 'cam_xpos')
_assert_eq(d.cam_xmat.reshape((-1, 3, 3)), dx.cam_xmat, 'cam_xmat') _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._impl, 'crb')
_assert_attr_eq(d, dx, 'qM') _assert_attr_eq(d, dx._impl, 'qM')
_assert_eq(dx._qM_sparse, np.zeros(0), '_qM_sparse')
# factor_m # factor_m
dx = jax.jit(mjx.factor_m)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.factor_m)(mx, mjx.put_data(m, d))
qLDLegacy = np.zeros(mx.nM) # pylint:disable=invalid-name qLDLegacy = np.zeros(mx.nM) # pylint:disable=invalid-name
for i in range(m.nM): for i in range(m.nM):
qLDLegacy[d.mapM2M[i]] = d.qLD[i] qLDLegacy[d.mapM2M[i]] = d.qLD[i]
_assert_eq(qLDLegacy, dx.qLD, 'qLD') _assert_eq(qLDLegacy, dx._impl.qLD, 'qLD')
_assert_attr_eq(d, dx, 'qLDiagInv') _assert_attr_eq(d, dx._impl, 'qLDiagInv')
_assert_eq(dx._qLD_sparse, np.zeros(0), '_qLD_sparse')
_assert_eq(dx._qLDiagInv_sparse, np.zeros(0), '_qLDiagInv_sparse')
# com_vel # com_vel
dx = jax.jit(mjx.com_vel)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.com_vel)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'cvel') _assert_attr_eq(d, dx, 'cvel')
_assert_attr_eq(d, dx, 'cdof_dot') _assert_attr_eq(d, dx._impl, 'cdof_dot')
# 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')
@@ -122,11 +119,11 @@ class SmoothTest(absltest.TestCase):
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
# tendon # tendon
dx = jax.jit(mjx.tendon)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.tendon)(mx, mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'ten_J') _assert_attr_eq(d, dx._impl, 'ten_J')
_assert_attr_eq(d, dx, 'ten_length') _assert_attr_eq(d, dx._impl, 'ten_length')
# transmission # transmission
dx = jax.jit(mjx.transmission)(mx, dx) dx = jax.jit(mjx.transmission)(mx, dx)
_assert_attr_eq(d, dx, 'actuator_length') _assert_attr_eq(d, dx._impl, 'actuator_length')
# convert sparse actuator_moment to dense representation # convert sparse actuator_moment to dense representation
moment = np.zeros((m.nu, m.nv)) moment = np.zeros((m.nu, m.nv))
@@ -137,7 +134,7 @@ class SmoothTest(absltest.TestCase):
d.moment_rowadr, d.moment_rowadr,
d.moment_colind, d.moment_colind,
) )
_assert_eq(moment, dx.actuator_moment, 'actuator_moment') _assert_eq(moment, dx._impl.actuator_moment, 'actuator_moment')
def test_disable_gravity(self): def test_disable_gravity(self):
m = mujoco.MjModel.from_xml_string(""" m = mujoco.MjModel.from_xml_string("""
@@ -197,7 +194,7 @@ class SmoothTest(absltest.TestCase):
mujoco.mj_transmission(m, d) mujoco.mj_transmission(m, d)
dx = jax.jit(mjx.transmission)(mx, dx) dx = jax.jit(mjx.transmission)(mx, dx)
_assert_attr_eq(d, dx, 'actuator_length') _assert_attr_eq(d, dx._impl, 'actuator_length')
# convert sparse actuator_moment to dense representation # convert sparse actuator_moment to dense representation
moment = np.zeros((m.nu, m.nv)) moment = np.zeros((m.nu, m.nv))
@@ -208,7 +205,7 @@ class SmoothTest(absltest.TestCase):
d.moment_rowadr, d.moment_rowadr,
d.moment_colind, d.moment_colind,
) )
_assert_eq(moment, dx.actuator_moment, 'actuator_moment') _assert_eq(moment, dx._impl.actuator_moment, 'actuator_moment')
def test_subtree_vel(self): def test_subtree_vel(self):
"""Tests MJX subtree_vel function matches MuJoCo mj_subtreeVel.""" """Tests MJX subtree_vel function matches MuJoCo mj_subtreeVel."""
@@ -226,8 +223,8 @@ class SmoothTest(absltest.TestCase):
mujoco.mj_subtreeVel(m, d) mujoco.mj_subtreeVel(m, d)
dx = jax.jit(mjx.subtree_vel)(mx, dx) dx = jax.jit(mjx.subtree_vel)(mx, dx)
_assert_attr_eq(d, dx, 'subtree_linvel') _assert_attr_eq(d, dx._impl, 'subtree_linvel')
_assert_attr_eq(d, dx, 'subtree_angmom') _assert_attr_eq(d, dx._impl, 'subtree_angmom')
class RnePostConstraintTest(parameterized.TestCase): class RnePostConstraintTest(parameterized.TestCase):
@@ -278,9 +275,9 @@ class RnePostConstraintTest(parameterized.TestCase):
mujoco.mj_rnePostConstraint(m, d) mujoco.mj_rnePostConstraint(m, d)
dx = jax.jit(mjx.rne_postconstraint)(mx, dx) dx = jax.jit(mjx.rne_postconstraint)(mx, dx)
_assert_eq(d.cacc, dx.cacc, 'cacc') _assert_eq(d.cacc, dx._impl.cacc, 'cacc')
_assert_eq(d.cfrc_ext, dx.cfrc_ext, 'cfrc_ext') _assert_eq(d.cfrc_ext, dx._impl.cfrc_ext, 'cfrc_ext')
_assert_eq(d.cfrc_int, dx.cfrc_int, 'cfrc_int') _assert_eq(d.cfrc_int, dx._impl.cfrc_int, 'cfrc_int')
class TendonTest(parameterized.TestCase): class TendonTest(parameterized.TestCase):
@@ -312,12 +309,12 @@ class TendonTest(parameterized.TestCase):
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
dx = jax.jit(mjx.forward)(mx, dx) dx = jax.jit(mjx.forward)(mx, dx)
_assert_eq(d.ten_length, dx.ten_length, 'ten_length') _assert_eq(d.ten_length, dx._impl.ten_length, 'ten_length')
_assert_eq(d.ten_J, dx.ten_J, 'ten_J') _assert_eq(d.ten_J, dx._impl.ten_J, 'ten_J')
_assert_eq(d.ten_wrapnum, dx.ten_wrapnum, 'ten_wrapnum') _assert_eq(d.ten_wrapnum, dx._impl.ten_wrapnum, 'ten_wrapnum')
_assert_eq(d.ten_wrapadr, dx.ten_wrapadr, 'ten_wrapadr') _assert_eq(d.ten_wrapadr, dx._impl.ten_wrapadr, 'ten_wrapadr')
_assert_eq(d.wrap_obj, dx.wrap_obj, 'wrap_obj') _assert_eq(d.wrap_obj, dx._impl.wrap_obj, 'wrap_obj')
_assert_eq(d.wrap_xpos, dx.wrap_xpos, 'wrap_xpos') _assert_eq(d.wrap_xpos, dx._impl.wrap_xpos, 'wrap_xpos')
if __name__ == '__main__': if __name__ == '__main__':
+60 -41
View File
@@ -24,8 +24,10 @@ from mujoco.mjx._src import support
from mujoco.mjx._src.dataclasses import PyTreeNode from mujoco.mjx._src.dataclasses import PyTreeNode
from mujoco.mjx._src.types import ConeType from mujoco.mjx._src.types import ConeType
from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import DataJAX
from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import DisableBit
from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Model
from mujoco.mjx._src.types import ModelJAX
from mujoco.mjx._src.types import SolverType from mujoco.mjx._src.types import SolverType
# pylint: enable=g-importing-member # pylint: enable=g-importing-member
@@ -73,14 +75,19 @@ class Context(PyTreeNode):
@classmethod @classmethod
def create(cls, m: Model, d: Data, grad: bool = True) -> 'Context': def create(cls, m: Model, d: Data, grad: bool = True) -> 'Context':
jaref = d.efc_J @ d.qacc - d.efc_aref if not isinstance(d._impl, DataJAX):
raise ValueError(
'Constraint context requires JAX backend implementation.'
)
jaref = d._impl.efc_J @ d.qacc - d._impl.efc_aref
# TODO(robotics-team): determine nv at which sparse mul is faster # TODO(robotics-team): determine nv at which sparse mul is faster
ma = support.mul_m(m, d, d.qacc) ma = support.mul_m(m, d, d.qacc)
nv_0 = jp.zeros(m.nv) nv_0 = jp.zeros(m.nv)
fri = 0.0 fri = 0.0
if m.opt.cone == ConeType.ELLIPTIC: if m.opt.cone == ConeType.ELLIPTIC:
friction = d.contact.friction[d.contact.dim > 1] friction = d._impl.contact.friction[d._impl.contact.dim > 1]
dim = d.contact.dim[d.contact.dim > 1] dim = d._impl.contact.dim[d._impl.contact.dim > 1]
mu = friction[:, 0] / jp.sqrt(m.opt.impratio) mu = friction[:, 0] / jp.sqrt(m.opt.impratio)
fri = jp.concatenate((mu[:, None], friction), axis=1) fri = jp.concatenate((mu[:, None], friction), axis=1)
for condim in (3, 4, 6): for condim in (3, 4, 6):
@@ -90,7 +97,7 @@ class Context(PyTreeNode):
qacc=d.qacc, qacc=d.qacc,
qfrc_constraint=d.qfrc_constraint, qfrc_constraint=d.qfrc_constraint,
Jaref=jaref, Jaref=jaref,
efc_force=d.efc_force, efc_force=d._impl.efc_force,
Ma=ma, Ma=ma,
grad=nv_0, grad=nv_0,
Mgrad=nv_0, Mgrad=nv_0,
@@ -145,18 +152,20 @@ class _LSPoint(PyTreeNode):
) -> '_LSPoint': ) -> '_LSPoint':
"""Creates a linesearch point with first and second derivatives.""" """Creates a linesearch point with first and second derivatives."""
# roughly corresponds to CGEval in mujoco/src/engine/engine_solver.c # roughly corresponds to CGEval in mujoco/src/engine/engine_solver.c
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('LSPoint requires JAX backend implementation.')
cost, deriv_0, deriv_1 = 0.0, 0.0, 0.0 cost, deriv_0, deriv_1 = 0.0, 0.0, 0.0
quad_total = quad_gauss quad_total = quad_gauss
x = ctx.Jaref + alpha * jv x = ctx.Jaref + alpha * jv
active = (x < 0).at[: d.ne + d.nf].set(True) active = (x < 0).at[: d._impl.ne + d._impl.nf].set(True)
dof_fl, ten_fl = m.dof_hasfrictionloss, m.tendon_hasfrictionloss dof_fl, ten_fl = m._impl.dof_hasfrictionloss, m._impl.tendon_hasfrictionloss
if (dof_fl.any() or ten_fl.any()) and not ( if (dof_fl.any() or ten_fl.any()) and not (
m.opt.disableflags & DisableBit.FRICTIONLOSS m.opt.disableflags & DisableBit.FRICTIONLOSS
): ):
f = d.efc_frictionloss f = d._impl.efc_frictionloss
r = 1.0 / (d.efc_D + (d.efc_D == 0.0) * mujoco.mjMINVAL) r = 1.0 / (d._impl.efc_D + (d._impl.efc_D == 0.0) * mujoco.mjMINVAL)
rf, z = r * f, jp.zeros_like(f) rf, z = r * f, jp.zeros_like(f)
linear_neg = (x <= -rf)[:, None] linear_neg = (x <= -rf)[:, None]
linear_pos = (x >= rf)[:, None] linear_pos = (x >= rf)[:, None]
@@ -174,13 +183,13 @@ class _LSPoint(PyTreeNode):
middle_zone = (tsqr > 0) & (n < (mu * t)) & ((mu * n + t) > 0) middle_zone = (tsqr > 0) & (n < (mu * t)) & ((mu * n + t) > 0)
# quadratic cost for equality, friction, limits, frictionless contacts # quadratic cost for equality, friction, limits, frictionless contacts
dim1 = d.contact.efc_address[d.contact.dim == 1] dim1 = d._impl.contact.efc_address[d._impl.contact.dim == 1]
nefl = d.ne + d.nf + d.nl nefl = d._impl.ne + d._impl.nf + d._impl.nl
active = active.at[nefl:].set(False).at[dim1].set(active[dim1]) active = active.at[nefl:].set(False).at[dim1].set(active[dim1])
quad_efld = jax.vmap(jp.multiply)(quad, active) quad_efld = jax.vmap(jp.multiply)(quad, active)
quad_total += jp.sum(quad_efld, axis=0) quad_total += jp.sum(quad_efld, axis=0)
# elliptic bottom zone: quadratic cost # elliptic bottom zone: quadratic cost
efc_elliptic = d.contact.efc_address[d.contact.dim > 1] efc_elliptic = d._impl.contact.efc_address[d._impl.contact.dim > 1]
quad_c = jax.vmap(jp.multiply)(quad[efc_elliptic], bottom_zone) quad_c = jax.vmap(jp.multiply)(quad[efc_elliptic], bottom_zone)
quad_total += jp.sum(quad_c, axis=0) quad_total += jp.sum(quad_c, axis=0)
# elliptic middle zone # elliptic middle zone
@@ -254,17 +263,20 @@ def _update_constraint(m: Model, d: Data, ctx: Context) -> Context:
Returns: Returns:
context with new constraint force and costs context with new constraint force and costs
""" """
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('_update_constraint requires JAX backend implementation.')
# ne constraints are always active, nf are conditionally active, others are # ne constraints are always active, nf are conditionally active, others are
# non-negative constraints. # non-negative constraints.
active = (ctx.Jaref < 0).at[: d.ne + d.nf].set(True) active = (ctx.Jaref < 0).at[: d._impl.ne + d._impl.nf].set(True)
floss_force, floss_cost = jp.zeros(d.nefc), 0.0 floss_force, floss_cost = jp.zeros(d._impl.nefc), 0.0
dof_fl, ten_fl = m.dof_hasfrictionloss, m.tendon_hasfrictionloss dof_fl, ten_fl = m._impl.dof_hasfrictionloss, m._impl.tendon_hasfrictionloss
if (dof_fl.any() or ten_fl.any()) and not ( if (dof_fl.any() or ten_fl.any()) and not (
m.opt.disableflags & DisableBit.FRICTIONLOSS m.opt.disableflags & DisableBit.FRICTIONLOSS
): ):
f = d.efc_frictionloss f = d._impl.efc_frictionloss
r = 1.0 / (d.efc_D + (d.efc_D == 0.0) * mujoco.mjMINVAL) r = 1.0 / (d._impl.efc_D + (d._impl.efc_D == 0.0) * mujoco.mjMINVAL)
linear_neg = (ctx.Jaref <= -r * f) * (f > 0) linear_neg = (ctx.Jaref <= -r * f) * (f > 0)
linear_pos = (ctx.Jaref >= r * f) * (f > 0) linear_pos = (ctx.Jaref >= r * f) * (f > 0)
active = active & ~linear_neg & ~linear_pos active = active & ~linear_neg & ~linear_pos
@@ -274,13 +286,13 @@ def _update_constraint(m: Model, d: Data, ctx: Context) -> Context:
floss_cost = floss_cost.sum() floss_cost = floss_cost.sum()
if m.opt.cone == ConeType.PYRAMIDAL: if m.opt.cone == ConeType.PYRAMIDAL:
efc_force = d.efc_D * -ctx.Jaref * active + floss_force efc_force = d._impl.efc_D * -ctx.Jaref * active + floss_force
cost = 0.5 * jp.sum(d.efc_D * ctx.Jaref * ctx.Jaref * active) cost = 0.5 * jp.sum(d._impl.efc_D * ctx.Jaref * ctx.Jaref * active)
dm, u, h = 0.0, 0.0, 0.0 dm, u, h = 0.0, 0.0, 0.0
elif m.opt.cone == ConeType.ELLIPTIC: elif m.opt.cone == ConeType.ELLIPTIC:
friction = d.contact.friction[d.contact.dim > 1] friction = d._impl.contact.friction[d._impl.contact.dim > 1]
efc_address = d.contact.efc_address[d.contact.dim > 1] efc_address = d._impl.contact.efc_address[d._impl.contact.dim > 1]
dim = d.contact.dim[d.contact.dim > 1] dim = d._impl.contact.dim[d._impl.contact.dim > 1]
# to prevent out of range append zeros to ctx.Jaref # to prevent out of range append zeros to ctx.Jaref
slice_fn = jax.vmap( slice_fn = jax.vmap(
lambda x: jax.lax.dynamic_slice( lambda x: jax.lax.dynamic_slice(
@@ -297,12 +309,12 @@ def _update_constraint(m: Model, d: Data, ctx: Context) -> Context:
adr_i.extend(range(addr, addr + condim)) adr_i.extend(range(addr, addr + condim))
adr_j.extend([i] * condim) adr_j.extend([i] * condim)
active = active.at[jp.array(adr_i)].set(bottom_zone[jp.array(adr_j)]) active = active.at[jp.array(adr_i)].set(bottom_zone[jp.array(adr_j)])
efc_force = d.efc_D * -ctx.Jaref * active + floss_force efc_force = d._impl.efc_D * -ctx.Jaref * active + floss_force
cost = 0.5 * jp.sum(d.efc_D * ctx.Jaref * ctx.Jaref * active) cost = 0.5 * jp.sum(d._impl.efc_D * ctx.Jaref * ctx.Jaref * active)
# middle zone: cone # middle zone: cone
middle_zone = (t > 0) & (n < (mu * t)) & ((mu * n + t) > 0) middle_zone = (t > 0) & (n < (mu * t)) & ((mu * n + t) > 0)
dm = d.efc_D[efc_address] / jp.maximum( dm = d._impl.efc_D[efc_address] / jp.maximum(
mu * mu * (1 + mu * mu), mujoco.mjMINVAL mu * mu * (1 + mu * mu), mujoco.mjMINVAL
) )
nmt = n - mu * t nmt = n - mu * t
@@ -339,7 +351,7 @@ def _update_constraint(m: Model, d: Data, ctx: Context) -> Context:
else: else:
raise NotImplementedError(f'unsupported cone type: {m.opt.cone}') raise NotImplementedError(f'unsupported cone type: {m.opt.cone}')
qfrc_constraint = d.efc_J.T @ efc_force qfrc_constraint = d._impl.efc_J.T @ efc_force
gauss = 0.5 * jp.dot(ctx.Ma - d.qfrc_smooth, ctx.qacc - d.qacc_smooth) gauss = 0.5 * jp.dot(ctx.Ma - d.qfrc_smooth, ctx.qacc - d.qacc_smooth)
ctx = ctx.replace( ctx = ctx.replace(
qfrc_constraint=qfrc_constraint, qfrc_constraint=qfrc_constraint,
@@ -371,6 +383,8 @@ def _update_gradient(m: Model, d: Data, ctx: Context) -> Context:
Raises: Raises:
NotImplementedError: for unsupported solver type NotImplementedError: for unsupported solver type
""" """
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('_update_gradient requires JAX backend implementation.')
grad = ctx.Ma - d.qfrc_smooth - ctx.qfrc_constraint grad = ctx.Ma - d.qfrc_smooth - ctx.qfrc_constraint
@@ -378,16 +392,16 @@ def _update_gradient(m: Model, d: Data, ctx: Context) -> Context:
mgrad = smooth.solve_m(m, d, grad) mgrad = smooth.solve_m(m, d, grad)
elif m.opt.solver == SolverType.NEWTON: elif m.opt.solver == SolverType.NEWTON:
if m.opt.cone == ConeType.ELLIPTIC: if m.opt.cone == ConeType.ELLIPTIC:
cm = jp.diag(d.efc_D * ctx.active) cm = jp.diag(d._impl.efc_D * ctx.active)
efc_address = d.contact.efc_address[d.contact.dim > 1] efc_address = d._impl.contact.efc_address[d._impl.contact.dim > 1]
dim = d.contact.dim[d.contact.dim > 1] dim = d._impl.contact.dim[d._impl.contact.dim > 1]
# set efc of cone H along diagonal # set efc of cone H along diagonal
for i, (condim, addr) in enumerate(zip(dim, efc_address)): for i, (condim, addr) in enumerate(zip(dim, efc_address)):
h_cone = ctx.h[i, :condim, :condim] h_cone = ctx.h[i, :condim, :condim]
cm = cm.at[addr : addr + condim, addr : addr + condim].add(h_cone) cm = cm.at[addr : addr + condim, addr : addr + condim].add(h_cone)
h = d.efc_J.T @ cm @ d.efc_J h = d._impl.efc_J.T @ cm @ d._impl.efc_J
else: else:
h = (d.efc_J.T * d.efc_D * ctx.active) @ d.efc_J h = (d._impl.efc_J.T * d._impl.efc_D * ctx.active) @ d._impl.efc_J
h = support.full_m(m, d) + h h = support.full_m(m, d) + h
h_ = jax.scipy.linalg.cho_factor(h) h_ = jax.scipy.linalg.cho_factor(h)
mgrad = jax.scipy.linalg.cho_solve(h_, grad) mgrad = jax.scipy.linalg.cho_solve(h_, grad)
@@ -414,12 +428,15 @@ def _linesearch(m: Model, d: Data, ctx: Context) -> Context:
Returns: Returns:
updated context with new qacc, Ma, Jaref updated context with new qacc, Ma, Jaref
""" """
if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX):
raise ValueError('_lineasearch requires JAX backend implementation.')
smag = math.norm(ctx.search) * m.stat.meaninertia * max(1, m.nv) smag = math.norm(ctx.search) * m.stat.meaninertia * max(1, m.nv)
gtol = m.opt.tolerance * m.opt.ls_tolerance * smag gtol = m.opt.tolerance * m.opt.ls_tolerance * smag
# compute Mv, Jv # compute Mv, Jv
mv = support.mul_m(m, d, ctx.search) mv = support.mul_m(m, d, ctx.search)
jv = d.efc_J @ ctx.search jv = d._impl.efc_J @ ctx.search
# prepare quadratics # prepare quadratics
quad_gauss = jp.stack(( quad_gauss = jp.stack((
@@ -428,13 +445,15 @@ def _linesearch(m: Model, d: Data, ctx: Context) -> Context:
0.5 * jp.dot(ctx.search, mv), 0.5 * jp.dot(ctx.search, mv),
)) ))
quad = jp.stack((0.5 * ctx.Jaref * ctx.Jaref, jv * ctx.Jaref, 0.5 * jv * jv)) quad = jp.stack((0.5 * ctx.Jaref * ctx.Jaref, jv * ctx.Jaref, 0.5 * jv * jv))
quad = (quad * d.efc_D).T quad = (quad * d._impl.efc_D).T
uu, v0, uv, vv = 0.0, 0.0, 0.0, 0.0 uu, v0, uv, vv = 0.0, 0.0, 0.0, 0.0
if m.opt.cone == ConeType.ELLIPTIC: if m.opt.cone == ConeType.ELLIPTIC:
mask = d.contact.dim > 1 mask = d._impl.contact.dim > 1
# complete vector quadratic (for bottom zone) # complete vector quadratic (for bottom zone)
efc_con, efc_fri = [], [] efc_con, efc_fri = [], []
for condim, addr in zip(d.contact.dim[mask], d.contact.efc_address[mask]): for condim, addr in zip(
d._impl.contact.dim[mask], d._impl.contact.efc_address[mask]
):
efc_con.extend([addr] * (condim - 1)) efc_con.extend([addr] * (condim - 1))
efc_fri.extend(range(addr + 1, addr + condim)) efc_fri.extend(range(addr + 1, addr + condim))
quad = quad.at[jp.array(efc_con)].add(quad[jp.array(efc_fri)]) quad = quad.at[jp.array(efc_con)].add(quad[jp.array(efc_fri)])
@@ -446,7 +465,7 @@ def _linesearch(m: Model, d: Data, ctx: Context) -> Context:
jp.concatenate((jv, jp.zeros(3))), (x,), (6,) jp.concatenate((jv, jp.zeros(3))), (x,), (6,)
) )
) )
efc_elliptic = d.contact.efc_address[mask] efc_elliptic = d._impl.contact.efc_address[mask]
v = jv_fn(efc_elliptic) * ctx.fri v = jv_fn(efc_elliptic) * ctx.fri
uu = jp.sum(ctx.u[:, 1:] * ctx.u[:, 1:], axis=1) uu = jp.sum(ctx.u[:, 1:] * ctx.u[:, 1:], axis=1)
v0 = v[:, 0] v0 = v[:, 0]
@@ -571,11 +590,11 @@ def solve(m: Model, d: Data) -> Data:
else: else:
ctx = jax.lax.while_loop(cond, body, ctx) ctx = jax.lax.while_loop(cond, body, ctx)
d = d.replace( d = d.tree_replace({
qacc_warmstart=ctx.qacc, 'qacc_warmstart': ctx.qacc,
qacc=ctx.qacc, 'qfrc_constraint': ctx.qfrc_constraint,
qfrc_constraint=ctx.qfrc_constraint, 'qacc': ctx.qacc,
efc_force=ctx.efc_force, '_impl.efc_force': ctx.efc_force,
) })
return d return d
+8 -8
View File
@@ -87,8 +87,8 @@ class SolverTest(parameterized.TestCase):
# MJX finds very similar solutions with the newton solver # MJX finds very similar solutions with the newton solver
if solver_ == mujoco.mjtSolver.mjSOL_NEWTON: if solver_ == mujoco.mjtSolver.mjSOL_NEWTON:
nnz = dx.efc_J.any(axis=1) nnz = dx._impl.efc_J.any(axis=1)
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') _assert_eq(d.efc_force, dx._impl.efc_force[nnz], 'efc_force')
_assert_attr_eq(d, dx, 'qfrc_constraint') _assert_attr_eq(d, dx, 'qfrc_constraint')
_assert_attr_eq(d, dx, 'qacc') _assert_attr_eq(d, dx, 'qacc')
@@ -108,9 +108,9 @@ class SolverTest(parameterized.TestCase):
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
mx = mjx.put_model(m) mx = mjx.put_model(m)
dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d))
nnz = dx.efc_J.any(axis=1) nnz = dx._impl.efc_J.any(axis=1)
# even without warmstart, newton converges quickly # even without warmstart, newton converges quickly
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=2e-4) _assert_eq(d.efc_force, dx._impl.efc_force[nnz], 'efc_force', tol=2e-4)
def test_sparse(self): def test_sparse(self):
"""Test solver works with sparse mass matrices.""" """Test solver works with sparse mass matrices."""
@@ -130,8 +130,8 @@ class SolverTest(parameterized.TestCase):
_assert_attr_eq(d, dx, 'qacc') _assert_attr_eq(d, dx, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint') _assert_attr_eq(d, dx, 'qfrc_constraint')
nnz = dx.efc_J.any(axis=1) nnz = dx._impl.efc_J.any(axis=1)
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') _assert_eq(d.efc_force, dx._impl.efc_force[nnz], 'efc_force')
def test_quad_frictionloss(self): def test_quad_frictionloss(self):
"""Test a case with quadratic frictionloss constraints.""" """Test a case with quadratic frictionloss constraints."""
@@ -144,8 +144,8 @@ class SolverTest(parameterized.TestCase):
_assert_attr_eq(d, dx, 'qacc') _assert_attr_eq(d, dx, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint') _assert_attr_eq(d, dx, 'qfrc_constraint')
nnz = dx.efc_J.any(axis=1) nnz = dx._impl.efc_J.any(axis=1)
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') _assert_eq(d.efc_force, dx._impl.efc_force[nnz], 'efc_force')
# TODO(taylorhowell): condim=1 with ConeType.ELLIPTIC # TODO(taylorhowell): condim=1 with ConeType.ELLIPTIC
@parameterized.product(condim=(3, 4, 6), cone=tuple(ConeType)) @parameterized.product(condim=(3, 4, 6), cone=tuple(ConeType))
+20 -19
View File
@@ -13,6 +13,7 @@
# limitations under the License. # limitations under the License.
# ============================================================================== # ==============================================================================
"""Engine support functions.""" """Engine support functions."""
from collections.abc import Iterable, Sequence from collections.abc import Iterable, Sequence
from typing import Optional, Tuple, Union from typing import Optional, Tuple, Union
@@ -92,7 +93,7 @@ def full_m(m: Model, d: Data) -> jax.Array:
"""Reconstitute dense mass matrix from qM.""" """Reconstitute dense mass matrix from qM."""
if not is_sparse(m): if not is_sparse(m):
return d.qM return d._impl.qM # pytype: disable=attribute-error
ij = [] ij = []
for i in range(m.nv): for i in range(m.nv):
@@ -103,7 +104,7 @@ def full_m(m: Model, d: Data) -> jax.Array:
i, j = (jp.array(x) for x in zip(*ij)) i, j = (jp.array(x) for x in zip(*ij))
mat = jp.zeros((m.nv, m.nv)).at[(i, j)].set(d.qM) mat = jp.zeros((m.nv, m.nv)).at[(i, j)].set(d._impl.qM) # pytype: disable=attribute-error
# also set upper triangular # also set upper triangular
mat = mat + jp.tril(mat, -1).T mat = mat + jp.tril(mat, -1).T
@@ -115,9 +116,9 @@ def mul_m(m: Model, d: Data, vec: jax.Array) -> jax.Array:
"""Multiply vector by inertia matrix.""" """Multiply vector by inertia matrix."""
if not is_sparse(m): if not is_sparse(m):
return d.qM @ vec return d._impl.qM @ vec # pytype: disable=attribute-error
diag_mul = d.qM[jp.array(m.dof_Madr)] * vec diag_mul = d._impl.qM[jp.array(m.dof_Madr)] * vec # pytype: disable=attribute-error
is_, js, madr_ijs = [], [], [] is_, js, madr_ijs = [], [], []
for i in range(m.nv): for i in range(m.nv):
@@ -131,8 +132,8 @@ def mul_m(m: Model, d: Data, vec: jax.Array) -> jax.Array:
i, j, madr_ij = (jp.array(x, dtype=jp.int32) for x in (is_, js, madr_ijs)) i, j, madr_ij = (jp.array(x, dtype=jp.int32) for x in (is_, js, madr_ijs))
out = diag_mul.at[i].add(d.qM[madr_ij] * vec[j]) out = diag_mul.at[i].add(d._impl.qM[madr_ij] * vec[j]) # pytype: disable=attribute-error
out = out.at[j].add(d.qM[madr_ij] * vec[i]) out = out.at[j].add(d._impl.qM[madr_ij] * vec[i]) # pytype: disable=attribute-error
return out return out
@@ -147,9 +148,9 @@ def jac(
mask = mask[jp.array(m.dof_bodyid)] > 0 mask = mask[jp.array(m.dof_bodyid)] > 0
offset = point - d.subtree_com[jp.array(m.body_rootid)[body_id]] offset = point - d.subtree_com[jp.array(m.body_rootid)[body_id]]
jacp = jax.vmap(lambda a, b=offset: a[3:] + jp.cross(a[:3], b))(d.cdof) jacp = jax.vmap(lambda a, b=offset: a[3:] + jp.cross(a[:3], b))(d._impl.cdof) # pytype: disable=attribute-error
jacp = jax.vmap(jp.multiply)(jacp, mask) jacp = jax.vmap(jp.multiply)(jacp, mask)
jacr = jax.vmap(jp.multiply)(d.cdof[:, :3], mask) jacr = jax.vmap(jp.multiply)(d._impl.cdof[:, :3], mask) # pytype: disable=attribute-error
return jacp, jacr return jacp, jacr
@@ -548,20 +549,20 @@ def contact_force(
m: Model, d: Data, contact_id: int, to_world_frame: bool = False m: Model, d: Data, contact_id: int, to_world_frame: bool = False
) -> jax.Array: ) -> jax.Array:
"""Extract 6D force:torque for one contact, in contact frame by default.""" """Extract 6D force:torque for one contact, in contact frame by default."""
efc_address = d.contact.efc_address[contact_id] efc_address = d._impl.contact.efc_address[contact_id] # pytype: disable=attribute-error
condim = d.contact.dim[contact_id] condim = d._impl.contact.dim[contact_id] # pytype: disable=attribute-error
if m.opt.cone == ConeType.PYRAMIDAL: if m.opt.cone == ConeType.PYRAMIDAL:
force = _decode_pyramid( force = _decode_pyramid(
d.efc_force[efc_address:], d.contact.friction[contact_id], condim d._impl.efc_force[efc_address:], d._impl.contact.friction[contact_id], condim # pytype: disable=attribute-error
) )
elif m.opt.cone == ConeType.ELLIPTIC: elif m.opt.cone == ConeType.ELLIPTIC:
force = d.efc_force[efc_address : efc_address + condim] force = d._impl.efc_force[efc_address : efc_address + condim] # pytype: disable=attribute-error
force = jp.concatenate([force, jp.zeros((6 - condim))]) force = jp.concatenate([force, jp.zeros((6 - condim))])
else: else:
raise ValueError(f'Unknown cone type: {m.opt.cone}') raise ValueError(f'Unknown cone type: {m.opt.cone}')
if to_world_frame: if to_world_frame:
force = force.reshape((-1, 3)) @ d.contact.frame[contact_id] force = force.reshape((-1, 3)) @ d._impl.contact.frame[contact_id] # pytype: disable=attribute-error
force = force.reshape(-1) force = force.reshape(-1)
return force * (efc_address >= 0) return force * (efc_address >= 0)
@@ -572,21 +573,21 @@ def contact_force_dim(
) -> Tuple[jax.Array, np.ndarray]: ) -> Tuple[jax.Array, np.ndarray]:
"""Extract 6D force:torque for contacts with dimension dim.""" """Extract 6D force:torque for contacts with dimension dim."""
# valid contact and condim indices # valid contact and condim indices
idx_dim = (d.contact.efc_address >= 0) & (d.contact.dim == dim) idx_dim = (d._impl.contact.efc_address >= 0) & (d._impl.contact.dim == dim) # pytype: disable=attribute-error
# contact force from efc # contact force from efc
if m.opt.cone == ConeType.PYRAMIDAL: if m.opt.cone == ConeType.PYRAMIDAL:
efc_address = ( efc_address = (
d.contact.efc_address[idx_dim, None] d._impl.contact.efc_address[idx_dim, None] # pytype: disable=attribute-error
+ np.arange(np.where(dim == 1, 1, 2 * (dim - 1)))[None] + np.arange(np.where(dim == 1, 1, 2 * (dim - 1)))[None]
) )
efc_force = d.efc_force[efc_address] efc_force = d._impl.efc_force[efc_address] # pytype: disable=attribute-error
force = jax.vmap(_decode_pyramid, in_axes=(0, 0, None))( force = jax.vmap(_decode_pyramid, in_axes=(0, 0, None))(
efc_force, d.contact.friction[idx_dim], dim efc_force, d._impl.contact.friction[idx_dim], dim # pytype: disable=attribute-error
) )
elif m.opt.cone == ConeType.ELLIPTIC: elif m.opt.cone == ConeType.ELLIPTIC:
efc_address = d.contact.efc_address[idx_dim, None] + np.arange(dim)[None] efc_address = d._impl.contact.efc_address[idx_dim, None] + np.arange(dim)[None] # pytype: disable=attribute-error
force = d.efc_force[efc_address] force = d._impl.efc_force[efc_address] # pytype: disable=attribute-error
force = jp.hstack([force, jp.zeros((force.shape[0], 6 - dim))]) force = jp.hstack([force, jp.zeros((force.shape[0], 6 - dim))])
else: else:
raise ValueError(f'Unknown cone type: {m.opt.cone}.') raise ValueError(f'Unknown cone type: {m.opt.cone}.')
+4 -4
View File
@@ -385,7 +385,7 @@ class SupportTest(parameterized.TestCase):
# map MJX contacts to MJ ones # map MJX contacts to MJ ones
def _find(g): def _find(g):
val = (g == dx.contact.geom).sum(axis=1) val = (g == dx._impl.contact.geom).sum(axis=1)
return np.where(val == 2)[0][0] return np.where(val == 2)[0][0]
contact_id_map = {i: _find(d.contact.geom[i]) for i in range(d.ncon)} contact_id_map = {i: _find(d.contact.geom[i]) for i in range(d.ncon)}
@@ -399,7 +399,7 @@ class SupportTest(parameterized.TestCase):
np.testing.assert_allclose(result, force, rtol=1e-5, atol=2) np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)
# check for zeros after first condim elements # check for zeros after first condim elements
condim = dx.contact.dim[j] condim = dx._impl.contact.dim[j]
if condim < 6: if condim < 6:
np.testing.assert_allclose(force[condim:], 0, rtol=1e-5, atol=1e-5) np.testing.assert_allclose(force[condim:], 0, rtol=1e-5, atol=1e-5)
@@ -412,8 +412,8 @@ class SupportTest(parameterized.TestCase):
), ),
)(mx, dx, j, True) )(mx, dx, j, True)
# back to contact frame # back to contact frame
force = force.at[:3].set(dx.contact.frame[j] @ force[:3]) force = force.at[:3].set(dx._impl.contact.frame[j] @ force[:3])
force = force.at[3:].set(dx.contact.frame[j] @ force[3:]) force = force.at[3:].set(dx._impl.contact.frame[j] @ force[3:])
np.testing.assert_allclose(result, force, rtol=1e-5, atol=2) np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)
def test_wrap_inside(self): def test_wrap_inside(self):
+10 -10
View File
@@ -106,20 +106,20 @@ def benchmark(
def efc_order(m: mujoco.MjModel, d: mujoco.MjData, dx: Data) -> np.ndarray: def efc_order(m: mujoco.MjModel, d: mujoco.MjData, dx: Data) -> np.ndarray:
"""Returns a sort order such that dx.efc_*[order][:d.nefc] == d.efc_*.""" """Returns a sort order such that dx.efc_*[order][:d._impl.nefc] == d.efc_*.""" # pytype: disable=attribute-error
# reorder efc rows to skip inactive constraints and match contact order # reorder efc rows to skip inactive constraints and match contact order
efl = dx.ne + dx.nf + dx.nl efl = dx._impl.ne + dx._impl.nf + dx._impl.nl # pytype: disable=attribute-error
order = np.arange(efl) order = np.arange(efl)
order[(dx.efc_J[:efl] == 0).all(axis=1)] = 2**16 # move empty rows to end order[(dx._impl.efc_J[:efl] == 0).all(axis=1)] = 2**16 # move empty rows to end # pytype: disable=attribute-error
for i in range(dx.ncon): for i in range(dx._impl.ncon): # pytype: disable=attribute-error
num_rows = dx.contact.dim[i] num_rows = dx._impl.contact.dim[i] # pytype: disable=attribute-error
if dx.contact.dim[i] > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL: if dx._impl.contact.dim[i] > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL: # pytype: disable=attribute-error
num_rows = (dx.contact.dim[i] - 1) * 2 num_rows = (dx._impl.contact.dim[i] - 1) * 2 # pytype: disable=attribute-error
if dx.contact.dist[i] > 0: # move empty contacts to end if dx._impl.contact.dist[i] > 0: # move empty contacts to end # pytype: disable=attribute-error
order = np.append(order, np.repeat(2**16, num_rows)) order = np.append(order, np.repeat(2**16, num_rows))
continue continue
contact_match = (d.contact.geom == dx.contact.geom[i]).all(axis=-1) contact_match = (d.contact.geom == dx._impl.contact.geom[i]).all(axis=-1) # pytype: disable=attribute-error
contact_match &= (d.contact.pos == dx.contact.pos[i]).all(axis=-1) contact_match &= (d.contact.pos == dx._impl.contact.pos[i]).all(axis=-1) # pytype: disable=attribute-error
assert contact_match.any(), f'contact {i} not found' assert contact_match.any(), f'contact {i} not found'
contact_id = np.nonzero(contact_match)[0][0] contact_id = np.nonzero(contact_match)[0][0]
order = np.append(order, np.repeat(efl + contact_id, num_rows)) order = np.append(order, np.repeat(efl + contact_id, num_rows))
File diff suppressed because it is too large Load Diff