Add pyink and isort config. Reformat.
PiperOrigin-RevId: 704533915 Change-Id: I37e9fd51261bd166b725c7460fc65d02fed2b391
This commit is contained in:
committed by
Copybara-Service
parent
6f6244b739
commit
f3b3024291
+5
-1
@@ -6,7 +6,7 @@ possible in your code contributions.
|
|||||||
|
|
||||||
### Scope of this guide
|
### Scope of this guide
|
||||||
|
|
||||||
MuJoCo has three main code categories:
|
Most of this guide involves C/C++ code. For Python, jump to the section [below](#python-code). For MuJoCo C/C++, code has three main categories:
|
||||||
|
|
||||||
1. **C code:** MuJoCo's core codebase. It consists of public headers under
|
1. **C code:** MuJoCo's core codebase. It consists of public headers under
|
||||||
`include/` and C source files and internal headers under `src/`. This style
|
`include/` and C source files and internal headers under `src/`. This style
|
||||||
@@ -158,3 +158,7 @@ example above.
|
|||||||
New code should use the C99 convention. When editing an existing function,
|
New code should use the C99 convention. When editing an existing function,
|
||||||
please move existing variable declarations into local scope. Pull requests
|
please move existing variable declarations into local scope. Pull requests
|
||||||
helping us to complete the migration are very welcome.
|
helping us to complete the migration are very welcome.
|
||||||
|
|
||||||
|
### [Python code](#python-code)
|
||||||
|
|
||||||
|
For Python code, run `pyink foo.py` to adhere to Google's [Python style guide](https://google.github.io/styleguide/pyguide.html). For sorting and cleaning imports, run `isort foo.py`. Both `pyink` and `isort` can be pip installed via `pip install pyink isort`.
|
||||||
|
|||||||
@@ -906,7 +906,8 @@ def _sat_gaussmap(
|
|||||||
return edge_axis * sign, degenerate_edge_axis
|
return edge_axis * sign, degenerate_edge_axis
|
||||||
|
|
||||||
edge_axes, degenerate_edge_axes = jax.vmap(get_normals)(
|
edge_axes, degenerate_edge_axes = jax.vmap(get_normals)(
|
||||||
edge_a_dir, edge_a_pt, edge_b_dir)
|
edge_a_dir, edge_a_pt, edge_b_dir
|
||||||
|
)
|
||||||
edge_dist = jax.vmap(jp.dot)(edge_axes, edge_b_pt - edge_a_pt)
|
edge_dist = jax.vmap(jp.dot)(edge_axes, edge_b_pt - edge_a_pt)
|
||||||
# handle degenerate axis
|
# handle degenerate axis
|
||||||
edge_dist = jp.where(degenerate_edge_axes, -jp.inf, edge_dist)
|
edge_dist = jp.where(degenerate_edge_axes, -jp.inf, edge_dist)
|
||||||
@@ -928,11 +929,14 @@ def _sat_gaussmap(
|
|||||||
dist,
|
dist,
|
||||||
)
|
)
|
||||||
a_closest, b_closest = math.closest_segment_to_segment_points(
|
a_closest, b_closest = math.closest_segment_to_segment_points(
|
||||||
edge_a_pt[best_edge_idx], edge_a_pt_2[best_edge_idx],
|
edge_a_pt[best_edge_idx],
|
||||||
edge_b_pt[best_edge_idx], edge_b_pt_2[best_edge_idx])
|
edge_a_pt_2[best_edge_idx],
|
||||||
|
edge_b_pt[best_edge_idx],
|
||||||
|
edge_b_pt_2[best_edge_idx],
|
||||||
|
)
|
||||||
pos = jp.where(
|
pos = jp.where(
|
||||||
is_edge_contact,
|
is_edge_contact, jp.tile(0.5 * (a_closest + b_closest), (4, 1)), pos
|
||||||
jp.tile(0.5 * (a_closest + b_closest), (4, 1)), pos)
|
)
|
||||||
|
|
||||||
return dist, pos, normal
|
return dist, pos, normal
|
||||||
|
|
||||||
|
|||||||
@@ -146,13 +146,13 @@ def geom_pairs(
|
|||||||
b_end = b_start + m.body_geomnum
|
b_end = b_start + m.body_geomnum
|
||||||
|
|
||||||
for b1 in range(m.nbody):
|
for b1 in range(m.nbody):
|
||||||
if not geom_con[b_start[b1]:b_end[b1]].any():
|
if not geom_con[b_start[b1] : b_end[b1]].any():
|
||||||
continue
|
continue
|
||||||
w1 = m.body_weldid[b1]
|
w1 = m.body_weldid[b1]
|
||||||
w1_p = m.body_weldid[m.body_parentid[w1]]
|
w1_p = m.body_weldid[m.body_parentid[w1]]
|
||||||
|
|
||||||
for b2 in range(b1, m.nbody):
|
for b2 in range(b1, m.nbody):
|
||||||
if not geom_con[b_start[b2]:b_end[b2]].any():
|
if not geom_con[b_start[b2] : b_end[b2]].any():
|
||||||
continue
|
continue
|
||||||
signature = (b1 << 16) + (b2)
|
signature = (b1 << 16) + (b2)
|
||||||
if signature in exclude_signature:
|
if signature in exclude_signature:
|
||||||
@@ -272,7 +272,7 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]:
|
|||||||
jp.clip(m.pair_friction[ip], a_min=eps),
|
jp.clip(m.pair_friction[ip], a_min=eps),
|
||||||
m.pair_solref[ip],
|
m.pair_solref[ip],
|
||||||
m.pair_solreffriction[ip],
|
m.pair_solreffriction[ip],
|
||||||
m.pair_solimp[ip]
|
m.pair_solimp[ip],
|
||||||
))
|
))
|
||||||
if geom1.size > 0 and geom2.size > 0:
|
if geom1.size > 0 and geom2.size > 0:
|
||||||
# other contacts get their params from geom fields
|
# other contacts get their params from geom fields
|
||||||
|
|||||||
@@ -218,7 +218,8 @@ class EllipsoidCollisionTest(parameterized.TestCase):
|
|||||||
self.assertLess(dx.contact.dist[0], 0)
|
self.assertLess(dx.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.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5
|
||||||
|
)
|
||||||
|
|
||||||
_ELLIPSOID_ELLIPSOID = """
|
_ELLIPSOID_ELLIPSOID = """
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -240,7 +241,8 @@ class EllipsoidCollisionTest(parameterized.TestCase):
|
|||||||
self.assertLess(dx.contact.dist[0], 0)
|
self.assertLess(dx.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-5)
|
dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-5
|
||||||
|
)
|
||||||
|
|
||||||
_ELLIPSOID_SPHERE = """
|
_ELLIPSOID_SPHERE = """
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -263,7 +265,8 @@ class EllipsoidCollisionTest(parameterized.TestCase):
|
|||||||
self.assertLess(dx.contact.dist[0], 0)
|
self.assertLess(dx.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-3)
|
dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-3
|
||||||
|
)
|
||||||
|
|
||||||
_ELLIPSOID_CAPSULE = """
|
_ELLIPSOID_CAPSULE = """
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -285,7 +288,8 @@ class EllipsoidCollisionTest(parameterized.TestCase):
|
|||||||
self.assertLess(dx.contact.dist[0], 0)
|
self.assertLess(dx.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-3)
|
dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-3
|
||||||
|
)
|
||||||
|
|
||||||
_ELLIPSOID_CYLINDER = """
|
_ELLIPSOID_CYLINDER = """
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -308,7 +312,8 @@ class EllipsoidCollisionTest(parameterized.TestCase):
|
|||||||
self.assertLess(dx.contact.dist[0], 0)
|
self.assertLess(dx.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.contact, d.contact, field.name, 'ellipsoid-cylinder', 1e-4
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
class CapsuleCollisionTest(parameterized.TestCase):
|
class CapsuleCollisionTest(parameterized.TestCase):
|
||||||
@@ -550,7 +555,8 @@ class CylinderTest(absltest.TestCase):
|
|||||||
|
|
||||||
# cylinder is vertical
|
# cylinder is vertical
|
||||||
xml = self._CYLINDER_PLANE.replace(
|
xml = self._CYLINDER_PLANE.replace(
|
||||||
'<geom fromto="-0.1 0 0 0.1 0 0"', '<geom fromto="0 0 -0.1 0 0 0.1"')
|
'<geom fromto="-0.1 0 0 0.1 0 0"', '<geom fromto="0 0 -0.1 0 0 0.1"'
|
||||||
|
)
|
||||||
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)
|
||||||
|
|
||||||
@@ -579,7 +585,8 @@ class CylinderTest(absltest.TestCase):
|
|||||||
self.assertLess(dx.contact.dist[0], 0)
|
self.assertLess(dx.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.contact, d.contact, field.name, 'sphere-cylinder', 1e-4
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
class ConvexTest(absltest.TestCase):
|
class ConvexTest(absltest.TestCase):
|
||||||
@@ -760,11 +767,14 @@ class ConvexTest(absltest.TestCase):
|
|||||||
np.testing.assert_array_less(0, c.dist[1:])
|
np.testing.assert_array_less(0, c.dist[1:])
|
||||||
np.testing.assert_array_almost_equal(c.frame[0, 0], np.array([0, 0, 1]))
|
np.testing.assert_array_almost_equal(c.frame[0, 0], np.array([0, 0, 1]))
|
||||||
np.testing.assert_array_almost_equal(
|
np.testing.assert_array_almost_equal(
|
||||||
c.pos[0], np.array([0, 2, 1.3155]), decimal=5)
|
c.pos[0], np.array([0, 2, 1.3155]), decimal=5
|
||||||
|
)
|
||||||
|
|
||||||
_, dx = _collide(
|
_, dx = _collide(
|
||||||
self._CONVEX_CONVEX_THIN.replace(
|
self._CONVEX_CONVEX_THIN.replace(
|
||||||
'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.contact
|
||||||
self.assertTrue((c.dist > 0).all())
|
self.assertTrue((c.dist > 0).all())
|
||||||
|
|
||||||
|
|||||||
@@ -29,6 +29,7 @@ from mujoco.mjx._src.types import Model
|
|||||||
|
|
||||||
def collider(ncon: int):
|
def collider(ncon: int):
|
||||||
"""Wraps collision functions for use by collision_driver."""
|
"""Wraps collision functions for use by collision_driver."""
|
||||||
|
|
||||||
def wrapper(func):
|
def wrapper(func):
|
||||||
def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision:
|
def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision:
|
||||||
g1, g2 = geom.T
|
g1, g2 = geom.T
|
||||||
@@ -119,7 +120,7 @@ def plane_cylinder(plane: GeomInfo, cylinder: GeomInfo) -> Collision:
|
|||||||
# disk parallel to plane: pick x-axis of cylinder, scale by radius
|
# disk parallel to plane: pick x-axis of cylinder, scale by radius
|
||||||
cylinder.mat[:, 0] * cylinder.size[0],
|
cylinder.mat[:, 0] * cylinder.size[0],
|
||||||
# general configuration: normalize vector, scale by radius
|
# general configuration: normalize vector, scale by radius
|
||||||
vec / len_ * cylinder.size[0]
|
vec / len_ * cylinder.size[0],
|
||||||
)
|
)
|
||||||
|
|
||||||
# project vector on normal
|
# project vector on normal
|
||||||
@@ -138,11 +139,15 @@ def plane_cylinder(plane: GeomInfo, cylinder: GeomInfo) -> Collision:
|
|||||||
d1 = dist0 + prjaxis + prjvec
|
d1 = dist0 + prjaxis + prjvec
|
||||||
d2 = dist0 + prjaxis + prjvec1
|
d2 = dist0 + prjaxis + prjvec1
|
||||||
dist = jp.array([d1, d2, d2])
|
dist = jp.array([d1, d2, d2])
|
||||||
pos = cylinder.pos + axis + jp.array([
|
pos = (
|
||||||
vec - n * d1 * 0.5,
|
cylinder.pos
|
||||||
vec1 + vec * -0.5 - n * d2 * 0.5,
|
+ axis
|
||||||
-vec1 + vec * -0.5 - n * d2 * 0.5,
|
+ jp.array([
|
||||||
])
|
vec - n * d1 * 0.5,
|
||||||
|
vec1 + vec * -0.5 - n * d2 * 0.5,
|
||||||
|
-vec1 + vec * -0.5 - n * d2 * 0.5,
|
||||||
|
])
|
||||||
|
)
|
||||||
|
|
||||||
# cylinder parallel to plane
|
# cylinder parallel to plane
|
||||||
cond = jp.abs(prjaxis) < 1e-3
|
cond = jp.abs(prjaxis) < 1e-3
|
||||||
|
|||||||
@@ -41,6 +41,7 @@ SDFFn = Callable[[jax.Array], jax.Array]
|
|||||||
|
|
||||||
def collider(ncon: int):
|
def collider(ncon: int):
|
||||||
"""Wraps collision functions for use by collision_driver."""
|
"""Wraps collision functions for use by collision_driver."""
|
||||||
|
|
||||||
def wrapper(func):
|
def wrapper(func):
|
||||||
def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision:
|
def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision:
|
||||||
g1, g2 = geom.T
|
g1, g2 = geom.T
|
||||||
@@ -81,7 +82,7 @@ def _capsule(pos: jax.Array, size: jax.Array):
|
|||||||
|
|
||||||
def _ellipsoid(pos: jax.Array, size: jax.Array) -> jax.Array:
|
def _ellipsoid(pos: jax.Array, size: jax.Array) -> jax.Array:
|
||||||
k0 = math.norm(pos / size)
|
k0 = math.norm(pos / size)
|
||||||
k1 = math.norm(pos / (size*size))
|
k1 = math.norm(pos / (size * size))
|
||||||
return k0 * (k0 - 1.0) / (k1 + (k1 == 0.0) * 1e-12)
|
return k0 * (k0 - 1.0) / (k1 + (k1 == 0.0) * 1e-12)
|
||||||
|
|
||||||
|
|
||||||
@@ -96,12 +97,12 @@ def _cylinder(pos: jax.Array, size: jax.Array) -> jax.Array:
|
|||||||
|
|
||||||
def _cylinder_grad(x: jax.Array, size: jax.Array) -> jax.Array:
|
def _cylinder_grad(x: jax.Array, size: jax.Array) -> jax.Array:
|
||||||
"""Gradient of the cylinder SDF wrt query point and singularities removed."""
|
"""Gradient of the cylinder SDF wrt query point and singularities removed."""
|
||||||
c = jp.sqrt(x[0]*x[0]+x[1]*x[1])
|
c = jp.sqrt(x[0] * x[0] + x[1] * x[1])
|
||||||
e = jp.abs(x[2])
|
e = jp.abs(x[2])
|
||||||
a = jp.array([c - size[0], e - size[1]])
|
a = jp.array([c - size[0], e - size[1]])
|
||||||
b = jp.array([jp.maximum(a[0], 0), jp.maximum(a[1], 0)])
|
b = jp.array([jp.maximum(a[0], 0), jp.maximum(a[1], 0)])
|
||||||
j = jp.argmax(a)
|
j = jp.argmax(a)
|
||||||
bnorm = jp.sqrt(b[0]*b[0] + b[1]*b[1])
|
bnorm = jp.sqrt(b[0] * b[0] + b[1] * b[1])
|
||||||
bnorm += jp.allclose(bnorm, 0) * 1e-12
|
bnorm += jp.allclose(bnorm, 0) * 1e-12
|
||||||
grada = jp.array([
|
grada = jp.array([
|
||||||
x[0] / (c + jp.allclose(c, 0) * 1e-12),
|
x[0] / (c + jp.allclose(c, 0) * 1e-12),
|
||||||
@@ -151,7 +152,7 @@ def _gradient_step(objective: SDFFn, state: GradientState) -> GradientState:
|
|||||||
"""Performs a step of gradient descent."""
|
"""Performs a step of gradient descent."""
|
||||||
# TODO: find better parameters
|
# TODO: find better parameters
|
||||||
amin = 1e-4 # minimum value for line search factor scaling the gradient
|
amin = 1e-4 # minimum value for line search factor scaling the gradient
|
||||||
amax = 2. # maximum value for line search factor scaling the gradient
|
amax = 2.0 # maximum value for line search factor scaling the gradient
|
||||||
nlinesearch = 10 # line search points
|
nlinesearch = 10 # line search points
|
||||||
grad = jax.grad(objective)(state.x)
|
grad = jax.grad(objective)(state.x)
|
||||||
alpha = jp.geomspace(amin, amax, nlinesearch).reshape(nlinesearch, -1)
|
alpha = jp.geomspace(amin, amax, nlinesearch).reshape(nlinesearch, -1)
|
||||||
@@ -179,7 +180,11 @@ def _gradient_descent(
|
|||||||
|
|
||||||
|
|
||||||
def _optim(
|
def _optim(
|
||||||
d1, d2, info1: GeomInfo, info2: GeomInfo, x0: jax.Array,
|
d1,
|
||||||
|
d2,
|
||||||
|
info1: GeomInfo,
|
||||||
|
info2: GeomInfo,
|
||||||
|
x0: jax.Array,
|
||||||
) -> Collision:
|
) -> Collision:
|
||||||
"""Optimizes the clearance function."""
|
"""Optimizes the clearance function."""
|
||||||
d1 = functools.partial(d1, size=info1.size)
|
d1 = functools.partial(d1, size=info1.size)
|
||||||
@@ -198,14 +203,14 @@ def _optim(
|
|||||||
|
|
||||||
@collider(ncon=1)
|
@collider(ncon=1)
|
||||||
def sphere_ellipsoid(s: GeomInfo, e: GeomInfo) -> Collision:
|
def sphere_ellipsoid(s: GeomInfo, e: GeomInfo) -> Collision:
|
||||||
""""Calculates contact between a sphere and an ellipsoid."""
|
"""Calculates contact between a sphere and an ellipsoid."""
|
||||||
x0 = 0.5 * (s.pos + e.pos)
|
x0 = 0.5 * (s.pos + e.pos)
|
||||||
return _optim(_sphere, _ellipsoid, s, e, x0)
|
return _optim(_sphere, _ellipsoid, s, e, x0)
|
||||||
|
|
||||||
|
|
||||||
@collider(ncon=1)
|
@collider(ncon=1)
|
||||||
def sphere_cylinder(s: GeomInfo, c: GeomInfo) -> Collision:
|
def sphere_cylinder(s: GeomInfo, c: GeomInfo) -> Collision:
|
||||||
""""Calculates contact between a sphere and a cylinder."""
|
"""Calculates contact between a sphere and a cylinder."""
|
||||||
# TODO: implement analytical version.
|
# TODO: implement analytical version.
|
||||||
x0 = 0.5 * (s.pos + c.pos)
|
x0 = 0.5 * (s.pos + c.pos)
|
||||||
return _optim(_sphere, _cylinder, s, c, x0)
|
return _optim(_sphere, _cylinder, s, c, x0)
|
||||||
@@ -213,14 +218,14 @@ def sphere_cylinder(s: GeomInfo, c: GeomInfo) -> Collision:
|
|||||||
|
|
||||||
@collider(ncon=1)
|
@collider(ncon=1)
|
||||||
def capsule_ellipsoid(c: GeomInfo, e: GeomInfo) -> Collision:
|
def capsule_ellipsoid(c: GeomInfo, e: GeomInfo) -> Collision:
|
||||||
""""Calculates contact between a capsule and an ellipsoid."""
|
""" "Calculates contact between a capsule and an ellipsoid."""
|
||||||
x0 = 0.5 * (c.pos + e.pos)
|
x0 = 0.5 * (c.pos + e.pos)
|
||||||
return _optim(_capsule, _ellipsoid, c, e, x0)
|
return _optim(_capsule, _ellipsoid, c, e, x0)
|
||||||
|
|
||||||
|
|
||||||
@collider(ncon=2)
|
@collider(ncon=2)
|
||||||
def capsule_cylinder(ca: GeomInfo, cy: GeomInfo) -> Collision:
|
def capsule_cylinder(ca: GeomInfo, cy: GeomInfo) -> Collision:
|
||||||
""""Calculates contact between a capsule and a cylinder."""
|
"""Calculates contact between a capsule and a cylinder."""
|
||||||
# TODO: improve robustness
|
# TODO: improve robustness
|
||||||
# Near sharp corners, the SDF might give the penetration depth with respect
|
# Near sharp corners, the SDF might give the penetration depth with respect
|
||||||
# to a surface that is not in collision. Possible solutions is to find the
|
# to a surface that is not in collision. Possible solutions is to find the
|
||||||
@@ -235,21 +240,21 @@ def capsule_cylinder(ca: GeomInfo, cy: GeomInfo) -> Collision:
|
|||||||
|
|
||||||
@collider(ncon=1)
|
@collider(ncon=1)
|
||||||
def ellipsoid_ellipsoid(e1: GeomInfo, e2: GeomInfo) -> Collision:
|
def ellipsoid_ellipsoid(e1: GeomInfo, e2: GeomInfo) -> Collision:
|
||||||
""""Calculates contact between two ellipsoids."""
|
"""Calculates contact between two ellipsoids."""
|
||||||
x0 = 0.5 * (e1.pos + e2.pos)
|
x0 = 0.5 * (e1.pos + e2.pos)
|
||||||
return _optim(_ellipsoid, _ellipsoid, e1, e2, x0)
|
return _optim(_ellipsoid, _ellipsoid, e1, e2, x0)
|
||||||
|
|
||||||
|
|
||||||
@collider(ncon=1)
|
@collider(ncon=1)
|
||||||
def ellipsoid_cylinder(e: GeomInfo, c: GeomInfo) -> Collision:
|
def ellipsoid_cylinder(e: GeomInfo, c: GeomInfo) -> Collision:
|
||||||
""""Calculates contact between and ellipsoid and a cylinder."""
|
"""Calculates contact between and ellipsoid and a cylinder."""
|
||||||
x0 = 0.5 * (e.pos + c.pos)
|
x0 = 0.5 * (e.pos + c.pos)
|
||||||
return _optim(_ellipsoid, _cylinder, e, c, x0)
|
return _optim(_ellipsoid, _cylinder, e, c, x0)
|
||||||
|
|
||||||
|
|
||||||
@collider(ncon=4)
|
@collider(ncon=4)
|
||||||
def cylinder_cylinder(c1: GeomInfo, c2: GeomInfo) -> Collision:
|
def cylinder_cylinder(c1: GeomInfo, c2: GeomInfo) -> Collision:
|
||||||
""""Calculates contact between a cylinder and a cylinder."""
|
"""Calculates contact between a cylinder and a cylinder."""
|
||||||
# TODO: improve robustness
|
# TODO: improve robustness
|
||||||
# Near sharp corners, the SDF might give the penetration depth with respect
|
# Near sharp corners, the SDF might give the penetration depth with respect
|
||||||
# to a surface that is not in collision. Possible solutions is to find the
|
# to a surface that is not in collision. Possible solutions is to find the
|
||||||
|
|||||||
@@ -73,6 +73,7 @@ class FunctionKey:
|
|||||||
resulting constraint jacobian is determined at compile time.
|
resulting constraint jacobian is determined at compile time.
|
||||||
subgrid_size: the size determines the hfield subgrid to collide with
|
subgrid_size: the size determines the hfield subgrid to collide with
|
||||||
"""
|
"""
|
||||||
|
|
||||||
types: Tuple[int, int]
|
types: Tuple[int, int]
|
||||||
data_ids: Tuple[int, int]
|
data_ids: Tuple[int, int]
|
||||||
condim: int
|
condim: int
|
||||||
|
|||||||
@@ -39,6 +39,7 @@ import numpy as np
|
|||||||
|
|
||||||
class _Efc(PyTreeNode):
|
class _Efc(PyTreeNode):
|
||||||
"""Support data for creating constraint matrices."""
|
"""Support data for creating constraint matrices."""
|
||||||
|
|
||||||
J: jax.Array
|
J: jax.Array
|
||||||
pos_aref: jax.Array
|
pos_aref: jax.Array
|
||||||
pos_imp: jax.Array
|
pos_imp: jax.Array
|
||||||
|
|||||||
@@ -30,7 +30,7 @@ _TOLERANCE = 5e-5
|
|||||||
|
|
||||||
|
|
||||||
def _assert_eq(a, b, name):
|
def _assert_eq(a, b, name):
|
||||||
tol = _TOLERANCE * 10 # avoid test noise
|
tol = _TOLERANCE * 10 # avoid test noise
|
||||||
err_msg = f'mismatch: {name}'
|
err_msg = f'mismatch: {name}'
|
||||||
np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol)
|
np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol)
|
||||||
|
|
||||||
@@ -75,7 +75,7 @@ class ConstraintTest(parameterized.TestCase):
|
|||||||
_assert_eq(0, dx.efc_aref[order][d.nefc :], 'efc_aref')
|
_assert_eq(0, dx.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.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.efc_pos[order][: d.nefc], 'efc_pos')
|
||||||
_assert_eq(dx.efc_pos[order][d.nefc:], 0, 'efc_pos')
|
_assert_eq(dx.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.efc_frictionloss[order][: d.nefc],
|
||||||
|
|||||||
@@ -16,7 +16,6 @@
|
|||||||
|
|
||||||
import copy
|
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 jax
|
import jax
|
||||||
@@ -57,7 +56,7 @@ def dataclass(clz: _T) -> _T:
|
|||||||
meta_fields.append(field)
|
meta_fields.append(field)
|
||||||
|
|
||||||
def replace(self, **updates):
|
def replace(self, **updates):
|
||||||
""""Returns a new object replacing the specified fields with new values."""
|
"""Returns a new object replacing the specified fields with new values."""
|
||||||
return dataclasses.replace(self, **updates)
|
return dataclasses.replace(self, **updates)
|
||||||
|
|
||||||
data_clz.replace = replace
|
data_clz.replace = replace
|
||||||
|
|||||||
@@ -34,6 +34,7 @@ def _strip_weak_type(tree):
|
|||||||
if isinstance(leaf, jax.Array):
|
if isinstance(leaf, jax.Array):
|
||||||
return leaf.astype(jax.dtypes.canonicalize_dtype(leaf.dtype))
|
return leaf.astype(jax.dtypes.canonicalize_dtype(leaf.dtype))
|
||||||
return leaf
|
return leaf
|
||||||
|
|
||||||
return jax.tree_util.tree_map(f, tree)
|
return jax.tree_util.tree_map(f, tree)
|
||||||
|
|
||||||
|
|
||||||
@@ -95,7 +96,7 @@ def put_model(
|
|||||||
m: the model to put onto device
|
m: the model to put onto device
|
||||||
device: which device to use - if unspecified picks the default device
|
device: which device to use - if unspecified picks the default device
|
||||||
_full_compat: put all MjModel fields onto device irrespective of MJX support
|
_full_compat: put all MjModel fields onto device irrespective of MJX support
|
||||||
This is an experimental feature. Avoid using it for now.
|
This is an experimental feature. Avoid using it for now.
|
||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
an mjx.Model placed on device
|
an mjx.Model placed on device
|
||||||
@@ -215,8 +216,8 @@ def make_data(
|
|||||||
m: the model to use
|
m: the model to use
|
||||||
device: which device to use - if unspecified picks the default device
|
device: which device to use - if unspecified picks the default device
|
||||||
_full_compat: create all MjData fields on device irrespective of MJX support
|
_full_compat: create all MjData fields on device irrespective of MJX support
|
||||||
This is an experimental feature. Avoid using it for now.
|
This is an experimental feature. Avoid using it for now. If using this
|
||||||
If using this flag, also use _full_compat for put_model.
|
flag, also use _full_compat for put_model.
|
||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
an initialized mjx.Data placed on device
|
an initialized mjx.Data placed on device
|
||||||
@@ -383,7 +384,7 @@ def make_data(
|
|||||||
contact=contact,
|
contact=contact,
|
||||||
efc_type=efc_type,
|
efc_type=efc_type,
|
||||||
eq_active=m.eq_active0,
|
eq_active=m.eq_active0,
|
||||||
**zero_fields
|
**zero_fields,
|
||||||
)
|
)
|
||||||
|
|
||||||
return d
|
return d
|
||||||
@@ -556,8 +557,8 @@ def put_data(
|
|||||||
d: the data to put on device
|
d: the data to put on device
|
||||||
device: which device to use - if unspecified picks the default device
|
device: which device to use - if unspecified picks the default device
|
||||||
_full_compat: put all MjModel fields onto device irrespective of MJX support
|
_full_compat: put all MjModel fields onto device irrespective of MJX support
|
||||||
This is an experimental feature. Avoid using it for now.
|
This is an experimental feature. Avoid using it for now. If using this
|
||||||
If using this flag, also use _full_compat for put_model.
|
flag, also use _full_compat for put_model.
|
||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
an mjx.Data placed on device
|
an mjx.Data placed on device
|
||||||
@@ -646,7 +647,7 @@ def put_data(
|
|||||||
if num_rows > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL:
|
if num_rows > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL:
|
||||||
num_rows = (num_rows - 1) * 2
|
num_rows = (num_rows - 1) * 2
|
||||||
efc_i, efc_o = d.contact.efc_address[id_from], efc_address[id_to]
|
efc_i, efc_o = d.contact.efc_address[id_from], efc_address[id_to]
|
||||||
value[efc_o:efc_o + num_rows] = fields[fname][efc_i:efc_i + num_rows]
|
value[efc_o : efc_o + num_rows] = fields[fname][efc_i : efc_i + num_rows]
|
||||||
|
|
||||||
fields[fname] = value
|
fields[fname] = value
|
||||||
|
|
||||||
|
|||||||
@@ -92,12 +92,11 @@ _MULTIPLE_CONSTRAINTS = """
|
|||||||
class ModelIOTest(parameterized.TestCase):
|
class ModelIOTest(parameterized.TestCase):
|
||||||
"""IO tests for mjx.Model."""
|
"""IO tests for mjx.Model."""
|
||||||
|
|
||||||
@parameterized.parameters(
|
@parameterized.parameters(_MULTIPLE_CONVEX_OBJECTS, _MULTIPLE_CONSTRAINTS)
|
||||||
_MULTIPLE_CONVEX_OBJECTS, _MULTIPLE_CONSTRAINTS
|
|
||||||
)
|
|
||||||
def test_put_model(self, xml):
|
def test_put_model(self, xml):
|
||||||
m = mujoco.MjModel.from_xml_string(xml)
|
m = mujoco.MjModel.from_xml_string(xml)
|
||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
|
|
||||||
def assert_not_weak_type(x):
|
def assert_not_weak_type(x):
|
||||||
if isinstance(x, jax.Array):
|
if isinstance(x, jax.Array):
|
||||||
assert not x.weak_type
|
assert not x.weak_type
|
||||||
|
|||||||
@@ -28,6 +28,7 @@ def matmul_unroll(a: jax.Array, b: jax.Array) -> jax.Array:
|
|||||||
Args:
|
Args:
|
||||||
a: left hand of matmul operand
|
a: left hand of matmul operand
|
||||||
b: right hand of matmul operand
|
b: right hand of matmul operand
|
||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
the matrix product of the inputs.
|
the matrix product of the inputs.
|
||||||
"""
|
"""
|
||||||
|
|||||||
@@ -172,8 +172,8 @@ def _merge_coplanar(
|
|||||||
|
|
||||||
# resize faces that exceed max polygon vertices
|
# resize faces that exceed max polygon vertices
|
||||||
if face.shape[0] > _MAX_HULL_FACE_VERTICES:
|
if face.shape[0] > _MAX_HULL_FACE_VERTICES:
|
||||||
name = m.names[m.name_meshadr[meshid]:]
|
name = m.names[m.name_meshadr[meshid] :]
|
||||||
name = name[:name.find(b'\x00')].decode('utf-8')
|
name = name[: name.find(b'\x00')].decode('utf-8')
|
||||||
warnings.warn(
|
warnings.warn(
|
||||||
f'Mesh "{name}" has a coplanar face with more than '
|
f'Mesh "{name}" has a coplanar face with more than '
|
||||||
f'{_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance '
|
f'{_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance '
|
||||||
|
|||||||
@@ -53,8 +53,9 @@ class MeshTest(absltest.TestCase):
|
|||||||
map_ = {v: k for k, v in enumerate(vidx)}
|
map_ = {v: k for k, v in enumerate(vidx)}
|
||||||
h_face = np.vectorize(map_.get)(convex_face)
|
h_face = np.vectorize(map_.get)(convex_face)
|
||||||
face_verts = sorted([tuple(sorted(set(s))) for s in h_face.tolist()])
|
face_verts = sorted([tuple(sorted(set(s))) for s in h_face.tolist()])
|
||||||
expected_face_verts = sorted([
|
expected_face_verts = sorted(
|
||||||
(0, 3, 4), (1, 3, 4), (0, 2, 4), (0, 1, 2, 3), (1, 2, 4)])
|
[(0, 3, 4), (1, 3, 4), (0, 2, 4), (0, 1, 2, 3), (1, 2, 4)]
|
||||||
|
)
|
||||||
self.assertSequenceEqual(
|
self.assertSequenceEqual(
|
||||||
face_verts,
|
face_verts,
|
||||||
expected_face_verts,
|
expected_face_verts,
|
||||||
|
|||||||
@@ -31,6 +31,7 @@ from mujoco.mjx._src.types import Model
|
|||||||
|
|
||||||
def _spring_damper(m: Model, d: Data) -> jax.Array:
|
def _spring_damper(m: Model, d: Data) -> jax.Array:
|
||||||
"""Applies joint level spring and damping forces."""
|
"""Applies joint level spring and damping forces."""
|
||||||
|
|
||||||
def fn(jnt_typs, stiffness, qpos_spring, qpos):
|
def fn(jnt_typs, stiffness, qpos_spring, qpos):
|
||||||
qpos_i = 0
|
qpos_i = 0
|
||||||
qfrcs = []
|
qfrcs = []
|
||||||
|
|||||||
@@ -269,7 +269,7 @@ def ray(
|
|||||||
geom_filter_dyn = (m.geom_matid != -1) | (m.geom_rgba[:, 3] != 0)
|
geom_filter_dyn = (m.geom_matid != -1) | (m.geom_rgba[:, 3] != 0)
|
||||||
geom_filter_dyn &= (m.geom_matid == -1) | (m.mat_rgba[m.geom_matid, 3] != 0)
|
geom_filter_dyn &= (m.geom_matid == -1) | (m.mat_rgba[m.geom_matid, 3] != 0)
|
||||||
for geom_type, fn in _RAY_FUNC.items():
|
for geom_type, fn in _RAY_FUNC.items():
|
||||||
id_, = np.nonzero(geom_filter & (m.geom_type == geom_type))
|
(id_,) = np.nonzero(geom_filter & (m.geom_type == geom_type))
|
||||||
|
|
||||||
if id_.size == 0:
|
if id_.size == 0:
|
||||||
continue
|
continue
|
||||||
|
|||||||
+11
-15
@@ -144,13 +144,11 @@ def _check_input(m: Model, args: Any, in_types: str) -> None:
|
|||||||
}
|
}
|
||||||
for idx, (arg, typ) in enumerate(zip(args, in_types)):
|
for idx, (arg, typ) in enumerate(zip(args, in_types)):
|
||||||
if len(arg) != size[typ]:
|
if len(arg) != size[typ]:
|
||||||
raise IndexError(
|
raise IndexError((
|
||||||
(
|
f'f argument "{idx}" with type "{typ}" has length "{len(arg)}"'
|
||||||
f'f argument "{idx}" with type "{typ}" has length "{len(arg)}"'
|
f' which does not match the in_types[{idx}] expected length of '
|
||||||
f' which does not match the in_types[{idx}] expected length of '
|
f'"{size[typ]}".'
|
||||||
f'"{size[typ]}".'
|
))
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _check_output(
|
def _check_output(
|
||||||
@@ -158,13 +156,11 @@ def _check_output(
|
|||||||
) -> None:
|
) -> None:
|
||||||
"""Checks that scan output has the right shape."""
|
"""Checks that scan output has the right shape."""
|
||||||
if y.shape[0] != take_ids.shape[0]:
|
if y.shape[0] != take_ids.shape[0]:
|
||||||
raise IndexError(
|
raise IndexError((
|
||||||
(
|
f'f output "{idx}" with type "{typ}" has shape "{y.shape[0]}" '
|
||||||
f'f output "{idx}" with type "{typ}" has shape "{y.shape[0]}" '
|
f'which does not match the out_types[{idx}] expected size of'
|
||||||
f'which does not match the out_types[{idx}] expected size of'
|
f' "{take_ids.shape[0]}".'
|
||||||
f' "{take_ids.shape[0]}".'
|
))
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def flat(
|
def flat(
|
||||||
@@ -400,7 +396,7 @@ def body_tree(
|
|||||||
if t == 'b':
|
if t == 'b':
|
||||||
continue
|
continue
|
||||||
elif t == 'j':
|
elif t == 'j':
|
||||||
key += (tuple(m.jnt_type[np.nonzero(m.jnt_bodyid == id_)[0]]))
|
key += tuple(m.jnt_type[np.nonzero(m.jnt_bodyid == id_)[0]])
|
||||||
elif t == 'v':
|
elif t == 'v':
|
||||||
key += (len(np.nonzero(m.dof_bodyid == id_)[0]),)
|
key += (len(np.nonzero(m.dof_bodyid == id_)[0]),)
|
||||||
elif t == 'q':
|
elif t == 'q':
|
||||||
|
|||||||
@@ -90,6 +90,7 @@ class ScanTest(absltest.TestCase):
|
|||||||
if tuple(jnt_types) == (JointType.FREE,):
|
if tuple(jnt_types) == (JointType.FREE,):
|
||||||
return None
|
return None
|
||||||
return val + sum(jnt_types)
|
return val + sum(jnt_types)
|
||||||
|
|
||||||
b_expect = jp.array([[0, 0], [3, 3], [8, 8]])
|
b_expect = jp.array([[0, 0], [3, 3], [8, 8]])
|
||||||
b_out = scan.flat(m, no_free, 'jb', 'b', m.jnt_type, b_in)
|
b_out = scan.flat(m, no_free, 'jb', 'b', m.jnt_type, b_in)
|
||||||
np.testing.assert_equal(np.array(b_out), np.array(b_expect))
|
np.testing.assert_equal(np.array(b_out), np.array(b_expect))
|
||||||
@@ -99,6 +100,7 @@ class ScanTest(absltest.TestCase):
|
|||||||
if jnt_types.size == 0:
|
if jnt_types.size == 0:
|
||||||
self.fail('world has no dofs, should not be called')
|
self.fail('world has no dofs, should not be called')
|
||||||
return val + sum(jnt_types)
|
return val + sum(jnt_types)
|
||||||
|
|
||||||
v_in = jp.ones((m.nv, 1))
|
v_in = jp.ones((m.nv, 1))
|
||||||
scan.flat(m, no_world, 'jv', 'v', m.jnt_type, v_in)
|
scan.flat(m, no_world, 'jv', 'v', m.jnt_type, v_in)
|
||||||
|
|
||||||
@@ -141,6 +143,7 @@ class ScanTest(absltest.TestCase):
|
|||||||
return None
|
return None
|
||||||
carry = jp.zeros_like(val) if carry is None else carry
|
carry = jp.zeros_like(val) if carry is None else carry
|
||||||
return carry + val + sum(jnt_types)
|
return carry + val + sum(jnt_types)
|
||||||
|
|
||||||
b_expect = jp.array([[0, 0], [3, 3], [8, 8]])
|
b_expect = jp.array([[0, 0], [3, 3], [8, 8]])
|
||||||
b_out = scan.body_tree(m, no_free, 'jb', 'b', m.jnt_type, b_in)
|
b_out = scan.body_tree(m, no_free, 'jb', 'b', m.jnt_type, b_in)
|
||||||
np.testing.assert_equal(np.array(b_out), np.array(b_expect))
|
np.testing.assert_equal(np.array(b_out), np.array(b_expect))
|
||||||
|
|||||||
@@ -17,13 +17,11 @@
|
|||||||
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 test_util
|
from mujoco.mjx._src import test_util
|
||||||
from mujoco.mjx._src.types import ConeType
|
from mujoco.mjx._src.types import ConeType
|
||||||
|
|
||||||
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
|
||||||
|
|||||||
@@ -334,7 +334,7 @@ def factor_m(m: Model, d: Data) -> Data:
|
|||||||
pivots = []
|
pivots = []
|
||||||
out = []
|
out = []
|
||||||
|
|
||||||
for (b, e, madr_d, madr_ij) in updates:
|
for b, e, madr_d, madr_ij in updates:
|
||||||
width = e - b
|
width = e - b
|
||||||
rows.append(np.arange(madr_ij, madr_ij + width))
|
rows.append(np.arange(madr_ij, madr_ij + width))
|
||||||
madr_ijs.append(np.full((width,), madr_ij))
|
madr_ijs.append(np.full((width,), madr_ij))
|
||||||
@@ -511,7 +511,6 @@ def subtree_vel(m: Model, d: Data) -> Data:
|
|||||||
angmom_child, mom_parent_child = carry
|
angmom_child, mom_parent_child = carry
|
||||||
return angmom + mom + angmom_child + mom_parent_child, mom_parent
|
return angmom + mom + angmom_child + mom_parent_child, mom_parent
|
||||||
|
|
||||||
|
|
||||||
subtree_angmom, _ = scan.body_tree(
|
subtree_angmom, _ = scan.body_tree(
|
||||||
m,
|
m,
|
||||||
_subtree_angmom,
|
_subtree_angmom,
|
||||||
@@ -535,6 +534,7 @@ def subtree_vel(m: Model, d: Data) -> Data:
|
|||||||
|
|
||||||
def rne(m: Model, d: Data) -> Data:
|
def rne(m: Model, d: Data) -> Data:
|
||||||
"""Computes inverse dynamics using the recursive Newton-Euler algorithm."""
|
"""Computes inverse dynamics using the recursive Newton-Euler algorithm."""
|
||||||
|
|
||||||
# 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):
|
def cacc_fn(cacc, cdof_dot, qvel):
|
||||||
if cacc is None:
|
if cacc is None:
|
||||||
|
|||||||
@@ -52,6 +52,7 @@ class _Context(PyTreeNode):
|
|||||||
u: friction cone (normal and tangents) (num(con.dim > 1), 6)
|
u: friction cone (normal and tangents) (num(con.dim > 1), 6)
|
||||||
h: cone hessian (num(con.dim > 1), 6, 6)
|
h: cone hessian (num(con.dim > 1), 6, 6)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
qacc: jax.Array
|
qacc: jax.Array
|
||||||
qfrc_constraint: jax.Array
|
qfrc_constraint: jax.Array
|
||||||
Jaref: jax.Array # pylint: disable=invalid-name
|
Jaref: jax.Array # pylint: disable=invalid-name
|
||||||
@@ -225,6 +226,7 @@ class _LSContext(PyTreeNode):
|
|||||||
|
|
||||||
def _while_loop_scan(cond_fun, body_fun, init_val, max_iter):
|
def _while_loop_scan(cond_fun, body_fun, init_val, max_iter):
|
||||||
"""Scan-based implementation (jit ok, reverse-mode autodiff ok)."""
|
"""Scan-based implementation (jit ok, reverse-mode autodiff ok)."""
|
||||||
|
|
||||||
def _iter(val):
|
def _iter(val):
|
||||||
next_val = body_fun(val)
|
next_val = body_fun(val)
|
||||||
next_cond = cond_fun(next_val)
|
next_cond = cond_fun(next_val)
|
||||||
@@ -382,7 +384,7 @@ def _update_gradient(m: Model, d: Data, ctx: _Context) -> _Context:
|
|||||||
# 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.efc_J.T @ cm @ d.efc_J
|
||||||
else:
|
else:
|
||||||
h = (d.efc_J.T * d.efc_D * ctx.active) @ d.efc_J
|
h = (d.efc_J.T * d.efc_D * ctx.active) @ d.efc_J
|
||||||
|
|||||||
@@ -116,7 +116,7 @@ def efc_order(m: mujoco.MjModel, d: mujoco.MjData, dx: Data) -> np.ndarray:
|
|||||||
if dx.contact.dim[i] > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL:
|
if dx.contact.dim[i] > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL:
|
||||||
num_rows = (dx.contact.dim[i] - 1) * 2
|
num_rows = (dx.contact.dim[i] - 1) * 2
|
||||||
if dx.contact.dist[i] > 0: # move empty contacts to end
|
if dx.contact.dist[i] > 0: # move empty contacts to end
|
||||||
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.contact.geom[i]).all(axis=-1)
|
||||||
contact_match &= (d.contact.pos == dx.contact.pos[i]).all(axis=-1)
|
contact_match &= (d.contact.pos == dx.contact.pos[i]).all(axis=-1)
|
||||||
|
|||||||
@@ -47,6 +47,7 @@ class DisableBit(enum.IntFlag):
|
|||||||
REFSAFE: integrator safety: make ref[0]>=2*timestep
|
REFSAFE: integrator safety: make ref[0]>=2*timestep
|
||||||
SENSOR: sensors
|
SENSOR: sensors
|
||||||
"""
|
"""
|
||||||
|
|
||||||
CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT
|
CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT
|
||||||
EQUALITY = mujoco.mjtDisableBit.mjDSBL_EQUALITY
|
EQUALITY = mujoco.mjtDisableBit.mjDSBL_EQUALITY
|
||||||
FRICTIONLOSS = mujoco.mjtDisableBit.mjDSBL_FRICTIONLOSS
|
FRICTIONLOSS = mujoco.mjtDisableBit.mjDSBL_FRICTIONLOSS
|
||||||
@@ -73,6 +74,7 @@ class JointType(enum.IntEnum):
|
|||||||
SLIDE: sliding distance along body-fixed axis (1,)
|
SLIDE: sliding distance along body-fixed axis (1,)
|
||||||
HINGE: rotation angle (rad) around body-fixed axis (1,)
|
HINGE: rotation angle (rad) around body-fixed axis (1,)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
FREE = mujoco.mjtJoint.mjJNT_FREE
|
FREE = mujoco.mjtJoint.mjJNT_FREE
|
||||||
BALL = mujoco.mjtJoint.mjJNT_BALL
|
BALL = mujoco.mjtJoint.mjJNT_BALL
|
||||||
SLIDE = mujoco.mjtJoint.mjJNT_SLIDE
|
SLIDE = mujoco.mjtJoint.mjJNT_SLIDE
|
||||||
@@ -93,6 +95,7 @@ class IntegratorType(enum.IntEnum):
|
|||||||
RK4: 4th-order Runge Kutta
|
RK4: 4th-order Runge Kutta
|
||||||
IMPLICITFAST: implicit in velocity, no rne derivative
|
IMPLICITFAST: implicit in velocity, no rne derivative
|
||||||
"""
|
"""
|
||||||
|
|
||||||
EULER = mujoco.mjtIntegrator.mjINT_EULER
|
EULER = mujoco.mjtIntegrator.mjINT_EULER
|
||||||
RK4 = mujoco.mjtIntegrator.mjINT_RK4
|
RK4 = mujoco.mjtIntegrator.mjINT_RK4
|
||||||
IMPLICITFAST = mujoco.mjtIntegrator.mjINT_IMPLICITFAST
|
IMPLICITFAST = mujoco.mjtIntegrator.mjINT_IMPLICITFAST
|
||||||
@@ -113,6 +116,7 @@ class GeomType(enum.IntEnum):
|
|||||||
MESH: mesh
|
MESH: mesh
|
||||||
SDF: signed distance field
|
SDF: signed distance field
|
||||||
"""
|
"""
|
||||||
|
|
||||||
PLANE = mujoco.mjtGeom.mjGEOM_PLANE
|
PLANE = mujoco.mjtGeom.mjGEOM_PLANE
|
||||||
HFIELD = mujoco.mjtGeom.mjGEOM_HFIELD
|
HFIELD = mujoco.mjtGeom.mjGEOM_HFIELD
|
||||||
SPHERE = mujoco.mjtGeom.mjGEOM_SPHERE
|
SPHERE = mujoco.mjtGeom.mjGEOM_SPHERE
|
||||||
@@ -134,6 +138,7 @@ class ConvexMesh(PyTreeNode):
|
|||||||
edge: edge indexes for all edges in the convex mesh
|
edge: edge indexes for all edges in the convex mesh
|
||||||
edge_face_normal: indexes for face normals adjacent to edges in `edge`
|
edge_face_normal: indexes for face normals adjacent to edges in `edge`
|
||||||
"""
|
"""
|
||||||
|
|
||||||
vert: jax.Array
|
vert: jax.Array
|
||||||
face: jax.Array
|
face: jax.Array
|
||||||
face_normal: jax.Array
|
face_normal: jax.Array
|
||||||
@@ -148,6 +153,7 @@ class ConeType(enum.IntEnum):
|
|||||||
PYRAMIDAL: pyramidal
|
PYRAMIDAL: pyramidal
|
||||||
ELLIPTIC: elliptic
|
ELLIPTIC: elliptic
|
||||||
"""
|
"""
|
||||||
|
|
||||||
PYRAMIDAL = mujoco.mjtCone.mjCONE_PYRAMIDAL
|
PYRAMIDAL = mujoco.mjtCone.mjCONE_PYRAMIDAL
|
||||||
ELLIPTIC = mujoco.mjtCone.mjCONE_ELLIPTIC
|
ELLIPTIC = mujoco.mjtCone.mjCONE_ELLIPTIC
|
||||||
|
|
||||||
@@ -160,6 +166,7 @@ class JacobianType(enum.IntEnum):
|
|||||||
SPARSE: sparse
|
SPARSE: sparse
|
||||||
AUTO: sparse if nv>60 and device is TPU, dense otherwise
|
AUTO: sparse if nv>60 and device is TPU, dense otherwise
|
||||||
"""
|
"""
|
||||||
|
|
||||||
DENSE = mujoco.mjtJacobian.mjJAC_DENSE
|
DENSE = mujoco.mjtJacobian.mjJAC_DENSE
|
||||||
SPARSE = mujoco.mjtJacobian.mjJAC_SPARSE
|
SPARSE = mujoco.mjtJacobian.mjJAC_SPARSE
|
||||||
AUTO = mujoco.mjtJacobian.mjJAC_AUTO
|
AUTO = mujoco.mjtJacobian.mjJAC_AUTO
|
||||||
@@ -172,6 +179,7 @@ class SolverType(enum.IntEnum):
|
|||||||
CG: Conjugate gradient (primal)
|
CG: Conjugate gradient (primal)
|
||||||
NEWTON: Newton (primal)
|
NEWTON: Newton (primal)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
# unsupported: PGS
|
# unsupported: PGS
|
||||||
CG = mujoco.mjtSolver.mjSOL_CG
|
CG = mujoco.mjtSolver.mjSOL_CG
|
||||||
NEWTON = mujoco.mjtSolver.mjSOL_NEWTON
|
NEWTON = mujoco.mjtSolver.mjSOL_NEWTON
|
||||||
@@ -186,6 +194,7 @@ class EqType(enum.IntEnum):
|
|||||||
JOINT: couple the values of two scalar joints with cubic
|
JOINT: couple the values of two scalar joints with cubic
|
||||||
TENDON: couple the lengths of two tendons with cubic
|
TENDON: couple the lengths of two tendons with cubic
|
||||||
"""
|
"""
|
||||||
|
|
||||||
CONNECT = mujoco.mjtEq.mjEQ_CONNECT
|
CONNECT = mujoco.mjtEq.mjEQ_CONNECT
|
||||||
WELD = mujoco.mjtEq.mjEQ_WELD
|
WELD = mujoco.mjtEq.mjEQ_WELD
|
||||||
JOINT = mujoco.mjtEq.mjEQ_JOINT
|
JOINT = mujoco.mjtEq.mjEQ_JOINT
|
||||||
@@ -203,6 +212,7 @@ class WrapType(enum.IntEnum):
|
|||||||
SPHERE: wrap around sphere
|
SPHERE: wrap around sphere
|
||||||
CYLINDER: wrap around (infinite) cylinder
|
CYLINDER: wrap around (infinite) cylinder
|
||||||
"""
|
"""
|
||||||
|
|
||||||
JOINT = mujoco.mjtWrap.mjWRAP_JOINT
|
JOINT = mujoco.mjtWrap.mjWRAP_JOINT
|
||||||
PULLEY = mujoco.mjtWrap.mjWRAP_PULLEY
|
PULLEY = mujoco.mjtWrap.mjWRAP_PULLEY
|
||||||
SITE = mujoco.mjtWrap.mjWRAP_SITE
|
SITE = mujoco.mjtWrap.mjWRAP_SITE
|
||||||
@@ -219,6 +229,7 @@ class TrnType(enum.IntEnum):
|
|||||||
TENDON: force on tendon
|
TENDON: force on tendon
|
||||||
SITE: force on site
|
SITE: force on site
|
||||||
"""
|
"""
|
||||||
|
|
||||||
JOINT = mujoco.mjtTrn.mjTRN_JOINT
|
JOINT = mujoco.mjtTrn.mjTRN_JOINT
|
||||||
JOINTINPARENT = mujoco.mjtTrn.mjTRN_JOINTINPARENT
|
JOINTINPARENT = mujoco.mjtTrn.mjTRN_JOINTINPARENT
|
||||||
SITE = mujoco.mjtTrn.mjTRN_SITE
|
SITE = mujoco.mjtTrn.mjTRN_SITE
|
||||||
@@ -236,6 +247,7 @@ class DynType(enum.IntEnum):
|
|||||||
FILTEREXACT: linear filter: da/dt = (u-a) / tau, with exact integration
|
FILTEREXACT: linear filter: da/dt = (u-a) / tau, with exact integration
|
||||||
MUSCLE: piece-wise linear filter with two time constants
|
MUSCLE: piece-wise linear filter with two time constants
|
||||||
"""
|
"""
|
||||||
|
|
||||||
NONE = mujoco.mjtDyn.mjDYN_NONE
|
NONE = mujoco.mjtDyn.mjDYN_NONE
|
||||||
INTEGRATOR = mujoco.mjtDyn.mjDYN_INTEGRATOR
|
INTEGRATOR = mujoco.mjtDyn.mjDYN_INTEGRATOR
|
||||||
FILTER = mujoco.mjtDyn.mjDYN_FILTER
|
FILTER = mujoco.mjtDyn.mjDYN_FILTER
|
||||||
@@ -252,6 +264,7 @@ class GainType(enum.IntEnum):
|
|||||||
AFFINE: const + kp*length + kv*velocity
|
AFFINE: const + kp*length + kv*velocity
|
||||||
MUSCLE: muscle FLV curve computed by muscle_gain
|
MUSCLE: muscle FLV curve computed by muscle_gain
|
||||||
"""
|
"""
|
||||||
|
|
||||||
FIXED = mujoco.mjtGain.mjGAIN_FIXED
|
FIXED = mujoco.mjtGain.mjGAIN_FIXED
|
||||||
AFFINE = mujoco.mjtGain.mjGAIN_AFFINE
|
AFFINE = mujoco.mjtGain.mjGAIN_AFFINE
|
||||||
MUSCLE = mujoco.mjtGain.mjGAIN_MUSCLE
|
MUSCLE = mujoco.mjtGain.mjGAIN_MUSCLE
|
||||||
@@ -266,6 +279,7 @@ class BiasType(enum.IntEnum):
|
|||||||
AFFINE: const + kp*length + kv*velocity
|
AFFINE: const + kp*length + kv*velocity
|
||||||
MUSCLE: muscle passive force computed by muscle_bias
|
MUSCLE: muscle passive force computed by muscle_bias
|
||||||
"""
|
"""
|
||||||
|
|
||||||
NONE = mujoco.mjtBias.mjBIAS_NONE
|
NONE = mujoco.mjtBias.mjBIAS_NONE
|
||||||
AFFINE = mujoco.mjtBias.mjBIAS_AFFINE
|
AFFINE = mujoco.mjtBias.mjBIAS_AFFINE
|
||||||
MUSCLE = mujoco.mjtBias.mjBIAS_MUSCLE
|
MUSCLE = mujoco.mjtBias.mjBIAS_MUSCLE
|
||||||
@@ -282,6 +296,7 @@ class ConstraintType(enum.IntEnum):
|
|||||||
CONTACT_FRICTIONLESS: frictionless contact
|
CONTACT_FRICTIONLESS: frictionless contact
|
||||||
CONTACT_PYRAMIDAL: frictional contact, pyramidal friction cone
|
CONTACT_PYRAMIDAL: frictional contact, pyramidal friction cone
|
||||||
"""
|
"""
|
||||||
|
|
||||||
EQUALITY = mujoco.mjtConstraint.mjCNSTR_EQUALITY
|
EQUALITY = mujoco.mjtConstraint.mjCNSTR_EQUALITY
|
||||||
FRICTION_DOF = mujoco.mjtConstraint.mjCNSTR_FRICTION_DOF
|
FRICTION_DOF = mujoco.mjtConstraint.mjCNSTR_FRICTION_DOF
|
||||||
FRICTION_TENDON = mujoco.mjtConstraint.mjCNSTR_FRICTION_TENDON
|
FRICTION_TENDON = mujoco.mjtConstraint.mjCNSTR_FRICTION_TENDON
|
||||||
@@ -302,6 +317,7 @@ class CamLightType(enum.IntEnum):
|
|||||||
TARGETBODY: pos fixed in body, rot tracks target body
|
TARGETBODY: pos fixed in body, rot tracks target body
|
||||||
TARGETBODYCOM: pos fixed in body, rot tracks target subtree com
|
TARGETBODYCOM: pos fixed in body, rot tracks target subtree com
|
||||||
"""
|
"""
|
||||||
|
|
||||||
FIXED = mujoco.mjtCamLight.mjCAMLIGHT_FIXED
|
FIXED = mujoco.mjtCamLight.mjCAMLIGHT_FIXED
|
||||||
TRACK = mujoco.mjtCamLight.mjCAMLIGHT_TRACK
|
TRACK = mujoco.mjtCamLight.mjCAMLIGHT_TRACK
|
||||||
TRACKCOM = mujoco.mjtCamLight.mjCAMLIGHT_TRACKCOM
|
TRACKCOM = mujoco.mjtCamLight.mjCAMLIGHT_TRACKCOM
|
||||||
@@ -346,6 +362,7 @@ class SensorType(enum.IntEnum):
|
|||||||
FRAMELINACC: 3D linear acceleration
|
FRAMELINACC: 3D linear acceleration
|
||||||
FRAMEANGACC: 3D angular acceleration
|
FRAMEANGACC: 3D angular acceleration
|
||||||
"""
|
"""
|
||||||
|
|
||||||
MAGNETOMETER = mujoco.mjtSensor.mjSENS_MAGNETOMETER
|
MAGNETOMETER = mujoco.mjtSensor.mjSENS_MAGNETOMETER
|
||||||
CAMPROJECTION = mujoco.mjtSensor.mjSENS_CAMPROJECTION
|
CAMPROJECTION = mujoco.mjtSensor.mjSENS_CAMPROJECTION
|
||||||
RANGEFINDER = mujoco.mjtSensor.mjSENS_RANGEFINDER
|
RANGEFINDER = mujoco.mjtSensor.mjSENS_RANGEFINDER
|
||||||
@@ -391,6 +408,7 @@ class ObjType(PyTreeNode):
|
|||||||
SITE: site
|
SITE: site
|
||||||
CAMERA: camera
|
CAMERA: camera
|
||||||
"""
|
"""
|
||||||
|
|
||||||
UNKNOWN = mujoco.mjtObj.mjOBJ_UNKNOWN
|
UNKNOWN = mujoco.mjtObj.mjOBJ_UNKNOWN
|
||||||
BODY = mujoco.mjtObj.mjOBJ_BODY
|
BODY = mujoco.mjtObj.mjOBJ_BODY
|
||||||
XBODY = mujoco.mjtObj.mjOBJ_XBODY
|
XBODY = mujoco.mjtObj.mjOBJ_XBODY
|
||||||
@@ -437,7 +455,7 @@ class Option(PyTreeNode):
|
|||||||
disableactuator: bit flags for disabling actuators by group id (not used)
|
disableactuator: bit flags for disabling actuators by group id (not used)
|
||||||
sdf_initpoints: number of starting points for gradient descent (not used)
|
sdf_initpoints: number of starting points for gradient descent (not used)
|
||||||
sdf_iterations: max number of iterations for gradient descent (not used)
|
sdf_iterations: max number of iterations for gradient descent (not used)
|
||||||
"""
|
""" # fmt: skip
|
||||||
timestep: jax.Array
|
timestep: jax.Array
|
||||||
apirate: jax.Array = _restricted_to('mujoco')
|
apirate: jax.Array = _restricted_to('mujoco')
|
||||||
impratio: jax.Array
|
impratio: jax.Array
|
||||||
@@ -480,6 +498,7 @@ class Statistic(PyTreeNode):
|
|||||||
extent: spatial extent (not used)
|
extent: spatial extent (not used)
|
||||||
center: center of model (not used)
|
center: center of model (not used)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
meaninertia: jax.Array
|
meaninertia: jax.Array
|
||||||
meanmass: jax.Array
|
meanmass: jax.Array
|
||||||
meansize: jax.Array
|
meansize: jax.Array
|
||||||
@@ -813,6 +832,7 @@ class Model(PyTreeNode):
|
|||||||
name_keyadr: keyframe name pointers (nkey,)
|
name_keyadr: keyframe name pointers (nkey,)
|
||||||
names: names of all objects, 0-terminated (nnames,)
|
names: names of all objects, 0-terminated (nnames,)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
nq: int
|
nq: int
|
||||||
nv: int
|
nv: int
|
||||||
nu: int
|
nu: int
|
||||||
@@ -1161,7 +1181,7 @@ class Contact(PyTreeNode):
|
|||||||
geom2: id of geom 2; deprecated, use geom[1]
|
geom2: id of geom 2; deprecated, use geom[1]
|
||||||
geom: geom ids (2,)
|
geom: geom ids (2,)
|
||||||
efc_address: address in efc; -1: not included
|
efc_address: address in efc; -1: not included
|
||||||
"""
|
""" # fmt: skip
|
||||||
dist: jax.Array
|
dist: jax.Array
|
||||||
pos: jax.Array
|
pos: jax.Array
|
||||||
frame: jax.Array
|
frame: jax.Array
|
||||||
@@ -1306,7 +1326,7 @@ class Data(PyTreeNode):
|
|||||||
_qM_sparse: qM in sparse representation (nM,)
|
_qM_sparse: qM in sparse representation (nM,)
|
||||||
_qLD_sparse: qLD in sparse representation (nM,)
|
_qLD_sparse: qLD in sparse representation (nM,)
|
||||||
_qLDiagInv_sparse: qLDiagInv in sparse representation (nv,)
|
_qLDiagInv_sparse: qLDiagInv in sparse representation (nv,)
|
||||||
"""
|
""" # fmt: skip
|
||||||
# constant sizes:
|
# constant sizes:
|
||||||
ne: int
|
ne: int
|
||||||
nf: int
|
nf: int
|
||||||
|
|||||||
@@ -22,7 +22,9 @@ from etils import epath
|
|||||||
import mujoco
|
import mujoco
|
||||||
from mujoco import mjx
|
from mujoco import mjx
|
||||||
|
|
||||||
_MJCF = flags.DEFINE_string('mjcf', None, 'path to model `.xml` or `.mjb`', required=True)
|
_MJCF = flags.DEFINE_string(
|
||||||
|
'mjcf', None, 'path to model `.xml` or `.mjb`', required=True
|
||||||
|
)
|
||||||
_BASE_PATH = flags.DEFINE_string(
|
_BASE_PATH = flags.DEFINE_string(
|
||||||
'base_path', None, 'base path, defaults to mujoco.mjx resource path'
|
'base_path', None, 'base path, defaults to mujoco.mjx resource path'
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -28,8 +28,9 @@ import mujoco.viewer
|
|||||||
|
|
||||||
|
|
||||||
_JIT = flags.DEFINE_bool('jit', True, 'To jit or not to jit.')
|
_JIT = flags.DEFINE_bool('jit', True, 'To jit or not to jit.')
|
||||||
_MODEL_PATH = flags.DEFINE_string('mjcf', None, 'Path to a MuJoCo MJCF file.',
|
_MODEL_PATH = flags.DEFINE_string(
|
||||||
required=True)
|
'mjcf', None, 'Path to a MuJoCo MJCF file.', required=True
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
_VIEWER_GLOBAL_STATE = {
|
_VIEWER_GLOBAL_STATE = {
|
||||||
|
|||||||
@@ -44,3 +44,24 @@ Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
|||||||
Documentation = "https://mujoco.readthedocs.io/en/3.2.7"
|
Documentation = "https://mujoco.readthedocs.io/en/3.2.7"
|
||||||
Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
||||||
Changelog = "https://mujoco.readthedocs.io/en/3.2.7/changelog.html"
|
Changelog = "https://mujoco.readthedocs.io/en/3.2.7/changelog.html"
|
||||||
|
|
||||||
|
[tool.isort]
|
||||||
|
force_single_line = true
|
||||||
|
force_sort_within_sections = true
|
||||||
|
lexicographical = true
|
||||||
|
single_line_exclusions = ["typing"]
|
||||||
|
order_by_type = false
|
||||||
|
group_by_package = true
|
||||||
|
line_length = 120
|
||||||
|
use_parentheses = true
|
||||||
|
multi_line_output = 3
|
||||||
|
skip_glob = ["**/*.ipynb"]
|
||||||
|
|
||||||
|
[tool.pyink]
|
||||||
|
line-length = 80
|
||||||
|
unstable = true
|
||||||
|
pyink-indentation = 2
|
||||||
|
pyink-use-majority-quotes = true
|
||||||
|
extend-exclude = '''(
|
||||||
|
.ipynb$
|
||||||
|
)'''
|
||||||
|
|||||||
+268
-142
@@ -120,13 +120,17 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
xml_2 = rb"""<mujoco><geom name="box" type="box" size="1 1 1"/></mujoco>"""
|
xml_2 = rb"""<mujoco><geom name="box" type="box" size="1 1 1"/></mujoco>"""
|
||||||
xml_3 = rb"""<mujoco><geom name="ball" type="sphere" size="1"/></mujoco>"""
|
xml_3 = rb"""<mujoco><geom name="ball" type="sphere" size="1"/></mujoco>"""
|
||||||
model = mujoco.MjModel.from_xml_string(
|
model = mujoco.MjModel.from_xml_string(
|
||||||
xml_1, {'model_.xml': xml_2, 'model__.xml': xml_3})
|
xml_1, {'model_.xml': xml_2, 'model__.xml': xml_3}
|
||||||
|
)
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'plane'), 0)
|
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'plane'), 0
|
||||||
|
)
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'box'), 1)
|
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'box'), 1
|
||||||
|
)
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'ball'), 2)
|
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'ball'), 2
|
||||||
|
)
|
||||||
|
|
||||||
def test_load_xml_repeated_asset_name(self):
|
def test_load_xml_repeated_asset_name(self):
|
||||||
# Assets aren't allowed to have the same filename (even if they have
|
# Assets aren't allowed to have the same filename (even if they have
|
||||||
@@ -139,23 +143,25 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
def test_can_read_array(self):
|
def test_can_read_array(self):
|
||||||
np.testing.assert_array_equal(
|
np.testing.assert_array_equal(
|
||||||
self.model.body_pos,
|
self.model.body_pos,
|
||||||
[[0, 0, 0], [0, 0, 0.1], [0, 0, 0], [0, 0, 0], [42.0, 0, 42.0]])
|
[[0, 0, 0], [0, 0, 0.1], [0, 0, 0], [0, 0, 0], [42.0, 0, 42.0]],
|
||||||
|
)
|
||||||
|
|
||||||
def test_can_set_array(self):
|
def test_can_set_array(self):
|
||||||
self.data.qpos = 0.12345
|
self.data.qpos = 0.12345
|
||||||
np.testing.assert_array_equal(
|
np.testing.assert_array_equal(
|
||||||
self.data.qpos, [0.12345]*len(self.data.qpos))
|
self.data.qpos, [0.12345] * len(self.data.qpos)
|
||||||
|
)
|
||||||
|
|
||||||
def test_array_is_a_view(self):
|
def test_array_is_a_view(self):
|
||||||
qpos_ref = self.data.qpos
|
qpos_ref = self.data.qpos
|
||||||
self.data.qpos = 0.789
|
self.data.qpos = 0.789
|
||||||
np.testing.assert_array_equal(
|
np.testing.assert_array_equal(qpos_ref, [0.789] * len(self.data.qpos))
|
||||||
qpos_ref, [0.789]*len(self.data.qpos))
|
|
||||||
|
|
||||||
# This test is disabled on PyPy as it uses sys.getrefcount
|
# This test is disabled on PyPy as it uses sys.getrefcount
|
||||||
# However PyPy is not officially supported by MuJoCo
|
# However PyPy is not officially supported by MuJoCo
|
||||||
@absltest.skipIf(sys.implementation.name == 'pypy',
|
@absltest.skipIf(
|
||||||
reason='requires sys.getrefcount')
|
sys.implementation.name == 'pypy', reason='requires sys.getrefcount'
|
||||||
|
)
|
||||||
def test_array_keeps_struct_alive(self):
|
def test_array_keeps_struct_alive(self):
|
||||||
model = mujoco.MjModel.from_xml_string(TEST_XML)
|
model = mujoco.MjModel.from_xml_string(TEST_XML)
|
||||||
qpos0 = model.qpos0
|
qpos0 = model.qpos0
|
||||||
@@ -185,11 +191,15 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
|
|
||||||
def test_named_indexing_actuator_ctrl(self):
|
def test_named_indexing_actuator_ctrl(self):
|
||||||
actuator_id = mujoco.mj_name2id(
|
actuator_id = mujoco.mj_name2id(
|
||||||
self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, 'myactuator')
|
self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, 'myactuator'
|
||||||
self.assertIs(self.data.actuator('myactuator'),
|
)
|
||||||
self.data.actuator(actuator_id))
|
self.assertIs(
|
||||||
self.assertIs(self.data.actuator('myactuator').ctrl,
|
self.data.actuator('myactuator'), self.data.actuator(actuator_id)
|
||||||
self.data.actuator(actuator_id).ctrl)
|
)
|
||||||
|
self.assertIs(
|
||||||
|
self.data.actuator('myactuator').ctrl,
|
||||||
|
self.data.actuator(actuator_id).ctrl,
|
||||||
|
)
|
||||||
self.assertEqual(self.data.actuator('myactuator').ctrl.shape, (1,))
|
self.assertEqual(self.data.actuator('myactuator').ctrl.shape, (1,))
|
||||||
|
|
||||||
# Test that the indexer is returning a view into the underlying struct.
|
# Test that the indexer is returning a view into the underlying struct.
|
||||||
@@ -202,41 +212,49 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
def test_named_indexing_invalid_names_in_model(self):
|
def test_named_indexing_invalid_names_in_model(self):
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
KeyError,
|
KeyError,
|
||||||
r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]"):
|
r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]",
|
||||||
|
):
|
||||||
self.model.geom('badgeom')
|
self.model.geom('badgeom')
|
||||||
|
|
||||||
def test_named_indexing_no_name_argument_in_model(self):
|
def test_named_indexing_no_name_argument_in_model(self):
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
KeyError,
|
KeyError,
|
||||||
r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]"):
|
r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]",
|
||||||
|
):
|
||||||
self.model.joint()
|
self.model.joint()
|
||||||
|
|
||||||
def test_named_indexing_invalid_names_in_data(self):
|
def test_named_indexing_invalid_names_in_data(self):
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
KeyError,
|
KeyError,
|
||||||
r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]"):
|
r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]",
|
||||||
|
):
|
||||||
self.data.geom('badgeom')
|
self.data.geom('badgeom')
|
||||||
|
|
||||||
def test_named_indexing_no_name_argument_in_data(self):
|
def test_named_indexing_no_name_argument_in_data(self):
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
KeyError,
|
KeyError,
|
||||||
r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]"):
|
r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]",
|
||||||
|
):
|
||||||
self.data.jnt()
|
self.data.jnt()
|
||||||
|
|
||||||
def test_named_indexing_invalid_index_in_model(self):
|
def test_named_indexing_invalid_index_in_model(self):
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
IndexError, r'Invalid index 3\. Valid indices from 0 to 2'):
|
IndexError, r'Invalid index 3\. Valid indices from 0 to 2'
|
||||||
|
):
|
||||||
self.model.geom(3)
|
self.model.geom(3)
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
IndexError, r'Invalid index -1\. Valid indices from 0 to 2'):
|
IndexError, r'Invalid index -1\. Valid indices from 0 to 2'
|
||||||
|
):
|
||||||
self.model.geom(-1)
|
self.model.geom(-1)
|
||||||
|
|
||||||
def test_named_indexing_invalid_index_in_data(self):
|
def test_named_indexing_invalid_index_in_data(self):
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
IndexError, r'Invalid index 3\. Valid indices from 0 to 2'):
|
IndexError, r'Invalid index 3\. Valid indices from 0 to 2'
|
||||||
|
):
|
||||||
self.data.geom(3)
|
self.data.geom(3)
|
||||||
with self.assertRaisesRegex(
|
with self.assertRaisesRegex(
|
||||||
IndexError, r'Invalid index -1\. Valid indices from 0 to 2'):
|
IndexError, r'Invalid index -1\. Valid indices from 0 to 2'
|
||||||
|
):
|
||||||
self.data.geom(-1)
|
self.data.geom(-1)
|
||||||
|
|
||||||
def test_named_indexing_geom_size(self):
|
def test_named_indexing_geom_size(self):
|
||||||
@@ -267,45 +285,53 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
|
|
||||||
def test_named_indexing_ragged_qpos(self):
|
def test_named_indexing_ragged_qpos(self):
|
||||||
balljoint_id = mujoco.mj_name2id(
|
balljoint_id = mujoco.mj_name2id(
|
||||||
self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myball')
|
self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myball'
|
||||||
|
)
|
||||||
self.assertIs(self.data.joint('myball'), self.data.joint(balljoint_id))
|
self.assertIs(self.data.joint('myball'), self.data.joint(balljoint_id))
|
||||||
self.assertIs(self.data.joint('myball').qpos,
|
self.assertIs(
|
||||||
self.data.joint(balljoint_id).qpos)
|
self.data.joint('myball').qpos, self.data.joint(balljoint_id).qpos
|
||||||
|
)
|
||||||
self.assertEqual(self.data.joint('myball').qpos.shape, (4,))
|
self.assertEqual(self.data.joint('myball').qpos.shape, (4,))
|
||||||
|
|
||||||
# Test that the indexer is returning a view into the underlying struct.
|
# Test that the indexer is returning a view into the underlying struct.
|
||||||
qpos_from_indexer = self.data.joint('myball').qpos
|
qpos_from_indexer = self.data.joint('myball').qpos
|
||||||
qpos_idx = self.model.jnt_qposadr[balljoint_id]
|
qpos_idx = self.model.jnt_qposadr[balljoint_id]
|
||||||
self.data.qpos[qpos_idx:qpos_idx+4] = [4, 5, 6, 7]
|
self.data.qpos[qpos_idx : qpos_idx + 4] = [4, 5, 6, 7]
|
||||||
np.testing.assert_array_equal(qpos_from_indexer, [4, 5, 6, 7])
|
np.testing.assert_array_equal(qpos_from_indexer, [4, 5, 6, 7])
|
||||||
self.data.joint('myball').qpos = [9, 8, 7, 6]
|
self.data.joint('myball').qpos = [9, 8, 7, 6]
|
||||||
np.testing.assert_array_equal(self.data.qpos[qpos_idx:qpos_idx+4],
|
np.testing.assert_array_equal(
|
||||||
[9, 8, 7, 6])
|
self.data.qpos[qpos_idx : qpos_idx + 4], [9, 8, 7, 6]
|
||||||
|
)
|
||||||
|
|
||||||
def test_named_indexing_ragged2d_cdof(self):
|
def test_named_indexing_ragged2d_cdof(self):
|
||||||
freejoint_id = mujoco.mj_name2id(
|
freejoint_id = mujoco.mj_name2id(
|
||||||
self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myfree')
|
self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myfree'
|
||||||
|
)
|
||||||
self.assertIs(self.data.joint('myfree'), self.data.joint(freejoint_id))
|
self.assertIs(self.data.joint('myfree'), self.data.joint(freejoint_id))
|
||||||
self.assertIs(self.data.joint('myfree').cdof,
|
self.assertIs(
|
||||||
self.data.joint(freejoint_id).cdof)
|
self.data.joint('myfree').cdof, self.data.joint(freejoint_id).cdof
|
||||||
|
)
|
||||||
self.assertEqual(self.data.joint('myfree').cdof.shape, (6, 6))
|
self.assertEqual(self.data.joint('myfree').cdof.shape, (6, 6))
|
||||||
|
|
||||||
# Test that the indexer is returning a view into the underlying struct.
|
# Test that the indexer is returning a view into the underlying struct.
|
||||||
cdof_from_indexer = self.data.joint('myfree').cdof
|
cdof_from_indexer = self.data.joint('myfree').cdof
|
||||||
dof_idx = self.model.jnt_dofadr[freejoint_id]
|
dof_idx = self.model.jnt_dofadr[freejoint_id]
|
||||||
self.data.cdof[dof_idx:dof_idx+6, :] = np.reshape(range(36), (6, 6))
|
self.data.cdof[dof_idx : dof_idx + 6, :] = np.reshape(range(36), (6, 6))
|
||||||
np.testing.assert_array_equal(cdof_from_indexer,
|
np.testing.assert_array_equal(
|
||||||
np.reshape(range(36), (6, 6)))
|
cdof_from_indexer, np.reshape(range(36), (6, 6))
|
||||||
|
)
|
||||||
self.data.joint('myfree').cdof = 42
|
self.data.joint('myfree').cdof = 42
|
||||||
np.testing.assert_array_equal(self.data.cdof[dof_idx:dof_idx+6], [[42]*6]*6)
|
np.testing.assert_array_equal(
|
||||||
|
self.data.cdof[dof_idx : dof_idx + 6], [[42] * 6] * 6
|
||||||
|
)
|
||||||
|
|
||||||
def test_named_indexing_repr_in_data(self):
|
def test_named_indexing_repr_in_data(self):
|
||||||
expected_repr = '''<_MjDataGeomViews
|
expected_repr = """<_MjDataGeomViews
|
||||||
id: 1
|
id: 1
|
||||||
name: 'mybox'
|
name: 'mybox'
|
||||||
xmat: array([0., 0., 0., 0., 0., 0., 0., 0., 0.])
|
xmat: array([0., 0., 0., 0., 0., 0., 0., 0., 0.])
|
||||||
xpos: array([0., 0., 0.])
|
xpos: array([0., 0., 0.])
|
||||||
>'''
|
>"""
|
||||||
self.assertEqual(expected_repr, repr(self.data.geom('mybox')))
|
self.assertEqual(expected_repr, repr(self.data.geom('mybox')))
|
||||||
|
|
||||||
def test_named_indexing_body_repr_in_data(self):
|
def test_named_indexing_body_repr_in_data(self):
|
||||||
@@ -328,8 +354,15 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
self.assertGreater(self.data._address, 0)
|
self.assertGreater(self.data._address, 0)
|
||||||
self.assertGreater(model2._address, 0)
|
self.assertGreater(model2._address, 0)
|
||||||
self.assertGreater(data2._address, 0)
|
self.assertGreater(data2._address, 0)
|
||||||
self.assertLen({self.model._address, self.data._address,
|
self.assertLen(
|
||||||
model2._address, data2._address}, 4)
|
{
|
||||||
|
self.model._address,
|
||||||
|
self.data._address,
|
||||||
|
model2._address,
|
||||||
|
data2._address,
|
||||||
|
},
|
||||||
|
4,
|
||||||
|
)
|
||||||
|
|
||||||
def test_mjmodel_can_read_and_write_opt(self):
|
def test_mjmodel_can_read_and_write_opt(self):
|
||||||
self.assertEqual(self.model.opt.timestep, 0.002)
|
self.assertEqual(self.model.opt.timestep, 0.002)
|
||||||
@@ -361,7 +394,9 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
def test_mjmodel_can_access_names_directly(self):
|
def test_mjmodel_can_access_names_directly(self):
|
||||||
# mjModel offers direct access to names array, to allow usecases other than
|
# mjModel offers direct access to names array, to allow usecases other than
|
||||||
# id2name
|
# id2name
|
||||||
model_name = str(self.model.names[0:self.model.names.find(b'\0')], 'utf-8')
|
model_name = str(
|
||||||
|
self.model.names[0 : self.model.names.find(b'\0')], 'utf-8'
|
||||||
|
)
|
||||||
self.assertEqual(model_name, 'test')
|
self.assertEqual(model_name, 'test')
|
||||||
|
|
||||||
start_index = self.model.name_geomadr[0]
|
start_index = self.model.name_geomadr[0]
|
||||||
@@ -402,15 +437,15 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
model_copy = copy.copy(self.model)
|
model_copy = copy.copy(self.model)
|
||||||
|
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_JOINT, 0),
|
mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_JOINT, 0), 'myfree'
|
||||||
'myfree')
|
)
|
||||||
|
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 0),
|
mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 0), 'myplane'
|
||||||
'myplane')
|
)
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 1),
|
mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 1), 'mybox'
|
||||||
'mybox')
|
)
|
||||||
|
|
||||||
# Make sure it's a copy.
|
# Make sure it's a copy.
|
||||||
self.model.geom_size[1] = 0.5
|
self.model.geom_size[1] = 0.5
|
||||||
@@ -420,7 +455,7 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
def test_mjdata_can_copy(self):
|
def test_mjdata_can_copy(self):
|
||||||
self.data.qpos = [0, 0, 0.1*np.sqrt(2) - 0.001,
|
self.data.qpos = [0, 0, 0.1*np.sqrt(2) - 0.001,
|
||||||
np.cos(np.pi/8), np.sin(np.pi/8), 0, 0, 0,
|
np.cos(np.pi/8), np.sin(np.pi/8), 0, 0, 0,
|
||||||
1, 0, 0, 0]
|
1, 0, 0, 0] # fmt: skip
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
|
|
||||||
data_copy = copy.copy(self.data)
|
data_copy = copy.copy(self.data)
|
||||||
@@ -455,7 +490,8 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
contact_copy.append(copy.copy(self.data.contact[i]))
|
contact_copy.append(copy.copy(self.data.contact[i]))
|
||||||
# Sort contacts in anticlockwise order
|
# Sort contacts in anticlockwise order
|
||||||
contact_copy = sorted(
|
contact_copy = sorted(
|
||||||
contact_copy, key=lambda x: np.arctan2(x.pos[1], x.pos[0]))
|
contact_copy, key=lambda x: np.arctan2(x.pos[1], x.pos[0])
|
||||||
|
)
|
||||||
np.testing.assert_allclose(contact_copy[0].pos[:2], [-0.1, -0.1])
|
np.testing.assert_allclose(contact_copy[0].pos[:2], [-0.1, -0.1])
|
||||||
np.testing.assert_allclose(contact_copy[1].pos[:2], [0.1, -0.1])
|
np.testing.assert_allclose(contact_copy[1].pos[:2], [0.1, -0.1])
|
||||||
np.testing.assert_allclose(contact_copy[2].pos[:2], [0.1, 0.1])
|
np.testing.assert_allclose(contact_copy[2].pos[:2], [0.1, 0.1])
|
||||||
@@ -502,7 +538,8 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
|
|
||||||
# Sort contacts in anticlockwise order
|
# Sort contacts in anticlockwise order
|
||||||
sorted_contact = sorted(
|
sorted_contact = sorted(
|
||||||
contact, key=lambda x: np.arctan2(x.pos[1], x.pos[0]))
|
contact, key=lambda x: np.arctan2(x.pos[1], x.pos[0])
|
||||||
|
)
|
||||||
np.testing.assert_allclose(sorted_contact[0].pos[:2], [-0.1, -0.1])
|
np.testing.assert_allclose(sorted_contact[0].pos[:2], [-0.1, -0.1])
|
||||||
np.testing.assert_allclose(sorted_contact[1].pos[:2], [0.1, -0.1])
|
np.testing.assert_allclose(sorted_contact[1].pos[:2], [0.1, -0.1])
|
||||||
np.testing.assert_allclose(sorted_contact[2].pos[:2], [0.1, 0.1])
|
np.testing.assert_allclose(sorted_contact[2].pos[:2], [0.1, 0.1])
|
||||||
@@ -589,7 +626,7 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
self.assertEqual(data2.ncon, 4)
|
self.assertEqual(data2.ncon, 4)
|
||||||
self.assertEqual(data2.contact, self.data.contact)
|
self.assertEqual(data2.contact, self.data.contact)
|
||||||
|
|
||||||
self.data.qpos[3:7] = [np.cos(np.pi/8), np.sin(np.pi/8), 0, 0]
|
self.data.qpos[3:7] = [np.cos(np.pi / 8), np.sin(np.pi / 8), 0, 0]
|
||||||
self.data.qpos[2] *= (np.sqrt(2) - 1) * 0.1 - 1e-6
|
self.data.qpos[2] *= (np.sqrt(2) - 1) * 0.1 - 1e-6
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
self.assertEqual(self.data.ncon, 2)
|
self.assertEqual(self.data.ncon, 2)
|
||||||
@@ -674,7 +711,7 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
|
|
||||||
def test_mju_rotVecQuat(self): # pylint: disable=invalid-name
|
def test_mju_rotVecQuat(self): # pylint: disable=invalid-name
|
||||||
vec = [1, 0, 0]
|
vec = [1, 0, 0]
|
||||||
quat = [np.cos(np.pi/8), 0, 0, np.sin(np.pi/8)]
|
quat = [np.cos(np.pi / 8), 0, 0, np.sin(np.pi / 8)]
|
||||||
expected = np.array([1, 1, 0]) / np.sqrt(2)
|
expected = np.array([1, 1, 0]) / np.sqrt(2)
|
||||||
|
|
||||||
# Check that the output argument works, and that the binding returns None.
|
# Check that the output argument works, and that the binding returns None.
|
||||||
@@ -722,7 +759,7 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
size = mujoco.mj_stateSize(self.model, spec)
|
size = mujoco.mj_stateSize(self.model, spec)
|
||||||
|
|
||||||
state_bad_size = np.empty(size + 1, np.float64)
|
state_bad_size = np.empty(size + 1, np.float64)
|
||||||
expected_message = ('state size should equal mj_stateSize(m, spec)')
|
expected_message = 'state size should equal mj_stateSize(m, spec)'
|
||||||
with self.assertRaisesWithLiteralMatch(TypeError, expected_message):
|
with self.assertRaisesWithLiteralMatch(TypeError, expected_message):
|
||||||
mujoco.mj_getState(self.model, self.data, state_bad_size, spec)
|
mujoco.mj_getState(self.model, self.data, state_bad_size, spec)
|
||||||
|
|
||||||
@@ -781,8 +818,9 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
|
|
||||||
mat = np.empty((3, 10), np.float64)
|
mat = np.empty((3, 10), np.float64)
|
||||||
mujoco.mj_angmomMat(self.model, self.data, mat, 0)
|
mujoco.mj_angmomMat(self.model, self.data, mat, 0)
|
||||||
np.testing.assert_almost_equal(mat @ self.data.qvel,
|
np.testing.assert_almost_equal(
|
||||||
self.data.subtree_angmom[0, :])
|
mat @ self.data.qvel, self.data.subtree_angmom[0, :]
|
||||||
|
)
|
||||||
|
|
||||||
def test_mj_jacSite(self): # pylint: disable=invalid-name
|
def test_mj_jacSite(self): # pylint: disable=invalid-name
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
@@ -792,20 +830,22 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
jacp = np.empty((3, 10), np.float64)
|
jacp = np.empty((3, 10), np.float64)
|
||||||
mujoco.mj_jacSite(self.model, self.data, jacp, None, site_id)
|
mujoco.mj_jacSite(self.model, self.data, jacp, None, site_id)
|
||||||
|
|
||||||
expected_jacp = np.array(
|
expected_jacp = np.array([
|
||||||
[[0, 0, 0, 0, 0, 0, -1, 0, 0, 0],
|
[0, 0, 0, 0, 0, 0, -1, 0, 0, 0],
|
||||||
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
||||||
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
|
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
||||||
|
])
|
||||||
np.testing.assert_array_equal(jacp, expected_jacp)
|
np.testing.assert_array_equal(jacp, expected_jacp)
|
||||||
|
|
||||||
# Call mj_jacSite with only jacr.
|
# Call mj_jacSite with only jacr.
|
||||||
jacr = np.empty((3, 10), np.float64)
|
jacr = np.empty((3, 10), np.float64)
|
||||||
mujoco.mj_jacSite(self.model, self.data, None, jacr, site_id)
|
mujoco.mj_jacSite(self.model, self.data, None, jacr, site_id)
|
||||||
|
|
||||||
expected_jacr = np.array(
|
expected_jacr = np.array([
|
||||||
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
||||||
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
|
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
|
||||||
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
|
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
||||||
|
])
|
||||||
np.testing.assert_array_equal(jacr, expected_jacr)
|
np.testing.assert_array_equal(jacr, expected_jacr)
|
||||||
|
|
||||||
# Call mj_jacSite with both jacp and jacr.
|
# Call mj_jacSite with both jacp and jacr.
|
||||||
@@ -818,12 +858,14 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
# Check that the jacp argument must have the right size.
|
# Check that the jacp argument must have the right size.
|
||||||
with self.assertRaises(TypeError):
|
with self.assertRaises(TypeError):
|
||||||
mujoco.mj_jacSite(
|
mujoco.mj_jacSite(
|
||||||
self.model, self.data, np.empty((3, 6), jacp.dtype), None, site_id)
|
self.model, self.data, np.empty((3, 6), jacp.dtype), None, site_id
|
||||||
|
)
|
||||||
|
|
||||||
# Check that the jacr argument must have the right size.
|
# Check that the jacr argument must have the right size.
|
||||||
with self.assertRaises(TypeError):
|
with self.assertRaises(TypeError):
|
||||||
mujoco.mj_jacSite(
|
mujoco.mj_jacSite(
|
||||||
self.model, self.data, None, np.empty((4, 7), jacr.dtype), site_id)
|
self.model, self.data, None, np.empty((4, 7), jacr.dtype), site_id
|
||||||
|
)
|
||||||
|
|
||||||
# The following two checks need to be done with fully initialized arrays,
|
# The following two checks need to be done with fully initialized arrays,
|
||||||
# since pybind11 prints out the array's contents when generating TypeErrors.
|
# since pybind11 prints out the array's contents when generating TypeErrors.
|
||||||
@@ -832,12 +874,14 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
# Check that the jacp argument must have the right dtype.
|
# Check that the jacp argument must have the right dtype.
|
||||||
with self.assertRaises(TypeError):
|
with self.assertRaises(TypeError):
|
||||||
mujoco.mj_jacSite(
|
mujoco.mj_jacSite(
|
||||||
self.model, self.data, np.zeros(jacp.shape, int), None, site_id)
|
self.model, self.data, np.zeros(jacp.shape, int), None, site_id
|
||||||
|
)
|
||||||
|
|
||||||
# Check that the jacr argument must have the right dtype.
|
# Check that the jacr argument must have the right dtype.
|
||||||
with self.assertRaises(TypeError):
|
with self.assertRaises(TypeError):
|
||||||
mujoco.mj_jacSite(
|
mujoco.mj_jacSite(
|
||||||
self.model, self.data, None, np.zeros(jacr.shape, int), site_id)
|
self.model, self.data, None, np.zeros(jacr.shape, int), site_id
|
||||||
|
)
|
||||||
|
|
||||||
def test_docstrings(self): # pylint: disable=invalid-name
|
def test_docstrings(self): # pylint: disable=invalid-name
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
@@ -845,13 +889,15 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
|||||||
"""mj_versionString() -> str
|
"""mj_versionString() -> str
|
||||||
|
|
||||||
Return the current version of MuJoCo as a null-terminated string.
|
Return the current version of MuJoCo as a null-terminated string.
|
||||||
""")
|
""",
|
||||||
|
)
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mj_Euler.__doc__,
|
mujoco.mj_Euler.__doc__,
|
||||||
"""mj_Euler(m: mujoco._structs.MjModel, d: mujoco._structs.MjData) -> None
|
"""mj_Euler(m: mujoco._structs.MjModel, d: mujoco._structs.MjData) -> None
|
||||||
|
|
||||||
Euler integrator, semi-implicit in velocity.
|
Euler integrator, semi-implicit in velocity.
|
||||||
""")
|
""",
|
||||||
|
)
|
||||||
|
|
||||||
def test_float_constant(self):
|
def test_float_constant(self):
|
||||||
self.assertEqual(mujoco.mjMAXVAL, 1e10)
|
self.assertEqual(mujoco.mjMAXVAL, 1e10)
|
||||||
@@ -866,17 +912,19 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
self.assertLen(mujoco.mjVISSTRING, mujoco.mjtVisFlag.mjNVISFLAG)
|
self.assertLen(mujoco.mjVISSTRING, mujoco.mjtVisFlag.mjNVISFLAG)
|
||||||
self.assertLen(mujoco.mjRNDSTRING, mujoco.mjtRndFlag.mjNRNDFLAG)
|
self.assertLen(mujoco.mjRNDSTRING, mujoco.mjtRndFlag.mjNRNDFLAG)
|
||||||
self.assertEqual(mujoco.mjDISABLESTRING[11], 'Refsafe')
|
self.assertEqual(mujoco.mjDISABLESTRING[11], 'Refsafe')
|
||||||
self.assertEqual(mujoco.mjVISSTRING[mujoco.mjtVisFlag.mjVIS_INERTIA],
|
self.assertEqual(
|
||||||
('Inertia', '0', 'I'))
|
mujoco.mjVISSTRING[mujoco.mjtVisFlag.mjVIS_INERTIA],
|
||||||
|
('Inertia', '0', 'I'),
|
||||||
|
)
|
||||||
|
|
||||||
def test_enum_values(self):
|
def test_enum_values(self):
|
||||||
self.assertEqual(mujoco.mjtJoint.mjJNT_FREE, 0)
|
self.assertEqual(mujoco.mjtJoint.mjJNT_FREE, 0)
|
||||||
self.assertEqual(mujoco.mjtJoint.mjJNT_BALL, 1)
|
self.assertEqual(mujoco.mjtJoint.mjJNT_BALL, 1)
|
||||||
self.assertEqual(mujoco.mjtJoint.mjJNT_SLIDE, 2)
|
self.assertEqual(mujoco.mjtJoint.mjJNT_SLIDE, 2)
|
||||||
self.assertEqual(mujoco.mjtJoint.mjJNT_HINGE, 3)
|
self.assertEqual(mujoco.mjtJoint.mjJNT_HINGE, 3)
|
||||||
self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1<<0)
|
self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0)
|
||||||
self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1<<1)
|
self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1)
|
||||||
self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1<<2)
|
self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2)
|
||||||
self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 7)
|
self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 7)
|
||||||
self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0)
|
self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0)
|
||||||
self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1)
|
self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1)
|
||||||
@@ -899,8 +947,9 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
x = ['a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k']
|
x = ['a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k']
|
||||||
self.assertEqual(x[mujoco.mjtFrame.mjFRAME_WORLD], 'h')
|
self.assertEqual(x[mujoco.mjtFrame.mjFRAME_WORLD], 'h')
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
x[mujoco.mjtFrame.mjFRAME_GEOM:mujoco.mjtFrame.mjFRAME_CAMERA],
|
x[mujoco.mjtFrame.mjFRAME_GEOM : mujoco.mjtFrame.mjFRAME_CAMERA],
|
||||||
['c', 'd'])
|
['c', 'd'],
|
||||||
|
)
|
||||||
|
|
||||||
def test_enum_ops(self):
|
def test_enum_ops(self):
|
||||||
# Note: when modifying this test, make sure the enum value is an odd number
|
# Note: when modifying this test, make sure the enum value is an odd number
|
||||||
@@ -909,10 +958,12 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
self.assertEqual(mujoco.mjtFrame.mjFRAME_WORLD, 7.0)
|
self.assertEqual(mujoco.mjtFrame.mjFRAME_WORLD, 7.0)
|
||||||
self.assertEqual(7, mujoco.mjtFrame.mjFRAME_WORLD)
|
self.assertEqual(7, mujoco.mjtFrame.mjFRAME_WORLD)
|
||||||
self.assertEqual(7.0, mujoco.mjtFrame.mjFRAME_WORLD)
|
self.assertEqual(7.0, mujoco.mjtFrame.mjFRAME_WORLD)
|
||||||
self.assertEqual(mujoco.mjtFrame.mjFRAME_WORLD,
|
self.assertEqual(
|
||||||
mujoco.mjtFrame.mjFRAME_WORLD)
|
mujoco.mjtFrame.mjFRAME_WORLD, mujoco.mjtFrame.mjFRAME_WORLD
|
||||||
self.assertNotEqual(mujoco.mjtFrame.mjFRAME_WORLD,
|
)
|
||||||
mujoco.mjtFrame.mjFRAME_NONE)
|
self.assertNotEqual(
|
||||||
|
mujoco.mjtFrame.mjFRAME_WORLD, mujoco.mjtFrame.mjFRAME_NONE
|
||||||
|
)
|
||||||
|
|
||||||
self.assertEqual(-mujoco.mjtFrame.mjFRAME_WORLD, -7)
|
self.assertEqual(-mujoco.mjtFrame.mjFRAME_WORLD, -7)
|
||||||
self.assertIsInstance(-mujoco.mjtFrame.mjFRAME_WORLD, int)
|
self.assertIsInstance(-mujoco.mjtFrame.mjFRAME_WORLD, int)
|
||||||
@@ -989,22 +1040,28 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
|
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
mujoco.mjtDisableBit.mjDSBL_GRAVITY | mujoco.mjtDisableBit.mjDSBL_LIMIT,
|
mujoco.mjtDisableBit.mjDSBL_GRAVITY | mujoco.mjtDisableBit.mjDSBL_LIMIT,
|
||||||
72)
|
72,
|
||||||
|
)
|
||||||
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE | 33, 33)
|
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE | 33, 33)
|
||||||
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE & 33, 32)
|
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE & 33, 32)
|
||||||
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE ^ 33, 1)
|
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE ^ 33, 1)
|
||||||
self.assertEqual(33 | mujoco.mjtDisableBit.mjDSBL_PASSIVE, 33)
|
self.assertEqual(33 | mujoco.mjtDisableBit.mjDSBL_PASSIVE, 33)
|
||||||
self.assertEqual(33 & mujoco.mjtDisableBit.mjDSBL_PASSIVE, 32)
|
self.assertEqual(33 & mujoco.mjtDisableBit.mjDSBL_PASSIVE, 32)
|
||||||
self.assertEqual(33 ^ mujoco.mjtDisableBit.mjDSBL_PASSIVE, 1)
|
self.assertEqual(33 ^ mujoco.mjtDisableBit.mjDSBL_PASSIVE, 1)
|
||||||
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL << 1,
|
self.assertEqual(
|
||||||
mujoco.mjtDisableBit.mjDSBL_WARMSTART)
|
mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL << 1,
|
||||||
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL >> 3,
|
mujoco.mjtDisableBit.mjDSBL_WARMSTART,
|
||||||
mujoco.mjtDisableBit.mjDSBL_CONTACT)
|
)
|
||||||
|
self.assertEqual(
|
||||||
|
mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL >> 3,
|
||||||
|
mujoco.mjtDisableBit.mjDSBL_CONTACT,
|
||||||
|
)
|
||||||
|
|
||||||
def test_can_raise_error(self):
|
def test_can_raise_error(self):
|
||||||
self.data.pstack = self.data.narena
|
self.data.pstack = self.data.narena
|
||||||
with self.assertRaisesRegex(mujoco.FatalError,
|
with self.assertRaisesRegex(
|
||||||
r'\Amj_stackAlloc: insufficient memory:'):
|
mujoco.FatalError, r'\Amj_stackAlloc: insufficient memory:'
|
||||||
|
):
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
|
|
||||||
def test_mjcb_time(self):
|
def test_mjcb_time(self):
|
||||||
@@ -1042,7 +1099,8 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
with self.assertRaises(TestError) as e:
|
with self.assertRaises(TestError) as e:
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
e.exception.args, ('string', (1, 2, 3), {'a': 1, 'b': 2}))
|
e.exception.args, ('string', (1, 2, 3), {'a': 1, 'b': 2})
|
||||||
|
)
|
||||||
|
|
||||||
# Should not raise now that we've cleared the callback.
|
# Should not raise now that we've cleared the callback.
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
@@ -1050,12 +1108,14 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
def test_mjcb_time_wrong_return_type(self):
|
def test_mjcb_time_wrong_return_type(self):
|
||||||
with temporary_callback(mujoco.set_mjcb_time, lambda: 'string'):
|
with temporary_callback(mujoco.set_mjcb_time, lambda: 'string'):
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
TypeError, 'mjcb_time callback did not return a number'):
|
TypeError, 'mjcb_time callback did not return a number'
|
||||||
|
):
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
|
|
||||||
def test_mjcb_time_not_callable(self):
|
def test_mjcb_time_not_callable(self):
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
TypeError, 'callback is not an Optional[Callable]'):
|
TypeError, 'callback is not an Optional[Callable]'
|
||||||
|
):
|
||||||
mujoco.set_mjcb_time(1)
|
mujoco.set_mjcb_time(1)
|
||||||
|
|
||||||
def test_mjcb_sensor(self):
|
def test_mjcb_sensor(self):
|
||||||
@@ -1088,8 +1148,9 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
|
|
||||||
# This test is disabled on PyPy as it uses sys.getrefcount
|
# This test is disabled on PyPy as it uses sys.getrefcount
|
||||||
# However PyPy is not officially supported by MuJoCo
|
# However PyPy is not officially supported by MuJoCo
|
||||||
@absltest.skipIf(sys.implementation.name == 'pypy',
|
@absltest.skipIf(
|
||||||
reason='requires sys.getrefcount')
|
sys.implementation.name == 'pypy', reason='requires sys.getrefcount'
|
||||||
|
)
|
||||||
def test_mjcb_control_not_leak_memory(self):
|
def test_mjcb_control_not_leak_memory(self):
|
||||||
model_instances = []
|
model_instances = []
|
||||||
data_instances = []
|
data_instances = []
|
||||||
@@ -1110,8 +1171,9 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
|
|
||||||
# This test is disabled on PyPy as it uses sys.getrefcount
|
# This test is disabled on PyPy as it uses sys.getrefcount
|
||||||
# However PyPy is not officially supported by MuJoCo
|
# However PyPy is not officially supported by MuJoCo
|
||||||
@absltest.skipIf(sys.implementation.name == 'pypy',
|
@absltest.skipIf(
|
||||||
reason='requires sys.getrefcount')
|
sys.implementation.name == 'pypy', reason='requires sys.getrefcount'
|
||||||
|
)
|
||||||
def test_mjdata_holds_ref_to_model(self):
|
def test_mjdata_holds_ref_to_model(self):
|
||||||
data = mujoco.MjData(mujoco.MjModel.from_xml_string('<mujoco/>'))
|
data = mujoco.MjData(mujoco.MjModel.from_xml_string('<mujoco/>'))
|
||||||
model = data.model
|
model = data.model
|
||||||
@@ -1150,9 +1212,15 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
# When the scene is updated, geoms are added to the scene
|
# When the scene is updated, geoms are added to the scene
|
||||||
# (ngeom is incremented)
|
# (ngeom is incremented)
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
mujoco.mjv_updateScene(self.model, self.data, mujoco.MjvOption(),
|
mujoco.mjv_updateScene(
|
||||||
None, mujoco.MjvCamera(),
|
self.model,
|
||||||
mujoco.mjtCatBit.mjCAT_ALL, scene)
|
self.data,
|
||||||
|
mujoco.MjvOption(),
|
||||||
|
None,
|
||||||
|
mujoco.MjvCamera(),
|
||||||
|
mujoco.mjtCatBit.mjCAT_ALL,
|
||||||
|
scene,
|
||||||
|
)
|
||||||
self.assertGreater(scene.ngeom, 0)
|
self.assertGreater(scene.ngeom, 0)
|
||||||
|
|
||||||
def test_mjv_scene_without_model(self):
|
def test_mjv_scene_without_model(self):
|
||||||
@@ -1164,10 +1232,19 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
# mj_ray has tricky argument types
|
# mj_ray has tricky argument types
|
||||||
geomid = np.zeros(1, np.int32)
|
geomid = np.zeros(1, np.int32)
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
mujoco.mj_ray(self.model, self.data, [0, 0, 0], [0, 0, 1], None, 0, 0,
|
mujoco.mj_ray(
|
||||||
geomid)
|
self.model, self.data, [0, 0, 0], [0, 0, 1], None, 0, 0, geomid
|
||||||
mujoco.mj_ray(self.model, self.data, [0, 0, 0], [0, 0, 1],
|
)
|
||||||
[0, 0, 0, 0, 0, 0], 0, 0, geomid)
|
mujoco.mj_ray(
|
||||||
|
self.model,
|
||||||
|
self.data,
|
||||||
|
[0, 0, 0],
|
||||||
|
[0, 0, 1],
|
||||||
|
[0, 0, 0, 0, 0, 0],
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
geomid,
|
||||||
|
)
|
||||||
# Check that named arguments work
|
# Check that named arguments work
|
||||||
mujoco.mj_ray(
|
mujoco.mj_ray(
|
||||||
m=self.model,
|
m=self.model,
|
||||||
@@ -1177,7 +1254,8 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
geomgroup=None,
|
geomgroup=None,
|
||||||
flg_static=0,
|
flg_static=0,
|
||||||
bodyexclude=0,
|
bodyexclude=0,
|
||||||
geomid=geomid)
|
geomid=geomid,
|
||||||
|
)
|
||||||
|
|
||||||
def test_mj_multi_ray(self):
|
def test_mj_multi_ray(self):
|
||||||
nray = 3
|
nray = 3
|
||||||
@@ -1201,14 +1279,13 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
geomid=geomid,
|
geomid=geomid,
|
||||||
dist=dist,
|
dist=dist,
|
||||||
nray=nray,
|
nray=nray,
|
||||||
cutoff=mujoco.mjMAXVAL)
|
cutoff=mujoco.mjMAXVAL,
|
||||||
|
)
|
||||||
|
|
||||||
for i in range(0, 3):
|
for i in range(0, 3):
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
dist[i],
|
dist[i],
|
||||||
mujoco.mj_ray(
|
mujoco.mj_ray(self.model, self.data, pnt, vec[i], None, 1, -1, geom1),
|
||||||
self.model, self.data, pnt, vec[i], None, 1, -1, geom1
|
|
||||||
),
|
|
||||||
)
|
)
|
||||||
self.assertEqual(geomid[i], geom1)
|
self.assertEqual(geomid[i], geom1)
|
||||||
self.assertEqual(geomid[i], geom_ex[i])
|
self.assertEqual(geomid[i], geom_ex[i])
|
||||||
@@ -1217,16 +1294,28 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
def test_inverse_fd_none(self):
|
def test_inverse_fd_none(self):
|
||||||
eps = 1e-6
|
eps = 1e-6
|
||||||
flg_centered = 0
|
flg_centered = 0
|
||||||
mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered,
|
mujoco.mjd_inverseFD(
|
||||||
None, None, None, None, None, None, None)
|
self.model,
|
||||||
|
self.data,
|
||||||
|
eps,
|
||||||
|
flg_centered,
|
||||||
|
None,
|
||||||
|
None,
|
||||||
|
None,
|
||||||
|
None,
|
||||||
|
None,
|
||||||
|
None,
|
||||||
|
None,
|
||||||
|
)
|
||||||
|
|
||||||
def test_geom_distance(self):
|
def test_geom_distance(self):
|
||||||
mujoco.mj_forward(self.model, self.data)
|
mujoco.mj_forward(self.model, self.data)
|
||||||
fromto = np.empty(6, np.float64)
|
fromto = np.empty(6, np.float64)
|
||||||
dist = mujoco.mj_geomDistance(self.model, self.data, 0, 2, 200, fromto)
|
dist = mujoco.mj_geomDistance(self.model, self.data, 0, 2, 200, fromto)
|
||||||
self.assertEqual(dist, 41.9)
|
self.assertEqual(dist, 41.9)
|
||||||
np.testing.assert_array_equal(fromto,
|
np.testing.assert_array_equal(
|
||||||
np.array((42., 0., 0., 42., 0., 41.9)))
|
fromto, np.array((42.0, 0.0, 0.0, 42.0, 0.0, 41.9))
|
||||||
|
)
|
||||||
|
|
||||||
def test_inverse_fd(self):
|
def test_inverse_fd(self):
|
||||||
eps = 1e-6
|
eps = 1e-6
|
||||||
@@ -1238,8 +1327,19 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
ds_dv = np.zeros((self.model.nv, self.model.nsensordata))
|
ds_dv = np.zeros((self.model.nv, self.model.nsensordata))
|
||||||
ds_da = np.zeros((self.model.nv, self.model.nsensordata))
|
ds_da = np.zeros((self.model.nv, self.model.nsensordata))
|
||||||
dm_dq = np.zeros((self.model.nv, self.model.nM))
|
dm_dq = np.zeros((self.model.nv, self.model.nM))
|
||||||
mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered,
|
mujoco.mjd_inverseFD(
|
||||||
df_dq, df_dv, df_da, ds_dq, ds_dv, ds_da, dm_dq)
|
self.model,
|
||||||
|
self.data,
|
||||||
|
eps,
|
||||||
|
flg_centered,
|
||||||
|
df_dq,
|
||||||
|
df_dv,
|
||||||
|
df_da,
|
||||||
|
ds_dq,
|
||||||
|
ds_dv,
|
||||||
|
ds_da,
|
||||||
|
dm_dq,
|
||||||
|
)
|
||||||
self.assertGreater(np.linalg.norm(df_dq), eps)
|
self.assertGreater(np.linalg.norm(df_dq), eps)
|
||||||
self.assertGreater(np.linalg.norm(df_dv), eps)
|
self.assertGreater(np.linalg.norm(df_dv), eps)
|
||||||
self.assertGreater(np.linalg.norm(df_da), eps)
|
self.assertGreater(np.linalg.norm(df_da), eps)
|
||||||
@@ -1272,15 +1372,17 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
n_total = 4
|
n_total = 4
|
||||||
n_band = 1
|
n_band = 1
|
||||||
n_dense = 1
|
n_dense = 1
|
||||||
dense = np.array([[1.0, 0, 0, 0.1],
|
dense = np.array([
|
||||||
[0, 2.0, 0, 0.2],
|
[1.0, 0, 0, 0.1],
|
||||||
[0, 0, 3.0, 0.3],
|
[0, 2.0, 0, 0.2],
|
||||||
[0.1, 0.2, 0.3, 4.0]])
|
[0, 0, 3.0, 0.3],
|
||||||
band = np.zeros(n_band*(n_total-n_dense) + n_dense*n_total)
|
[0.1, 0.2, 0.3, 4.0],
|
||||||
|
])
|
||||||
|
band = np.zeros(n_band * (n_total - n_dense) + n_dense * n_total)
|
||||||
mujoco.mju_dense2Band(band, dense, n_total, n_band, n_dense)
|
mujoco.mju_dense2Band(band, dense, n_total, n_band, n_dense)
|
||||||
for i in range(4):
|
for i in range(4):
|
||||||
index = mujoco.mju_bandDiag(i, n_total, n_band, n_dense)
|
index = mujoco.mju_bandDiag(i, n_total, n_band, n_dense)
|
||||||
self.assertEqual(band[index], i+1)
|
self.assertEqual(band[index], i + 1)
|
||||||
dense2 = np.zeros((n_total, n_total))
|
dense2 = np.zeros((n_total, n_total))
|
||||||
flg_sym = 1
|
flg_sym = 1
|
||||||
mujoco.mju_band2Dense(dense2, band, n_total, n_band, n_dense, flg_sym)
|
mujoco.mju_band2Dense(dense2, band, n_total, n_band, n_dense, flg_sym)
|
||||||
@@ -1288,20 +1390,22 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
vec = np.array([[2.0], [2.0], [3.0], [4.0]])
|
vec = np.array([[2.0], [2.0], [3.0], [4.0]])
|
||||||
res = np.zeros_like(vec)
|
res = np.zeros_like(vec)
|
||||||
n_vec = 1
|
n_vec = 1
|
||||||
mujoco.mju_bandMulMatVec(res, band, vec,
|
mujoco.mju_bandMulMatVec(
|
||||||
n_total, n_band, n_dense, n_vec, flg_sym)
|
res, band, vec, n_total, n_band, n_dense, n_vec, flg_sym
|
||||||
|
)
|
||||||
np.testing.assert_array_equal(res, dense @ vec)
|
np.testing.assert_array_equal(res, dense @ vec)
|
||||||
diag_add = 0
|
diag_add = 0
|
||||||
diag_mul = 0
|
diag_mul = 0
|
||||||
mujoco.mju_cholFactorBand(band, n_total, n_band, n_dense,
|
mujoco.mju_cholFactorBand(
|
||||||
diag_add, diag_mul)
|
band, n_total, n_band, n_dense, diag_add, diag_mul
|
||||||
|
)
|
||||||
mujoco.mju_cholSolveBand(res, band, vec, n_total, n_band, n_dense)
|
mujoco.mju_cholSolveBand(res, band, vec, n_total, n_band, n_dense)
|
||||||
np.testing.assert_almost_equal(res, np.linalg.solve(dense, vec))
|
np.testing.assert_almost_equal(res, np.linalg.solve(dense, vec))
|
||||||
|
|
||||||
def test_mju_box_qp(self):
|
def test_mju_box_qp(self):
|
||||||
n = 5
|
n = 5
|
||||||
res = np.zeros(n)
|
res = np.zeros(n)
|
||||||
r = np.zeros((n, n+7))
|
r = np.zeros((n, n + 7))
|
||||||
index = np.zeros(n, np.int32)
|
index = np.zeros(n, np.int32)
|
||||||
h = np.eye(n)
|
h = np.eye(n)
|
||||||
g = np.ones((n,))
|
g = np.ones((n,))
|
||||||
@@ -1324,7 +1428,7 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
mat = np.linspace(0, 1, 16).reshape(4, 4)
|
mat = np.linspace(0, 1, 16).reshape(4, 4)
|
||||||
res = np.empty((4, 4), np.float64)
|
res = np.empty((4, 4), np.float64)
|
||||||
mujoco.mju_symmetrize(res, mat)
|
mujoco.mju_symmetrize(res, mat)
|
||||||
np.testing.assert_array_equal(res, 0.5*(mat + mat.T))
|
np.testing.assert_array_equal(res, 0.5 * (mat + mat.T))
|
||||||
|
|
||||||
def test_mju_clip(self):
|
def test_mju_clip(self):
|
||||||
self.assertEqual(mujoco.mju_clip(1.5, 1.0, 2.0), 1.5)
|
self.assertEqual(mujoco.mju_clip(1.5, 1.0, 2.0), 1.5)
|
||||||
@@ -1332,14 +1436,14 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
self.assertEqual(mujoco.mju_clip(1.5, 0.0, 1.0), 1.0)
|
self.assertEqual(mujoco.mju_clip(1.5, 0.0, 1.0), 1.0)
|
||||||
|
|
||||||
def test_mju_mul_vec_mat_vec(self):
|
def test_mju_mul_vec_mat_vec(self):
|
||||||
vec1 = np.array([1., 2., 3.])
|
vec1 = np.array([1.0, 2.0, 3.0])
|
||||||
vec2 = np.array([3., 2., 1.])
|
vec2 = np.array([3.0, 2.0, 1.0])
|
||||||
mat = np.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.]])
|
mat = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]])
|
||||||
self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.)
|
self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.0)
|
||||||
|
|
||||||
def test_mju_dense_to_sparse(self):
|
def test_mju_dense_to_sparse(self):
|
||||||
mat = np.array([[0., 1., 0.], [2., 0., 3.]])
|
mat = np.array([[0.0, 1.0, 0.0], [2.0, 0.0, 3.0]])
|
||||||
expected_vals = np.array([1., 2., 3.])
|
expected_vals = np.array([1.0, 2.0, 3.0])
|
||||||
expected_rownnz = np.array([1, 2])
|
expected_rownnz = np.array([1, 2])
|
||||||
expected_rowadr = np.array([0, 1])
|
expected_rowadr = np.array([0, 1])
|
||||||
expected_colind = np.array([1, 0, 2])
|
expected_colind = np.array([1, 0, 2])
|
||||||
@@ -1355,8 +1459,8 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
np.testing.assert_array_equal(col_ind, expected_colind)
|
np.testing.assert_array_equal(col_ind, expected_colind)
|
||||||
|
|
||||||
def test_mju_sparse_to_dense(self):
|
def test_mju_sparse_to_dense(self):
|
||||||
expected = np.array([[0., 1., 0.], [2., 0., 3.]])
|
expected = np.array([[0.0, 1.0, 0.0], [2.0, 0.0, 3.0]])
|
||||||
mat = np.array((1., 2., 3.))
|
mat = np.array((1.0, 2.0, 3.0))
|
||||||
rownnz = np.array([1, 2])
|
rownnz = np.array([1, 2])
|
||||||
rowadr = np.array([0, 1])
|
rowadr = np.array([0, 1])
|
||||||
colind = np.array([1, 0, 2])
|
colind = np.array([1, 0, 2])
|
||||||
@@ -1366,10 +1470,10 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
|
|
||||||
def test_mju_euler_to_quat(self):
|
def test_mju_euler_to_quat(self):
|
||||||
quat = np.zeros(4)
|
quat = np.zeros(4)
|
||||||
euler = np.array([0, np.pi/2, 0])
|
euler = np.array([0, np.pi / 2, 0])
|
||||||
seq = 'xyz'
|
seq = 'xyz'
|
||||||
mujoco.mju_euler2Quat(quat, euler, seq)
|
mujoco.mju_euler2Quat(quat, euler, seq)
|
||||||
expected_quat = np.array([np.sqrt(0.5), 0, np.sqrt(0.5), 0.])
|
expected_quat = np.array([np.sqrt(0.5), 0, np.sqrt(0.5), 0.0])
|
||||||
np.testing.assert_almost_equal(quat, expected_quat)
|
np.testing.assert_almost_equal(quat, expected_quat)
|
||||||
|
|
||||||
error = 'mju_euler2Quat: seq must contain exactly 3 characters'
|
error = 'mju_euler2Quat: seq must contain exactly 3 characters'
|
||||||
@@ -1377,7 +1481,7 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
mujoco.mju_euler2Quat(quat, euler, 'xy')
|
mujoco.mju_euler2Quat(quat, euler, 'xy')
|
||||||
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error):
|
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error):
|
||||||
mujoco.mju_euler2Quat(quat, euler, 'xyzy')
|
mujoco.mju_euler2Quat(quat, euler, 'xyzy')
|
||||||
error = 'mju_euler2Quat: seq[2] is \'p\', should be one of x, y, z, X, Y, Z'
|
error = "mju_euler2Quat: seq[2] is 'p', should be one of x, y, z, X, Y, Z"
|
||||||
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error):
|
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error):
|
||||||
mujoco.mju_euler2Quat(quat, euler, 'xYp')
|
mujoco.mju_euler2Quat(quat, euler, 'xYp')
|
||||||
|
|
||||||
@@ -1396,8 +1500,16 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
mujoco.mj_step(self.model, self.data)
|
mujoco.mj_step(self.model, self.data)
|
||||||
data2 = pickle.loads(pickle.dumps(self.data))
|
data2 = pickle.loads(pickle.dumps(self.data))
|
||||||
attr_to_compare = (
|
attr_to_compare = (
|
||||||
'time', 'qpos', 'qvel', 'qacc', 'xpos', 'mocap_pos',
|
'time',
|
||||||
'warning', 'energy', 'contact', 'efc_J'
|
'qpos',
|
||||||
|
'qvel',
|
||||||
|
'qacc',
|
||||||
|
'xpos',
|
||||||
|
'mocap_pos',
|
||||||
|
'warning',
|
||||||
|
'energy',
|
||||||
|
'contact',
|
||||||
|
'efc_J',
|
||||||
)
|
)
|
||||||
self._assert_attributes_equal(data2, self.data, attr_to_compare)
|
self._assert_attributes_equal(data2, self.data, attr_to_compare)
|
||||||
for _ in range(10):
|
for _ in range(10):
|
||||||
@@ -1410,8 +1522,16 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
mujoco.mj_step(self.model, self.data)
|
mujoco.mj_step(self.model, self.data)
|
||||||
data2 = pickle.loads(pickle.dumps(self.data))
|
data2 = pickle.loads(pickle.dumps(self.data))
|
||||||
attr_to_compare = (
|
attr_to_compare = (
|
||||||
'time', 'qpos', 'qvel', 'qacc', 'xpos', 'mocap_pos',
|
'time',
|
||||||
'warning', 'energy', 'contact', 'efc_J'
|
'qpos',
|
||||||
|
'qvel',
|
||||||
|
'qacc',
|
||||||
|
'xpos',
|
||||||
|
'mocap_pos',
|
||||||
|
'warning',
|
||||||
|
'energy',
|
||||||
|
'contact',
|
||||||
|
'efc_J',
|
||||||
)
|
)
|
||||||
self._assert_attributes_equal(data2, self.data, attr_to_compare)
|
self._assert_attributes_equal(data2, self.data, attr_to_compare)
|
||||||
for _ in range(10):
|
for _ in range(10):
|
||||||
@@ -1422,7 +1542,10 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
def test_pickle_mjmodel(self):
|
def test_pickle_mjmodel(self):
|
||||||
model2 = pickle.loads(pickle.dumps(self.model))
|
model2 = pickle.loads(pickle.dumps(self.model))
|
||||||
attr_to_compare = (
|
attr_to_compare = (
|
||||||
'nq', 'nmat', 'body_pos', 'names',
|
'nq',
|
||||||
|
'nmat',
|
||||||
|
'body_pos',
|
||||||
|
'names',
|
||||||
)
|
)
|
||||||
self._assert_attributes_equal(model2, self.model, attr_to_compare)
|
self._assert_attributes_equal(model2, self.model, attr_to_compare)
|
||||||
|
|
||||||
@@ -1506,8 +1629,11 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
else:
|
else:
|
||||||
self.assertEqual(actual_value, expected_value)
|
self.assertEqual(actual_value, expected_value)
|
||||||
except AssertionError as e:
|
except AssertionError as e:
|
||||||
self.fail("Attribute '{}' differs from expected value: {}".format(
|
self.fail(
|
||||||
name, str(e)))
|
"Attribute '{}' differs from expected value: {}".format(
|
||||||
|
name, str(e)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
|
|||||||
@@ -57,6 +57,7 @@ class MemoryLeakTest(absltest.TestCase):
|
|||||||
soft = -1
|
soft = -1
|
||||||
try:
|
try:
|
||||||
import resource # pylint: disable=g-import-not-at-top
|
import resource # pylint: disable=g-import-not-at-top
|
||||||
|
|
||||||
soft, hard = resource.getrlimit(resource.RLIMIT_AS)
|
soft, hard = resource.getrlimit(resource.RLIMIT_AS)
|
||||||
resource.setrlimit(resource.RLIMIT_AS, (limit_in_bytes, hard))
|
resource.setrlimit(resource.RLIMIT_AS, (limit_in_bytes, hard))
|
||||||
except (ImportError, ValueError):
|
except (ImportError, ValueError):
|
||||||
@@ -65,5 +66,5 @@ class MemoryLeakTest(absltest.TestCase):
|
|||||||
return soft
|
return soft
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == "__main__":
|
||||||
absltest.main()
|
absltest.main()
|
||||||
|
|||||||
+18
-12
@@ -209,7 +209,7 @@ def least_squares(
|
|||||||
|
|
||||||
# Decrease mu agressively: sequential decreases grow exponentially.
|
# Decrease mu agressively: sequential decreases grow exponentially.
|
||||||
def decrease_mu(mu, n_reduc):
|
def decrease_mu(mu, n_reduc):
|
||||||
dmu = (1/mu_factor) ** (2**n_reduc)
|
dmu = (1 / mu_factor) ** (2**n_reduc)
|
||||||
mu = 0.0 if mu * dmu < mu_min else mu * dmu
|
mu = 0.0 if mu * dmu < mu_min else mu * dmu
|
||||||
n_reduc += 1
|
n_reduc += 1
|
||||||
return mu, n_reduc
|
return mu, n_reduc
|
||||||
@@ -427,7 +427,6 @@ def jacobian_fd(
|
|||||||
Returns:
|
Returns:
|
||||||
jac: Jacobian of the residual at x.
|
jac: Jacobian of the residual at x.
|
||||||
n_res: updated number of residual evaluations (add x.size).
|
n_res: updated number of residual evaluations (add x.size).
|
||||||
|
|
||||||
"""
|
"""
|
||||||
n = x.size
|
n = x.size
|
||||||
if bounds is None:
|
if bounds is None:
|
||||||
@@ -438,7 +437,7 @@ def jacobian_fd(
|
|||||||
xh = x + np.diag(eps_vec)
|
xh = x + np.diag(eps_vec)
|
||||||
rh = residual(xh)
|
rh = residual(xh)
|
||||||
jac = (rh - r) / eps_vec
|
jac = (rh - r) / eps_vec
|
||||||
return jac, n_res+n
|
return jac, n_res + n
|
||||||
|
|
||||||
|
|
||||||
def check_jacobian(
|
def check_jacobian(
|
||||||
@@ -467,14 +466,15 @@ def check_jacobian(
|
|||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
n_res: updated number of residual evaluations.
|
n_res: updated number of residual evaluations.
|
||||||
|
|
||||||
"""
|
"""
|
||||||
jac_fd, n_res = jacobian_fd(residual, x, r, eps, n_res, bounds)
|
jac_fd, n_res = jacobian_fd(residual, x, r, eps, n_res, bounds)
|
||||||
denom = np.abs(jac).sum() + np.abs(jac_fd).sum() + 1e-8
|
denom = np.abs(jac).sum() + np.abs(jac_fd).sum() + 1e-8
|
||||||
rel_diff = np.abs(jac - jac_fd) / denom
|
rel_diff = np.abs(jac - jac_fd) / denom
|
||||||
if np.any(rel_diff > 1e-5):
|
if np.any(rel_diff > 1e-5):
|
||||||
raise ValueError(f'User-provided {name} does not match finite-differences '
|
raise ValueError(
|
||||||
'to a relative tolerance of 1e-5.')
|
f'User-provided {name} does not match finite-differences '
|
||||||
|
'to a relative tolerance of 1e-5.'
|
||||||
|
)
|
||||||
print(f'User-provided {name} matches finite-differences.', file=output)
|
print(f'User-provided {name} matches finite-differences.', file=output)
|
||||||
return n_res
|
return n_res
|
||||||
|
|
||||||
@@ -489,8 +489,8 @@ def check_norm(
|
|||||||
|
|
||||||
Args:
|
Args:
|
||||||
r: residual vector.
|
r: residual vector.
|
||||||
norm: Norm function returning either the norm scalar or its gradient
|
norm: Norm function returning either the norm scalar or its gradient and
|
||||||
and Gauss-Newton Hessian.
|
Gauss-Newton Hessian.
|
||||||
eps: finite-difference step size.
|
eps: finite-difference step size.
|
||||||
output: Optional file or StringIO to which to print messages.
|
output: Optional file or StringIO to which to print messages.
|
||||||
"""
|
"""
|
||||||
@@ -506,12 +506,16 @@ def check_norm(
|
|||||||
# Check that Hessian is positive-definite.
|
# Check that Hessian is positive-definite.
|
||||||
if np.any(np.linalg.eigvals(n_h) < 0):
|
if np.any(np.linalg.eigvals(n_h) < 0):
|
||||||
h_min = np.min(np.linalg.eigvals(n_h))
|
h_min = np.min(np.linalg.eigvals(n_h))
|
||||||
raise ValueError('User-provided norm Hessian is not positive definite. '
|
raise ValueError(
|
||||||
f'Minimum eigenvalue is {h_min:<.4g}')
|
'User-provided norm Hessian is not positive definite. '
|
||||||
|
f'Minimum eigenvalue is {h_min:<.4g}'
|
||||||
|
)
|
||||||
|
|
||||||
# Local function returning norm values (vectorized).
|
# Local function returning norm values (vectorized).
|
||||||
def norm_vec(v):
|
def norm_vec(v):
|
||||||
norms = [np.atleast_2d(norm.value(v[:, i:i+1])) for i in range(v.shape[1])]
|
norms = [
|
||||||
|
np.atleast_2d(norm.value(v[:, i : i + 1])) for i in range(v.shape[1])
|
||||||
|
]
|
||||||
return np.hstack(norms)
|
return np.hstack(norms)
|
||||||
|
|
||||||
# Check the norm gradient.
|
# Check the norm gradient.
|
||||||
@@ -519,7 +523,9 @@ def check_norm(
|
|||||||
|
|
||||||
# Local function returning norm gradients (vectorized).
|
# Local function returning norm gradients (vectorized).
|
||||||
def grad_vec(v):
|
def grad_vec(v):
|
||||||
gradients = [norm.grad_hess(v[:, i:i+1], eye)[0] for i in range(v.shape[1])]
|
gradients = [
|
||||||
|
norm.grad_hess(v[:, i : i + 1], eye)[0] for i in range(v.shape[1])
|
||||||
|
]
|
||||||
return np.hstack(gradients)
|
return np.hstack(gradients)
|
||||||
|
|
||||||
# Check the norm Hessian.
|
# Check the norm Hessian.
|
||||||
|
|||||||
@@ -56,8 +56,9 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
|
|
||||||
x0 = np.array((0.0, 0.0))
|
x0 = np.array((0.0, 0.0))
|
||||||
out = io.StringIO()
|
out = io.StringIO()
|
||||||
x, _ = minimize.least_squares(x0, residual, jacobian=jacobian, output=out,
|
x, _ = minimize.least_squares(
|
||||||
check_derivatives=True)
|
x0, residual, jacobian=jacobian, output=out, check_derivatives=True
|
||||||
|
)
|
||||||
expected_x = np.array((1.0, 1.0))
|
expected_x = np.array((1.0, 1.0))
|
||||||
np.testing.assert_array_almost_equal(x, expected_x)
|
np.testing.assert_array_almost_equal(x, expected_x)
|
||||||
self.assertIn('norm(dx) < tol', out.getvalue())
|
self.assertIn('norm(dx) < tol', out.getvalue())
|
||||||
@@ -67,9 +68,15 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
def bad_jacobian(x, r):
|
def bad_jacobian(x, r):
|
||||||
del r # Unused.
|
del r # Unused.
|
||||||
return np.array([[-1, 0], [-20 * x[0, 0], 15]])
|
return np.array([[-1, 0], [-20 * x[0, 0], 15]])
|
||||||
|
|
||||||
with self.assertRaisesRegex(ValueError, r'\bJacobian does not match\b'):
|
with self.assertRaisesRegex(ValueError, r'\bJacobian does not match\b'):
|
||||||
minimize.least_squares(x0, residual, jacobian=bad_jacobian, output=out,
|
minimize.least_squares(
|
||||||
check_derivatives=True)
|
x0,
|
||||||
|
residual,
|
||||||
|
jacobian=bad_jacobian,
|
||||||
|
output=out,
|
||||||
|
check_derivatives=True,
|
||||||
|
)
|
||||||
|
|
||||||
def test_max_iter(self) -> None:
|
def test_max_iter(self) -> None:
|
||||||
dim = 20 # High-D Rosenbrock
|
dim = 20 # High-D Rosenbrock
|
||||||
@@ -98,13 +105,16 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
x0 = np.array((0.0, 0.0))
|
x0 = np.array((0.0, 0.0))
|
||||||
expected_x = np.array((1.0, 1.0))
|
expected_x = np.array((1.0, 1.0))
|
||||||
|
|
||||||
bounds_types = {'inbounds': [np.array((-2.0, -2.0)), np.array((2.0, 2.0))],
|
bounds_types = {
|
||||||
'onlower': [np.array((-2.0, 2.0)), np.array((0.5, 3.0))],
|
'inbounds': [np.array((-2.0, -2.0)), np.array((2.0, 2.0))],
|
||||||
'onupper': [np.array((-2.0, -2.0)), np.array((0.5, 2.0))]}
|
'onlower': [np.array((-2.0, 2.0)), np.array((0.5, 3.0))],
|
||||||
|
'onupper': [np.array((-2.0, -2.0)), np.array((0.5, 2.0))],
|
||||||
|
}
|
||||||
|
|
||||||
# In bounds finds true minimum.
|
# In bounds finds true minimum.
|
||||||
x, _ = minimize.least_squares(x0, residual, bounds=bounds_types['inbounds'],
|
x, _ = minimize.least_squares(
|
||||||
output=out)
|
x0, residual, bounds=bounds_types['inbounds'], output=out
|
||||||
|
)
|
||||||
np.testing.assert_array_almost_equal(x, expected_x)
|
np.testing.assert_array_almost_equal(x, expected_x)
|
||||||
self.assertIn('norm(dx) < tol', out.getvalue())
|
self.assertIn('norm(dx) < tol', out.getvalue())
|
||||||
|
|
||||||
@@ -157,8 +167,9 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
print(f'Hello iteration {len(trace)}!', file=out)
|
print(f'Hello iteration {len(trace)}!', file=out)
|
||||||
|
|
||||||
x0 = np.array((0.0, 0.0))
|
x0 = np.array((0.0, 0.0))
|
||||||
x, _ = minimize.least_squares(x0, residual, output=out,
|
x, _ = minimize.least_squares(
|
||||||
iter_callback=iter_callback)
|
x0, residual, output=out, iter_callback=iter_callback
|
||||||
|
)
|
||||||
expected_x = np.array((1.0, 1.0))
|
expected_x = np.array((1.0, 1.0))
|
||||||
np.testing.assert_array_almost_equal(x, expected_x)
|
np.testing.assert_array_almost_equal(x, expected_x)
|
||||||
self.assertIn('Hello iteration 3!', out.getvalue())
|
self.assertIn('Hello iteration 3!', out.getvalue())
|
||||||
@@ -170,11 +181,12 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
p = 0.01 # Smoothing radius for smooth-L2 norm.
|
p = 0.01 # Smoothing radius for smooth-L2 norm.
|
||||||
|
|
||||||
class SmoothL2(minimize.Norm):
|
class SmoothL2(minimize.Norm):
|
||||||
|
|
||||||
def value(self, r):
|
def value(self, r):
|
||||||
return np.sqrt((r.T @ r).item() + p*p) - p
|
return np.sqrt((r.T @ r).item() + p * p) - p
|
||||||
|
|
||||||
def grad_hess(self, r, proj):
|
def grad_hess(self, r, proj):
|
||||||
s = np.sqrt((r.T @ r).item() + p*p)
|
s = np.sqrt((r.T @ r).item() + p * p)
|
||||||
y_r = r / s
|
y_r = r / s
|
||||||
grad = proj.T @ y_r
|
grad = proj.T @ y_r
|
||||||
y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
|
y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
|
||||||
@@ -183,8 +195,9 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
|
|
||||||
out = io.StringIO()
|
out = io.StringIO()
|
||||||
x0 = np.array((0.0, 0.0))
|
x0 = np.array((0.0, 0.0))
|
||||||
x, _ = minimize.least_squares(x0, residual, norm=SmoothL2(), output=out,
|
x, _ = minimize.least_squares(
|
||||||
check_derivatives=True)
|
x0, residual, norm=SmoothL2(), output=out, check_derivatives=True
|
||||||
|
)
|
||||||
expected_x = np.array((1.0, 1.0))
|
expected_x = np.array((1.0, 1.0))
|
||||||
np.testing.assert_array_almost_equal(x, expected_x)
|
np.testing.assert_array_almost_equal(x, expected_x)
|
||||||
self.assertIn('norm(dx) < tol', out.getvalue())
|
self.assertIn('norm(dx) < tol', out.getvalue())
|
||||||
@@ -192,11 +205,12 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
self.assertIn('User-provided norm Hessian matches', out.getvalue())
|
self.assertIn('User-provided norm Hessian matches', out.getvalue())
|
||||||
|
|
||||||
class SmoothL2BadGrad(minimize.Norm):
|
class SmoothL2BadGrad(minimize.Norm):
|
||||||
|
|
||||||
def value(self, r):
|
def value(self, r):
|
||||||
return np.sqrt((r.T @ r).item() + p*p) - p
|
return np.sqrt((r.T @ r).item() + p * p) - p
|
||||||
|
|
||||||
def grad_hess(self, r, proj):
|
def grad_hess(self, r, proj):
|
||||||
s = np.sqrt((r.T @ r).item() + p*p)
|
s = np.sqrt((r.T @ r).item() + p * p)
|
||||||
y_r = r / s
|
y_r = r / s
|
||||||
grad = proj.T @ (y_r + 0.001) # 0.001 is erronous.
|
grad = proj.T @ (y_r + 0.001) # 0.001 is erronous.
|
||||||
y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
|
y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
|
||||||
@@ -204,15 +218,21 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
return grad, hess
|
return grad, hess
|
||||||
|
|
||||||
with self.assertRaisesRegex(ValueError, r'\bgradient does not match\b'):
|
with self.assertRaisesRegex(ValueError, r'\bgradient does not match\b'):
|
||||||
minimize.least_squares(x0, residual, norm=SmoothL2BadGrad(), output=out,
|
minimize.least_squares(
|
||||||
check_derivatives=True)
|
x0,
|
||||||
|
residual,
|
||||||
|
norm=SmoothL2BadGrad(),
|
||||||
|
output=out,
|
||||||
|
check_derivatives=True,
|
||||||
|
)
|
||||||
|
|
||||||
class SmoothL2BadHess(minimize.Norm):
|
class SmoothL2BadHess(minimize.Norm):
|
||||||
|
|
||||||
def value(self, r):
|
def value(self, r):
|
||||||
return np.sqrt((r.T @ r).item() + p*p) - p
|
return np.sqrt((r.T @ r).item() + p * p) - p
|
||||||
|
|
||||||
def grad_hess(self, r, proj):
|
def grad_hess(self, r, proj):
|
||||||
s = np.sqrt((r.T @ r).item() + p*p)
|
s = np.sqrt((r.T @ r).item() + p * p)
|
||||||
y_r = r / s
|
y_r = r / s
|
||||||
grad = proj.T @ y_r
|
grad = proj.T @ y_r
|
||||||
y_rr = (1.001 * np.eye(r.size) - y_r @ y_r.T) / s # 1.001 is erronous.
|
y_rr = (1.001 * np.eye(r.size) - y_r @ y_r.T) / s # 1.001 is erronous.
|
||||||
@@ -220,15 +240,21 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
return grad, hess
|
return grad, hess
|
||||||
|
|
||||||
with self.assertRaisesRegex(ValueError, r'\bHessian does not match\b'):
|
with self.assertRaisesRegex(ValueError, r'\bHessian does not match\b'):
|
||||||
minimize.least_squares(x0, residual, norm=SmoothL2BadHess(), output=out,
|
minimize.least_squares(
|
||||||
check_derivatives=True)
|
x0,
|
||||||
|
residual,
|
||||||
|
norm=SmoothL2BadHess(),
|
||||||
|
output=out,
|
||||||
|
check_derivatives=True,
|
||||||
|
)
|
||||||
|
|
||||||
class SmoothL2AsymHess(minimize.Norm):
|
class SmoothL2AsymHess(minimize.Norm):
|
||||||
|
|
||||||
def value(self, r):
|
def value(self, r):
|
||||||
return np.sqrt((r.T @ r).item() + p*p) - p
|
return np.sqrt((r.T @ r).item() + p * p) - p
|
||||||
|
|
||||||
def grad_hess(self, r, proj):
|
def grad_hess(self, r, proj):
|
||||||
s = np.sqrt((r.T @ r).item() + p*p)
|
s = np.sqrt((r.T @ r).item() + p * p)
|
||||||
y_r = r / s
|
y_r = r / s
|
||||||
grad = proj.T @ y_r
|
grad = proj.T @ y_r
|
||||||
y_rr = (np.eye(r.size) - (y_r + 0.0001) @ y_r.T) / s
|
y_rr = (np.eye(r.size) - (y_r + 0.0001) @ y_r.T) / s
|
||||||
@@ -236,15 +262,21 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
return grad, hess
|
return grad, hess
|
||||||
|
|
||||||
with self.assertRaisesRegex(ValueError, r'\bnot symmetric\b'):
|
with self.assertRaisesRegex(ValueError, r'\bnot symmetric\b'):
|
||||||
minimize.least_squares(x0, residual, norm=SmoothL2AsymHess(), output=out,
|
minimize.least_squares(
|
||||||
check_derivatives=True)
|
x0,
|
||||||
|
residual,
|
||||||
|
norm=SmoothL2AsymHess(),
|
||||||
|
output=out,
|
||||||
|
check_derivatives=True,
|
||||||
|
)
|
||||||
|
|
||||||
class SmoothL2NegHess(minimize.Norm):
|
class SmoothL2NegHess(minimize.Norm):
|
||||||
|
|
||||||
def value(self, r):
|
def value(self, r):
|
||||||
return np.sqrt((r.T @ r).item() + p*p) - p
|
return np.sqrt((r.T @ r).item() + p * p) - p
|
||||||
|
|
||||||
def grad_hess(self, r, proj):
|
def grad_hess(self, r, proj):
|
||||||
s = np.sqrt((r.T @ r).item() + p*p)
|
s = np.sqrt((r.T @ r).item() + p * p)
|
||||||
y_r = r / s
|
y_r = r / s
|
||||||
grad = proj.T @ y_r
|
grad = proj.T @ y_r
|
||||||
y_rr = -(np.eye(r.size) - y_r @ y_r.T) / s # Negative-definite.
|
y_rr = -(np.eye(r.size) - y_r @ y_r.T) / s # Negative-definite.
|
||||||
@@ -252,7 +284,14 @@ class MinimizeTest(absltest.TestCase):
|
|||||||
return grad, hess
|
return grad, hess
|
||||||
|
|
||||||
with self.assertRaisesRegex(ValueError, r'\bnot positive definite\b'):
|
with self.assertRaisesRegex(ValueError, r'\bnot positive definite\b'):
|
||||||
minimize.least_squares(x0, residual, norm=SmoothL2NegHess(), output=out,
|
minimize.least_squares(
|
||||||
check_derivatives=True)
|
x0,
|
||||||
|
residual,
|
||||||
|
norm=SmoothL2NegHess(),
|
||||||
|
output=out,
|
||||||
|
check_derivatives=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
absltest.main()
|
absltest.main()
|
||||||
|
|||||||
@@ -63,7 +63,8 @@ class MshTest(absltest.TestCase):
|
|||||||
obj = msh2obj.msh_to_obj(msh_path)
|
obj = msh2obj.msh_to_obj(msh_path)
|
||||||
|
|
||||||
obj_model = mujoco.MjModel.from_xml_string(
|
obj_model = mujoco.MjModel.from_xml_string(
|
||||||
_XML, {"abdomen_1_body.obj": obj.encode()})
|
_XML, {"abdomen_1_body.obj": obj.encode()}
|
||||||
|
)
|
||||||
|
|
||||||
for field in _MESH_FIELDS:
|
for field in _MESH_FIELDS:
|
||||||
np.testing.assert_allclose(
|
np.testing.assert_allclose(
|
||||||
@@ -73,5 +74,6 @@ class MshTest(absltest.TestCase):
|
|||||||
err_msg=f"Field {field} does not match between msh and obj models.",
|
err_msg=f"Field {field} does not match between msh and obj models.",
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
absltest.main()
|
absltest.main()
|
||||||
|
|||||||
@@ -19,8 +19,9 @@ import mujoco
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
@absltest.skipUnless(hasattr(mujoco, 'GLContext'),
|
@absltest.skipUnless(
|
||||||
'MuJoCo rendering is disabled')
|
hasattr(mujoco, 'GLContext'), 'MuJoCo rendering is disabled'
|
||||||
|
)
|
||||||
class MuJoCoRenderTest(absltest.TestCase):
|
class MuJoCoRenderTest(absltest.TestCase):
|
||||||
|
|
||||||
def setUp(self):
|
def setUp(self):
|
||||||
@@ -48,8 +49,14 @@ class MuJoCoRenderTest(absltest.TestCase):
|
|||||||
|
|
||||||
scene = mujoco.MjvScene(self.model, maxgeom=0)
|
scene = mujoco.MjvScene(self.model, maxgeom=0)
|
||||||
mujoco.mjv_updateScene(
|
mujoco.mjv_updateScene(
|
||||||
self.model, self.data, mujoco.MjvOption(), mujoco.MjvPerturb(),
|
self.model,
|
||||||
mujoco.MjvCamera(), mujoco.mjtCatBit.mjCAT_ALL, scene)
|
self.data,
|
||||||
|
mujoco.MjvOption(),
|
||||||
|
mujoco.MjvPerturb(),
|
||||||
|
mujoco.MjvCamera(),
|
||||||
|
mujoco.mjtCatBit.mjCAT_ALL,
|
||||||
|
scene,
|
||||||
|
)
|
||||||
|
|
||||||
context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150)
|
context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150)
|
||||||
mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, context)
|
mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, context)
|
||||||
@@ -62,7 +69,7 @@ class MuJoCoRenderTest(absltest.TestCase):
|
|||||||
mujoco.mjr_rectangle(blue_rect, 0, 0, 1, 1)
|
mujoco.mjr_rectangle(blue_rect, 0, 0, 1, 1)
|
||||||
|
|
||||||
expected_upside_down_image = np.zeros((480, 640, 3), dtype=np.uint8)
|
expected_upside_down_image = np.zeros((480, 640, 3), dtype=np.uint8)
|
||||||
expected_upside_down_image[67:67+123, 56:56+234, 2] = 255
|
expected_upside_down_image[67 : 67 + 123, 56 : 56 + 234, 2] = 255
|
||||||
|
|
||||||
upside_down_image = np.empty((480, 640, 3), dtype=np.uint8)
|
upside_down_image = np.empty((480, 640, 3), dtype=np.uint8)
|
||||||
mujoco.mjr_readPixels(upside_down_image, None, full_rect, context)
|
mujoco.mjr_readPixels(upside_down_image, None, full_rect, context)
|
||||||
@@ -71,7 +78,8 @@ class MuJoCoRenderTest(absltest.TestCase):
|
|||||||
# Check that mjr_readPixels can accept a flattened array.
|
# Check that mjr_readPixels can accept a flattened array.
|
||||||
upside_down_image[:] = 0
|
upside_down_image[:] = 0
|
||||||
mujoco.mjr_readPixels(
|
mujoco.mjr_readPixels(
|
||||||
np.reshape(upside_down_image, -1), None, full_rect, context)
|
np.reshape(upside_down_image, -1), None, full_rect, context
|
||||||
|
)
|
||||||
np.testing.assert_array_equal(upside_down_image, expected_upside_down_image)
|
np.testing.assert_array_equal(upside_down_image, expected_upside_down_image)
|
||||||
context.free()
|
context.free()
|
||||||
|
|
||||||
@@ -81,8 +89,14 @@ class MuJoCoRenderTest(absltest.TestCase):
|
|||||||
|
|
||||||
scene = mujoco.MjvScene(self.model, maxgeom=0)
|
scene = mujoco.MjvScene(self.model, maxgeom=0)
|
||||||
mujoco.mjv_updateScene(
|
mujoco.mjv_updateScene(
|
||||||
self.model, self.data, mujoco.MjvOption(), None,
|
self.model,
|
||||||
mujoco.MjvCamera(), mujoco.mjtCatBit.mjCAT_ALL, scene)
|
self.data,
|
||||||
|
mujoco.MjvOption(),
|
||||||
|
None,
|
||||||
|
mujoco.MjvCamera(),
|
||||||
|
mujoco.mjtCatBit.mjCAT_ALL,
|
||||||
|
scene,
|
||||||
|
)
|
||||||
|
|
||||||
context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150)
|
context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150)
|
||||||
mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, context)
|
mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, context)
|
||||||
|
|||||||
+13
-11
@@ -32,7 +32,7 @@ class Renderer:
|
|||||||
model: _structs.MjModel,
|
model: _structs.MjModel,
|
||||||
height: int = 240,
|
height: int = 240,
|
||||||
width: int = 320,
|
width: int = 320,
|
||||||
max_geom: int = 10000
|
max_geom: int = 10000,
|
||||||
) -> None:
|
) -> None:
|
||||||
"""Initializes a new `Renderer`.
|
"""Initializes a new `Renderer`.
|
||||||
|
|
||||||
@@ -43,6 +43,7 @@ class Renderer:
|
|||||||
max_geom: Optional integer specifying the maximum number of geoms that can
|
max_geom: Optional integer specifying the maximum number of geoms that can
|
||||||
be rendered in the same scene. If None this will be chosen automatically
|
be rendered in the same scene. If None this will be chosen automatically
|
||||||
based on the estimated maximum number of renderable geoms in the model.
|
based on the estimated maximum number of renderable geoms in the model.
|
||||||
|
|
||||||
Raises:
|
Raises:
|
||||||
ValueError: If `camera_id` is outside the valid range, or if `width` or
|
ValueError: If `camera_id` is outside the valid range, or if `width` or
|
||||||
`height` exceed the dimensions of MuJoCo's offscreen framebuffer.
|
`height` exceed the dimensions of MuJoCo's offscreen framebuffer.
|
||||||
@@ -220,9 +221,7 @@ the clause:
|
|||||||
# Convert 3-channel uint8 to 1-channel uint32.
|
# Convert 3-channel uint8 to 1-channel uint32.
|
||||||
image3 = out.astype(np.uint32)
|
image3 = out.astype(np.uint32)
|
||||||
segimage = (
|
segimage = (
|
||||||
image3[:, :, 0]
|
image3[:, :, 0] + image3[:, :, 1] * (2**8) + image3[:, :, 2] * (2**16)
|
||||||
+ image3[:, :, 1] * (2**8)
|
|
||||||
+ image3[:, :, 2] * (2**16)
|
|
||||||
)
|
)
|
||||||
# Remap segid to 2-channel (object ID, object type) pair.
|
# Remap segid to 2-channel (object ID, object type) pair.
|
||||||
# Seg ID 0 is background -- will be remapped to (-1, -1).
|
# Seg ID 0 is background -- will be remapped to (-1, -1).
|
||||||
@@ -251,15 +250,15 @@ the clause:
|
|||||||
self,
|
self,
|
||||||
data: _structs.MjData,
|
data: _structs.MjData,
|
||||||
camera: Union[int, str, _structs.MjvCamera] = -1,
|
camera: Union[int, str, _structs.MjvCamera] = -1,
|
||||||
scene_option: Optional[_structs.MjvOption] = None
|
scene_option: Optional[_structs.MjvOption] = None,
|
||||||
):
|
):
|
||||||
"""Updates geometry used for rendering.
|
"""Updates geometry used for rendering.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
data: An instance of `MjData`.
|
data: An instance of `MjData`.
|
||||||
camera: An instance of `MjvCamera`, a string or an integer
|
camera: An instance of `MjvCamera`, a string or an integer
|
||||||
scene_option: A custom `MjvOption` instance to use to render
|
scene_option: A custom `MjvOption` instance to use to render the scene
|
||||||
the scene instead of the default.
|
instead of the default.
|
||||||
|
|
||||||
Raises:
|
Raises:
|
||||||
ValueError: If `camera_id` is outside the valid range, or if camera does
|
ValueError: If `camera_id` is outside the valid range, or if camera does
|
||||||
@@ -274,8 +273,10 @@ the clause:
|
|||||||
if camera_id == -1:
|
if camera_id == -1:
|
||||||
raise ValueError(f'The camera "{camera}" does not exist.')
|
raise ValueError(f'The camera "{camera}" does not exist.')
|
||||||
if camera_id < -1 or camera_id >= self._model.ncam:
|
if camera_id < -1 or camera_id >= self._model.ncam:
|
||||||
raise ValueError(f'The camera id {camera_id} is out of'
|
raise ValueError(
|
||||||
f' range [-1, {self._model.ncam}).')
|
f'The camera id {camera_id} is out of'
|
||||||
|
f' range [-1, {self._model.ncam}).'
|
||||||
|
)
|
||||||
|
|
||||||
# Render camera.
|
# Render camera.
|
||||||
camera = _structs.MjvCamera()
|
camera = _structs.MjvCamera()
|
||||||
@@ -295,7 +296,8 @@ the clause:
|
|||||||
data,
|
data,
|
||||||
scene_option,
|
scene_option,
|
||||||
None,
|
None,
|
||||||
camera, _enums.mjtCatBit.mjCAT_ALL.value,
|
camera,
|
||||||
|
_enums.mjtCatBit.mjCAT_ALL.value,
|
||||||
self._scene,
|
self._scene,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@@ -20,9 +20,11 @@ import mujoco
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
@absltest.skipUnless(hasattr(mujoco, 'GLContext'),
|
@absltest.skipUnless(
|
||||||
'MuJoCo rendering is disabled')
|
hasattr(mujoco, 'GLContext'), 'MuJoCo rendering is disabled'
|
||||||
|
)
|
||||||
class MuJoCoRendererTest(parameterized.TestCase):
|
class MuJoCoRendererTest(parameterized.TestCase):
|
||||||
|
|
||||||
def test_renderer_unknown_camera_name(self):
|
def test_renderer_unknown_camera_name(self):
|
||||||
xml = """
|
xml = """
|
||||||
<mujoco>
|
<mujoco>
|
||||||
|
|||||||
+70
-43
@@ -23,17 +23,19 @@ import numpy as np
|
|||||||
from numpy import typing as npt
|
from numpy import typing as npt
|
||||||
|
|
||||||
|
|
||||||
def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]],
|
def rollout(
|
||||||
data: mujoco.MjData,
|
model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]],
|
||||||
initial_state: npt.ArrayLike,
|
data: mujoco.MjData,
|
||||||
control: Optional[npt.ArrayLike] = None,
|
initial_state: npt.ArrayLike,
|
||||||
*, # require subsequent arguments to be named
|
control: Optional[npt.ArrayLike] = None,
|
||||||
control_spec: int = mujoco.mjtState.mjSTATE_CTRL.value,
|
*, # require subsequent arguments to be named
|
||||||
skip_checks: bool = False,
|
control_spec: int = mujoco.mjtState.mjSTATE_CTRL.value,
|
||||||
nstep: Optional[int] = None,
|
skip_checks: bool = False,
|
||||||
initial_warmstart: Optional[npt.ArrayLike] = None,
|
nstep: Optional[int] = None,
|
||||||
state: Optional[npt.ArrayLike] = None,
|
initial_warmstart: Optional[npt.ArrayLike] = None,
|
||||||
sensordata: Optional[npt.ArrayLike] = None):
|
state: Optional[npt.ArrayLike] = None,
|
||||||
|
sensordata: Optional[npt.ArrayLike] = None,
|
||||||
|
):
|
||||||
"""Rolls out open-loop trajectories from initial states, get subsequent states and sensor values.
|
"""Rolls out open-loop trajectories from initial states, get subsequent states and sensor values.
|
||||||
|
|
||||||
Python wrapper for rollout.cc, see documentation therein.
|
Python wrapper for rollout.cc, see documentation therein.
|
||||||
@@ -66,15 +68,24 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]],
|
|||||||
|
|
||||||
Raises:
|
Raises:
|
||||||
ValueError: bad shapes or sizes.
|
ValueError: bad shapes or sizes.
|
||||||
"""
|
""" # fmt: skip
|
||||||
# skip_checks shortcut:
|
# skip_checks shortcut:
|
||||||
# don't infer nroll/nstep
|
# don't infer nroll/nstep
|
||||||
# don't support singleton expansion
|
# don't support singleton expansion
|
||||||
# don't allocate output arrays
|
# don't allocate output arrays
|
||||||
# just call rollout and return
|
# just call rollout and return
|
||||||
if skip_checks:
|
if skip_checks:
|
||||||
_rollout.rollout(model, data, nstep, control_spec, initial_state,
|
_rollout.rollout(
|
||||||
initial_warmstart, control, state, sensordata)
|
model,
|
||||||
|
data,
|
||||||
|
nstep,
|
||||||
|
control_spec,
|
||||||
|
initial_state,
|
||||||
|
initial_warmstart,
|
||||||
|
control,
|
||||||
|
state,
|
||||||
|
sensordata,
|
||||||
|
)
|
||||||
return state, sensordata
|
return state, sensordata
|
||||||
|
|
||||||
if not isinstance(model, mujoco.MjModel):
|
if not isinstance(model, mujoco.MjModel):
|
||||||
@@ -92,17 +103,16 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]],
|
|||||||
initial_warmstart=initial_warmstart,
|
initial_warmstart=initial_warmstart,
|
||||||
control=control,
|
control=control,
|
||||||
state=state,
|
state=state,
|
||||||
sensordata=sensordata)
|
sensordata=sensordata,
|
||||||
|
)
|
||||||
|
|
||||||
# check number of dimensions
|
# check number of dimensions
|
||||||
_check_number_of_dimensions(2,
|
_check_number_of_dimensions(
|
||||||
initial_state=initial_state,
|
2, initial_state=initial_state, initial_warmstart=initial_warmstart
|
||||||
initial_warmstart=initial_warmstart)
|
)
|
||||||
_check_number_of_dimensions(3,
|
_check_number_of_dimensions(
|
||||||
control=control,
|
3, control=control, state=state, sensordata=sensordata
|
||||||
state=state,
|
)
|
||||||
sensordata=sensordata)
|
|
||||||
|
|
||||||
# ensure 2D, make contiguous, row-major (C ordering)
|
# ensure 2D, make contiguous, row-major (C ordering)
|
||||||
initial_state = _ensure_2d(initial_state)
|
initial_state = _ensure_2d(initial_state)
|
||||||
@@ -114,38 +124,46 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]],
|
|||||||
sensordata = _ensure_3d(sensordata)
|
sensordata = _ensure_3d(sensordata)
|
||||||
|
|
||||||
# infer nroll, check for incompatibilities
|
# infer nroll, check for incompatibilities
|
||||||
nroll = _infer_dimension(0, 1,
|
nroll = _infer_dimension(
|
||||||
initial_state=initial_state,
|
0,
|
||||||
initial_warmstart=initial_warmstart,
|
1,
|
||||||
control=control,
|
initial_state=initial_state,
|
||||||
state=state,
|
initial_warmstart=initial_warmstart,
|
||||||
sensordata=sensordata)
|
control=control,
|
||||||
|
state=state,
|
||||||
|
sensordata=sensordata,
|
||||||
|
)
|
||||||
if isinstance(model, list) and nroll == 1:
|
if isinstance(model, list) and nroll == 1:
|
||||||
nroll = len(model)
|
nroll = len(model)
|
||||||
|
|
||||||
if isinstance(model, list) and len(model) != nroll:
|
if isinstance(model, list) and len(model) != nroll:
|
||||||
raise ValueError(f'nroll inferred as {nroll} '
|
raise ValueError(
|
||||||
f'but model is length {len(model)}')
|
f'nroll inferred as {nroll} but model is length {len(model)}'
|
||||||
|
)
|
||||||
elif not isinstance(model, list):
|
elif not isinstance(model, list):
|
||||||
model = [model] # Use a length 1 list to simplify code below
|
model = [model] # Use a length 1 list to simplify code below
|
||||||
|
|
||||||
# infer nstep, check for incompatibilities
|
# infer nstep, check for incompatibilities
|
||||||
nstep = _infer_dimension(1, nstep or 1,
|
nstep = _infer_dimension(
|
||||||
control=control,
|
1, nstep or 1, control=control, state=state, sensordata=sensordata
|
||||||
state=state,
|
)
|
||||||
sensordata=sensordata)
|
|
||||||
|
|
||||||
# get nstate/ncontrol/nv/nsensordata
|
# get nstate/ncontrol/nv/nsensordata
|
||||||
# check that they are equal across models
|
# check that they are equal across models
|
||||||
nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS.value)
|
nstate = mujoco.mj_stateSize(
|
||||||
|
model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS.value
|
||||||
|
)
|
||||||
ncontrol = mujoco.mj_stateSize(model[0], control_spec)
|
ncontrol = mujoco.mj_stateSize(model[0], control_spec)
|
||||||
nv = model[0].nv
|
nv = model[0].nv
|
||||||
nsensordata = model[0].nsensordata
|
nsensordata = model[0].nsensordata
|
||||||
for m in model[1:]:
|
for m in model[1:]:
|
||||||
if (nstate != mujoco.mj_stateSize(m, mujoco.mjtState.mjSTATE_FULLPHYSICS.value)
|
if (
|
||||||
|
nstate
|
||||||
|
!= mujoco.mj_stateSize(m, mujoco.mjtState.mjSTATE_FULLPHYSICS.value)
|
||||||
or ncontrol != mujoco.mj_stateSize(m, control_spec)
|
or ncontrol != mujoco.mj_stateSize(m, control_spec)
|
||||||
or nv != m.nv
|
or nv != m.nv
|
||||||
or nsensordata != m.nsensordata):
|
or nsensordata != m.nsensordata
|
||||||
|
):
|
||||||
raise ValueError('models are not compatible')
|
raise ValueError('models are not compatible')
|
||||||
|
|
||||||
# check trailing dimensions
|
# check trailing dimensions
|
||||||
@@ -167,8 +185,17 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]],
|
|||||||
sensordata = np.empty((nroll, nstep, nsensordata))
|
sensordata = np.empty((nroll, nstep, nsensordata))
|
||||||
|
|
||||||
# call rollout
|
# call rollout
|
||||||
_rollout.rollout(model, data, nstep, control_spec, initial_state,
|
_rollout.rollout(
|
||||||
initial_warmstart, control, state, sensordata)
|
model,
|
||||||
|
data,
|
||||||
|
nstep,
|
||||||
|
control_spec,
|
||||||
|
initial_state,
|
||||||
|
initial_warmstart,
|
||||||
|
control,
|
||||||
|
state,
|
||||||
|
sensordata,
|
||||||
|
)
|
||||||
|
|
||||||
# return outputs
|
# return outputs
|
||||||
return state, sensordata
|
return state, sensordata
|
||||||
@@ -227,8 +254,8 @@ def _infer_dimension(dim, value, **kwargs):
|
|||||||
Args:
|
Args:
|
||||||
dim: Dimension to be inferred.
|
dim: Dimension to be inferred.
|
||||||
value: Initial guess of inferred value (1: unknown).
|
value: Initial guess of inferred value (1: unknown).
|
||||||
**kwargs: List of arrays which should all have the same size (or 1)
|
**kwargs: List of arrays which should all have the same size (or 1) along
|
||||||
along dimension dim.
|
dimension dim.
|
||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
Inferred dimension.
|
Inferred dimension.
|
||||||
|
|||||||
+125
-74
@@ -127,10 +127,12 @@ TEST_XML_DIVERGE = r"""
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
"""
|
"""
|
||||||
|
|
||||||
ALL_MODELS = {'TEST_XML': TEST_XML,
|
ALL_MODELS = {
|
||||||
'TEST_XML_NO_SENSORS': TEST_XML_NO_SENSORS,
|
'TEST_XML': TEST_XML,
|
||||||
'TEST_XML_NO_ACTUATORS': TEST_XML_NO_ACTUATORS,
|
'TEST_XML_NO_SENSORS': TEST_XML_NO_SENSORS,
|
||||||
'TEST_XML_EMPTY': TEST_XML_EMPTY}
|
'TEST_XML_NO_ACTUATORS': TEST_XML_NO_ACTUATORS,
|
||||||
|
'TEST_XML_EMPTY': TEST_XML_EMPTY,
|
||||||
|
}
|
||||||
|
|
||||||
# ------------------------------ tests -----------------------------------------
|
# ------------------------------ tests -----------------------------------------
|
||||||
|
|
||||||
@@ -242,8 +244,9 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
initial_state = np.random.randn(nstate)
|
initial_state = np.random.randn(nstate)
|
||||||
control = np.random.randn(nstep, model.nu)
|
control = np.random.randn(nstep, model.nu)
|
||||||
initial_warmstart = np.tile(data.qacc_warmstart.copy(), (nroll, 1))
|
initial_warmstart = np.tile(data.qacc_warmstart.copy(), (nroll, 1))
|
||||||
state, sensordata = rollout.rollout(model, data, initial_state, control,
|
state, sensordata = rollout.rollout(
|
||||||
initial_warmstart=initial_warmstart)
|
model, data, initial_state, control, initial_warmstart=initial_warmstart
|
||||||
|
)
|
||||||
|
|
||||||
mujoco.mj_resetData(model, data)
|
mujoco.mj_resetData(model, data)
|
||||||
initial_state = np.tile(initial_state, (nroll, 1))
|
initial_state = np.tile(initial_state, (nroll, 1))
|
||||||
@@ -264,8 +267,9 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
initial_state = np.random.randn(nstate)
|
initial_state = np.random.randn(nstate)
|
||||||
control = np.random.randn(nstep, model.nu)
|
control = np.random.randn(nstep, model.nu)
|
||||||
state = np.empty((nroll, nstep, nstate))
|
state = np.empty((nroll, nstep, nstate))
|
||||||
state, sensordata = rollout.rollout(model, data, initial_state, control,
|
state, sensordata = rollout.rollout(
|
||||||
state=state)
|
model, data, initial_state, control, state=state
|
||||||
|
)
|
||||||
|
|
||||||
mujoco.mj_resetData(model, data)
|
mujoco.mj_resetData(model, data)
|
||||||
initial_state = np.tile(initial_state, (nroll, 1))
|
initial_state = np.tile(initial_state, (nroll, 1))
|
||||||
@@ -286,8 +290,9 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
initial_state = np.random.randn(nstate)
|
initial_state = np.random.randn(nstate)
|
||||||
control = np.random.randn(nstep, model.nu)
|
control = np.random.randn(nstep, model.nu)
|
||||||
sensordata = np.empty((nroll, nstep, model.nsensordata))
|
sensordata = np.empty((nroll, nstep, model.nsensordata))
|
||||||
state, sensordata = rollout.rollout(model, data, initial_state, control,
|
state, sensordata = rollout.rollout(
|
||||||
sensordata=sensordata)
|
model, data, initial_state, control, sensordata=sensordata
|
||||||
|
)
|
||||||
|
|
||||||
mujoco.mj_resetData(model, data)
|
mujoco.mj_resetData(model, data)
|
||||||
initial_state = np.tile(initial_state, (nroll, 1))
|
initial_state = np.tile(initial_state, (nroll, 1))
|
||||||
@@ -309,8 +314,9 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
control = np.random.randn(model.nu)
|
control = np.random.randn(model.nu)
|
||||||
state = np.empty((nroll, nstep, nstate))
|
state = np.empty((nroll, nstep, nstate))
|
||||||
sensordata = np.empty((nroll, nstep, model.nsensordata))
|
sensordata = np.empty((nroll, nstep, model.nsensordata))
|
||||||
rollout.rollout(model, data, initial_state, control,
|
rollout.rollout(
|
||||||
state=state, sensordata=sensordata)
|
model, data, initial_state, control, state=state, sensordata=sensordata
|
||||||
|
)
|
||||||
|
|
||||||
control = np.tile(control, (nstep, 1))
|
control = np.tile(control, (nstep, 1))
|
||||||
py_state, py_sensordata = py_rollout(model, data, initial_state, control)
|
py_state, py_sensordata = py_rollout(model, data, initial_state, control)
|
||||||
@@ -374,8 +380,9 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
initial_state = np.random.randn(nroll, nstate)
|
initial_state = np.random.randn(nroll, nstate)
|
||||||
control = np.random.randn(nroll, 1, model.nu)
|
control = np.random.randn(nroll, 1, model.nu)
|
||||||
state = np.empty((nroll, nstep, nstate))
|
state = np.empty((nroll, nstep, nstate))
|
||||||
state, sensordata = rollout.rollout(model, data, initial_state, control,
|
state, sensordata = rollout.rollout(
|
||||||
state=state)
|
model, data, initial_state, control, state=state
|
||||||
|
)
|
||||||
|
|
||||||
control = np.repeat(control, nstep, axis=1)
|
control = np.repeat(control, nstep, axis=1)
|
||||||
py_state, py_sensordata = py_rollout(model, data, initial_state, control)
|
py_state, py_sensordata = py_rollout(model, data, initial_state, control)
|
||||||
@@ -393,17 +400,21 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
|
|
||||||
initial_state = np.random.randn(nroll, nstate)
|
initial_state = np.random.randn(nroll, nstate)
|
||||||
|
|
||||||
control_spec = (mujoco.mjtState.mjSTATE_CTRL |
|
control_spec = (
|
||||||
mujoco.mjtState.mjSTATE_QFRC_APPLIED |
|
mujoco.mjtState.mjSTATE_CTRL
|
||||||
mujoco.mjtState.mjSTATE_XFRC_APPLIED)
|
| mujoco.mjtState.mjSTATE_QFRC_APPLIED
|
||||||
|
| mujoco.mjtState.mjSTATE_XFRC_APPLIED
|
||||||
|
)
|
||||||
ncontrol = mujoco.mj_stateSize(model, control_spec)
|
ncontrol = mujoco.mj_stateSize(model, control_spec)
|
||||||
control = np.random.randn(nroll, nstep, ncontrol)
|
control = np.random.randn(nroll, nstep, ncontrol)
|
||||||
|
|
||||||
state, sensordata = rollout.rollout(model, data, initial_state, control,
|
state, sensordata = rollout.rollout(
|
||||||
control_spec=control_spec)
|
model, data, initial_state, control, control_spec=control_spec
|
||||||
|
)
|
||||||
|
|
||||||
py_state, py_sensordata = py_rollout(model, data, initial_state, control,
|
py_state, py_sensordata = py_rollout(
|
||||||
control_spec=control_spec)
|
model, data, initial_state, control, control_spec=control_spec
|
||||||
|
)
|
||||||
np.testing.assert_array_equal(state, py_state)
|
np.testing.assert_array_equal(state, py_state)
|
||||||
np.testing.assert_array_equal(sensordata, py_sensordata)
|
np.testing.assert_array_equal(sensordata, py_sensordata)
|
||||||
|
|
||||||
@@ -416,15 +427,19 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
initial_state = np.empty((nroll, nstate))
|
initial_state = np.empty((nroll, nstate))
|
||||||
|
|
||||||
# get diverging (0, 2) and non-diverging (1, 3) states
|
# get diverging (0, 2) and non-diverging (1, 3) states
|
||||||
mujoco.mj_getState(model, data, initial_state[0],
|
mujoco.mj_getState(
|
||||||
mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
model, data, initial_state[0], mujoco.mjtState.mjSTATE_FULLPHYSICS
|
||||||
mujoco.mj_getState(model, data, initial_state[2],
|
)
|
||||||
mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
mujoco.mj_getState(
|
||||||
|
model, data, initial_state[2], mujoco.mjtState.mjSTATE_FULLPHYSICS
|
||||||
|
)
|
||||||
mujoco.mj_resetDataKeyframe(model, data, 0) # keyframe 0 does not diverge
|
mujoco.mj_resetDataKeyframe(model, data, 0) # keyframe 0 does not diverge
|
||||||
mujoco.mj_getState(model, data, initial_state[1],
|
mujoco.mj_getState(
|
||||||
mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
model, data, initial_state[1], mujoco.mjtState.mjSTATE_FULLPHYSICS
|
||||||
mujoco.mj_getState(model, data, initial_state[3],
|
)
|
||||||
mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
mujoco.mj_getState(
|
||||||
|
model, data, initial_state[3], mujoco.mjtState.mjSTATE_FULLPHYSICS
|
||||||
|
)
|
||||||
|
|
||||||
nstep = 10000 # divergence after ~15s, timestep = 2e-3
|
nstep = 10000 # divergence after ~15s, timestep = 2e-3
|
||||||
|
|
||||||
@@ -459,27 +474,40 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
thread_local.data = mujoco.MjData(model)
|
thread_local.data = mujoco.MjData(model)
|
||||||
|
|
||||||
model_list = [model] * nroll
|
model_list = [model] * nroll
|
||||||
|
|
||||||
def call_rollout(initial_state, control, state, sensordata):
|
def call_rollout(initial_state, control, state, sensordata):
|
||||||
rollout.rollout(model_list, thread_local.data, initial_state, control,
|
rollout.rollout(
|
||||||
skip_checks=True,
|
model_list,
|
||||||
nstep=nstep, state=state, sensordata=sensordata)
|
thread_local.data,
|
||||||
|
initial_state,
|
||||||
|
control,
|
||||||
|
skip_checks=True,
|
||||||
|
nstep=nstep,
|
||||||
|
state=state,
|
||||||
|
sensordata=sensordata,
|
||||||
|
)
|
||||||
|
|
||||||
n = nroll // num_workers # integer division
|
n = nroll // num_workers # integer division
|
||||||
chunks = [] # a list of tuples, one per worker
|
chunks = [] # a list of tuples, one per worker
|
||||||
for i in range(num_workers-1):
|
for i in range(num_workers - 1):
|
||||||
chunks.append((initial_state[i*n:(i+1)*n],
|
chunks.append((
|
||||||
control[i*n:(i+1)*n],
|
initial_state[i * n : (i + 1) * n],
|
||||||
state[i*n:(i+1)*n],
|
control[i * n : (i + 1) * n],
|
||||||
sensordata[i*n:(i+1)*n]))
|
state[i * n : (i + 1) * n],
|
||||||
|
sensordata[i * n : (i + 1) * n],
|
||||||
|
))
|
||||||
|
|
||||||
# last chunk, absorbing the remainder:
|
# last chunk, absorbing the remainder:
|
||||||
chunks.append((initial_state[(num_workers-1)*n:],
|
chunks.append((
|
||||||
control[(num_workers-1)*n:],
|
initial_state[(num_workers - 1) * n :],
|
||||||
state[(num_workers-1)*n:],
|
control[(num_workers - 1) * n :],
|
||||||
sensordata[(num_workers-1)*n:]))
|
state[(num_workers - 1) * n :],
|
||||||
|
sensordata[(num_workers - 1) * n :],
|
||||||
|
))
|
||||||
|
|
||||||
with concurrent.futures.ThreadPoolExecutor(
|
with concurrent.futures.ThreadPoolExecutor(
|
||||||
max_workers=num_workers, initializer=thread_initializer) as executor:
|
max_workers=num_workers, initializer=thread_initializer
|
||||||
|
) as executor:
|
||||||
futures = []
|
futures = []
|
||||||
for chunk in chunks:
|
for chunk in chunks:
|
||||||
futures.append(executor.submit(call_rollout, *chunk))
|
futures.append(executor.submit(call_rollout, *chunk))
|
||||||
@@ -513,12 +541,14 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
state, _ = rollout.rollout(model, data, state1[0], control)
|
state, _ = rollout.rollout(model, data, state1[0], control)
|
||||||
|
|
||||||
# assert that stepping without warmstarts is not exact
|
# assert that stepping without warmstarts is not exact
|
||||||
np.testing.assert_raises(AssertionError,
|
np.testing.assert_raises(
|
||||||
np.testing.assert_array_equal, state, state2)
|
AssertionError, np.testing.assert_array_equal, state, state2
|
||||||
|
)
|
||||||
|
|
||||||
# take step using rollout, take warmstart into account
|
# take step using rollout, take warmstart into account
|
||||||
state, _ = rollout.rollout(model, data, state1, control,
|
state, _ = rollout.rollout(
|
||||||
initial_warmstart=initial_warmstart)
|
model, data, state1, control, initial_warmstart=initial_warmstart
|
||||||
|
)
|
||||||
|
|
||||||
# assert exact equality
|
# assert exact equality
|
||||||
np.testing.assert_array_equal(state, np.expand_dims(state2, axis=0))
|
np.testing.assert_array_equal(state, np.expand_dims(state2, axis=0))
|
||||||
@@ -530,19 +560,21 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
|
|
||||||
initial_state = np.zeros(nstate)
|
initial_state = np.zeros(nstate)
|
||||||
|
|
||||||
control_spec = (mujoco.mjtState.mjSTATE_MOCAP_POS |
|
control_spec = (
|
||||||
mujoco.mjtState.mjSTATE_MOCAP_QUAT)
|
mujoco.mjtState.mjSTATE_MOCAP_POS | mujoco.mjtState.mjSTATE_MOCAP_QUAT
|
||||||
|
)
|
||||||
|
|
||||||
pos1 = np.array((1., 2., 3.))
|
pos1 = np.array((1.0, 2.0, 3.0))
|
||||||
quat1 = np.array((1., 2., 3., 4.))
|
quat1 = np.array((1.0, 2.0, 3.0, 4.0))
|
||||||
quat1 /= np.linalg.norm(quat1)
|
quat1 /= np.linalg.norm(quat1)
|
||||||
pos2 = np.array((2., 3., 4.))
|
pos2 = np.array((2.0, 3.0, 4.0))
|
||||||
quat2 = np.array((2., 3., 4., 5.))
|
quat2 = np.array((2.0, 3.0, 4.0, 5.0))
|
||||||
quat2 /= np.linalg.norm(quat2)
|
quat2 /= np.linalg.norm(quat2)
|
||||||
control = np.hstack((pos1, pos2, quat1, quat2))
|
control = np.hstack((pos1, pos2, quat1, quat2))
|
||||||
|
|
||||||
_, sensordata = rollout.rollout(model, data, initial_state, control,
|
_, sensordata = rollout.rollout(
|
||||||
control_spec=control_spec)
|
model, data, initial_state, control, control_spec=control_spec
|
||||||
|
)
|
||||||
|
|
||||||
np.testing.assert_array_almost_equal(sensordata[0][0][:3], pos1)
|
np.testing.assert_array_almost_equal(sensordata[0][0][:3], pos1)
|
||||||
np.testing.assert_array_almost_equal(sensordata[0][0][3:], quat1)
|
np.testing.assert_array_almost_equal(sensordata[0][0][3:], quat1)
|
||||||
@@ -562,7 +594,8 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
|
|
||||||
model.opt.solver = 10 # invalid solver type
|
model.opt.solver = 10 # invalid solver type
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
mujoco.FatalError, 'mj_fwdConstraint: unknown solver type 10'):
|
mujoco.FatalError, 'mj_fwdConstraint: unknown solver type 10'
|
||||||
|
):
|
||||||
rollout.rollout(model, data, initial_state, ctrl)
|
rollout.rollout(model, data, initial_state, ctrl)
|
||||||
|
|
||||||
def test_invalid(self):
|
def test_invalid(self):
|
||||||
@@ -576,12 +609,14 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
|
|
||||||
control = 'string'
|
control = 'string'
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
ValueError, 'control must be a numpy array or float'):
|
ValueError, 'control must be a numpy array or float'
|
||||||
|
):
|
||||||
rollout.rollout(model, data, initial_state, control)
|
rollout.rollout(model, data, initial_state, control)
|
||||||
|
|
||||||
control = np.zeros((2, 3, 4, 5))
|
control = np.zeros((2, 3, 4, 5))
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
ValueError, 'control can have at most 3 dimensions'):
|
ValueError, 'control can have at most 3 dimensions'
|
||||||
|
):
|
||||||
rollout.rollout(model, data, initial_state, control)
|
rollout.rollout(model, data, initial_state, control)
|
||||||
|
|
||||||
def test_bad_sizes(self):
|
def test_bad_sizes(self):
|
||||||
@@ -594,28 +629,33 @@ class MuJoCoRolloutTest(parameterized.TestCase):
|
|||||||
|
|
||||||
initial_state = np.random.randn(nroll, nstate + 1)
|
initial_state = np.random.randn(nroll, nstate + 1)
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
ValueError, 'trailing dimension of initial_state must be 6, got 7'):
|
ValueError, 'trailing dimension of initial_state must be 6, got 7'
|
||||||
|
):
|
||||||
rollout.rollout(model, data, initial_state)
|
rollout.rollout(model, data, initial_state)
|
||||||
|
|
||||||
initial_state = np.random.randn(nroll, nstate)
|
initial_state = np.random.randn(nroll, nstate)
|
||||||
control = np.random.randn(1, nstep, model.nu + 1)
|
control = np.random.randn(1, nstep, model.nu + 1)
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
ValueError, 'trailing dimension of control must be 2, got 3'):
|
ValueError, 'trailing dimension of control must be 2, got 3'
|
||||||
|
):
|
||||||
rollout.rollout(model, data, initial_state, control)
|
rollout.rollout(model, data, initial_state, control)
|
||||||
|
|
||||||
control = np.random.randn(nroll, nstep, model.nu)
|
control = np.random.randn(nroll, nstep, model.nu)
|
||||||
state = np.random.randn(nroll, nstep+1, nstate) # incompatible nstep
|
state = np.random.randn(nroll, nstep + 1, nstate) # incompatible nstep
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
ValueError, 'dimension 1 inferred as 3 but state has 4'):
|
ValueError, 'dimension 1 inferred as 3 but state has 4'
|
||||||
|
):
|
||||||
rollout.rollout(model, data, initial_state, control, state=state)
|
rollout.rollout(model, data, initial_state, control, state=state)
|
||||||
|
|
||||||
initial_state = np.random.randn(nroll, nstate)
|
initial_state = np.random.randn(nroll, nstate)
|
||||||
control = np.random.randn(nroll, nstep, model.nu)
|
control = np.random.randn(nroll, nstep, model.nu)
|
||||||
bad_spec = mujoco.mjtState.mjSTATE_ACT
|
bad_spec = mujoco.mjtState.mjSTATE_ACT
|
||||||
with self.assertRaisesWithLiteralMatch(
|
with self.assertRaisesWithLiteralMatch(
|
||||||
ValueError, 'control_spec can only contain bits in mjSTATE_USER'):
|
ValueError, 'control_spec can only contain bits in mjSTATE_USER'
|
||||||
rollout.rollout(model, data, initial_state, control,
|
):
|
||||||
control_spec=bad_spec)
|
rollout.rollout(
|
||||||
|
model, data, initial_state, control, control_spec=bad_spec
|
||||||
|
)
|
||||||
|
|
||||||
def test_stateless(self):
|
def test_stateless(self):
|
||||||
model = mujoco.MjModel.from_xml_string(TEST_XML)
|
model = mujoco.MjModel.from_xml_string(TEST_XML)
|
||||||
@@ -655,8 +695,9 @@ def get_state(model, data):
|
|||||||
return state.reshape((1, nstate))
|
return state.reshape((1, nstate))
|
||||||
|
|
||||||
|
|
||||||
def step(model, data, state, control,
|
def step(
|
||||||
control_spec=mujoco.mjtState.mjSTATE_CTRL):
|
model, data, state, control, control_spec=mujoco.mjtState.mjSTATE_CTRL
|
||||||
|
):
|
||||||
if state is not None:
|
if state is not None:
|
||||||
mujoco.mj_setState(model, data, state, mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
mujoco.mj_setState(model, data, state, mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
||||||
mujoco.mj_setState(model, data, control, control_spec)
|
mujoco.mj_setState(model, data, control, control_spec)
|
||||||
@@ -664,8 +705,13 @@ def step(model, data, state, control,
|
|||||||
return (get_state(model, data), data.sensordata)
|
return (get_state(model, data), data.sensordata)
|
||||||
|
|
||||||
|
|
||||||
def one_rollout(model, data, initial_state, control,
|
def one_rollout(
|
||||||
control_spec=mujoco.mjtState.mjSTATE_CTRL):
|
model,
|
||||||
|
data,
|
||||||
|
initial_state,
|
||||||
|
control,
|
||||||
|
control_spec=mujoco.mjtState.mjSTATE_CTRL,
|
||||||
|
):
|
||||||
nstep = control.shape[0]
|
nstep = control.shape[0]
|
||||||
nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
||||||
state = np.empty((nstep, nstate))
|
state = np.empty((nstep, nstate))
|
||||||
@@ -673,9 +719,9 @@ def one_rollout(model, data, initial_state, control,
|
|||||||
|
|
||||||
mujoco.mj_resetData(model, data)
|
mujoco.mj_resetData(model, data)
|
||||||
for t in range(nstep):
|
for t in range(nstep):
|
||||||
state[t], sensordata[t] = step(model, data,
|
state[t], sensordata[t] = step(
|
||||||
initial_state if t == 0 else None,
|
model, data, initial_state if t == 0 else None, control[t], control_spec
|
||||||
control[t], control_spec)
|
)
|
||||||
return state, sensordata
|
return state, sensordata
|
||||||
|
|
||||||
|
|
||||||
@@ -700,15 +746,20 @@ def ensure_3d(arg):
|
|||||||
return np.ascontiguousarray(arg, dtype=np.float64)
|
return np.ascontiguousarray(arg, dtype=np.float64)
|
||||||
|
|
||||||
|
|
||||||
def py_rollout(model, data, initial_state, control,
|
def py_rollout(
|
||||||
control_spec=mujoco.mjtState.mjSTATE_CTRL):
|
model,
|
||||||
|
data,
|
||||||
|
initial_state,
|
||||||
|
control,
|
||||||
|
control_spec=mujoco.mjtState.mjSTATE_CTRL,
|
||||||
|
):
|
||||||
initial_state = ensure_2d(initial_state)
|
initial_state = ensure_2d(initial_state)
|
||||||
control = ensure_3d(control)
|
control = ensure_3d(control)
|
||||||
nroll = initial_state.shape[0]
|
nroll = initial_state.shape[0]
|
||||||
nstep = control.shape[1]
|
nstep = control.shape[1]
|
||||||
|
|
||||||
if isinstance(model, mujoco.MjModel):
|
if isinstance(model, mujoco.MjModel):
|
||||||
model = [model]*nroll
|
model = [model] * nroll
|
||||||
|
|
||||||
nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS)
|
||||||
|
|
||||||
|
|||||||
+24
-11
@@ -104,7 +104,9 @@ class SpecsTest(absltest.TestCase):
|
|||||||
self.assertEqual(model.nuser_site, 6)
|
self.assertEqual(model.nuser_site, 6)
|
||||||
np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6])
|
np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6])
|
||||||
|
|
||||||
self.assertEqual(spec.to_xml(), textwrap.dedent("""\
|
self.assertEqual(
|
||||||
|
spec.to_xml(),
|
||||||
|
textwrap.dedent("""\
|
||||||
<mujoco model="MuJoCo Model">
|
<mujoco model="MuJoCo Model">
|
||||||
<compiler angle="radian"/>
|
<compiler angle="radian"/>
|
||||||
|
|
||||||
@@ -116,7 +118,8 @@ class SpecsTest(absltest.TestCase):
|
|||||||
</body>
|
</body>
|
||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
"""),)
|
"""),
|
||||||
|
)
|
||||||
|
|
||||||
def test_kwarg(self):
|
def test_kwarg(self):
|
||||||
# Create a spec.
|
# Create a spec.
|
||||||
@@ -467,7 +470,7 @@ class SpecsTest(absltest.TestCase):
|
|||||||
# Try to compile, get error.
|
# Try to compile, get error.
|
||||||
expected_error = (
|
expected_error = (
|
||||||
'Error: size 0 must be positive in geom\n'
|
'Error: size 0 must be positive in geom\n'
|
||||||
+ f'Element name \'MyGeom\', id 0, geom added on line {added_on_line}'
|
+ f"Element name 'MyGeom', id 0, geom added on line {added_on_line}"
|
||||||
)
|
)
|
||||||
with self.assertRaisesRegex(ValueError, expected_error):
|
with self.assertRaisesRegex(ValueError, expected_error):
|
||||||
spec.compile()
|
spec.compile()
|
||||||
@@ -531,7 +534,9 @@ class SpecsTest(absltest.TestCase):
|
|||||||
spec.worldbody.add_geom(main)
|
spec.worldbody.add_geom(main)
|
||||||
|
|
||||||
spec.compile()
|
spec.compile()
|
||||||
self.assertEqual(spec.to_xml(), textwrap.dedent("""\
|
self.assertEqual(
|
||||||
|
spec.to_xml(),
|
||||||
|
textwrap.dedent("""\
|
||||||
<mujoco model="test">
|
<mujoco model="test">
|
||||||
<compiler angle="radian"/>
|
<compiler angle="radian"/>
|
||||||
|
|
||||||
@@ -547,7 +552,8 @@ class SpecsTest(absltest.TestCase):
|
|||||||
<geom/>
|
<geom/>
|
||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
"""))
|
"""),
|
||||||
|
)
|
||||||
spec = mujoco.MjSpec()
|
spec = mujoco.MjSpec()
|
||||||
spec.modelname = 'test'
|
spec.modelname = 'test'
|
||||||
|
|
||||||
@@ -561,7 +567,9 @@ class SpecsTest(absltest.TestCase):
|
|||||||
spec.worldbody.add_geom(main)
|
spec.worldbody.add_geom(main)
|
||||||
|
|
||||||
spec.compile()
|
spec.compile()
|
||||||
self.assertEqual(spec.to_xml(), textwrap.dedent("""\
|
self.assertEqual(
|
||||||
|
spec.to_xml(),
|
||||||
|
textwrap.dedent("""\
|
||||||
<mujoco model="test">
|
<mujoco model="test">
|
||||||
<compiler angle="radian"/>
|
<compiler angle="radian"/>
|
||||||
|
|
||||||
@@ -577,7 +585,8 @@ class SpecsTest(absltest.TestCase):
|
|||||||
<geom/>
|
<geom/>
|
||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
"""))
|
"""),
|
||||||
|
)
|
||||||
|
|
||||||
def test_element_list(self):
|
def test_element_list(self):
|
||||||
spec = mujoco.MjSpec()
|
spec = mujoco.MjSpec()
|
||||||
@@ -718,13 +727,17 @@ class SpecsTest(absltest.TestCase):
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
"""
|
"""
|
||||||
spec = mujoco.MjSpec.from_string(textwrap.dedent("""
|
spec = mujoco.MjSpec.from_string(
|
||||||
|
textwrap.dedent("""
|
||||||
<mujoco model="MuJoCo Model">
|
<mujoco model="MuJoCo Model">
|
||||||
<include file="included.xml"/>
|
<include file="included.xml"/>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
"""), {'included.xml': included_xml.encode('utf-8')})
|
"""),
|
||||||
self.assertEqual(spec.worldbody.first_body().first_geom().type,
|
{'included.xml': included_xml.encode('utf-8')},
|
||||||
mujoco.mjtGeom.mjGEOM_BOX)
|
)
|
||||||
|
self.assertEqual(
|
||||||
|
spec.worldbody.first_body().first_geom().type, mujoco.mjtGeom.mjGEOM_BOX
|
||||||
|
)
|
||||||
|
|
||||||
def test_delete(self):
|
def test_delete(self):
|
||||||
file_path = epath.resource_path("mujoco") / "testdata" / "model.xml"
|
file_path = epath.resource_path("mujoco") / "testdata" / "model.xml"
|
||||||
|
|||||||
+35
-22
@@ -42,7 +42,7 @@ PERCENT_REALTIME = (
|
|||||||
10, 8, 6.6, 5, 4, 3.3, 2.5, 2, 1.6, 1.3,
|
10, 8, 6.6, 5, 4, 3.3, 2.5, 2, 1.6, 1.3,
|
||||||
1, 0.8, 0.66, 0.5, 0.4, 0.33, 0.25, 0.2, 0.16, 0.13,
|
1, 0.8, 0.66, 0.5, 0.4, 0.33, 0.25, 0.2, 0.16, 0.13,
|
||||||
0.1
|
0.1
|
||||||
)
|
) # fmt: skip
|
||||||
|
|
||||||
# Maximum time mis-alignment before re-sync.
|
# Maximum time mis-alignment before re-sync.
|
||||||
MAX_SYNC_MISALIGN = 0.1
|
MAX_SYNC_MISALIGN = 0.1
|
||||||
@@ -194,12 +194,13 @@ def _file_loader(path: str) -> _LoaderWithPathType:
|
|||||||
|
|
||||||
|
|
||||||
def _reload(
|
def _reload(
|
||||||
simulate: _Simulate, loader: _InternalLoaderType,
|
simulate: _Simulate,
|
||||||
notify_loaded: Optional[Callable[[], None]] = None
|
loader: _InternalLoaderType,
|
||||||
|
notify_loaded: Optional[Callable[[], None]] = None,
|
||||||
) -> Optional[Tuple[mujoco.MjModel, mujoco.MjData]]:
|
) -> Optional[Tuple[mujoco.MjModel, mujoco.MjData]]:
|
||||||
"""Internal function for reloading a model in the viewer."""
|
"""Internal function for reloading a model in the viewer."""
|
||||||
try:
|
try:
|
||||||
simulate.load_message('') # path is unknown at this point
|
simulate.load_message('') # path is unknown at this point
|
||||||
load_tuple = loader()
|
load_tuple = loader()
|
||||||
except Exception as e: # pylint: disable=broad-except
|
except Exception as e: # pylint: disable=broad-except
|
||||||
simulate.load_error = str(e)
|
simulate.load_error = str(e)
|
||||||
@@ -275,14 +276,16 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
|
|||||||
# Inject noise.
|
# Inject noise.
|
||||||
if simulate.ctrl_noise_std != 0.0:
|
if simulate.ctrl_noise_std != 0.0:
|
||||||
# Convert rate and scale to discrete time (Ornstein–Uhlenbeck).
|
# Convert rate and scale to discrete time (Ornstein–Uhlenbeck).
|
||||||
rate = math.exp(-m.opt.timestep /
|
rate = math.exp(
|
||||||
max(simulate.ctrl_noise_rate, mujoco.mjMINVAL))
|
-m.opt.timestep / max(simulate.ctrl_noise_rate, mujoco.mjMINVAL)
|
||||||
|
)
|
||||||
scale = simulate.ctrl_noise_std * math.sqrt(1 - rate * rate)
|
scale = simulate.ctrl_noise_std * math.sqrt(1 - rate * rate)
|
||||||
|
|
||||||
for i in range(m.nu):
|
for i in range(m.nu):
|
||||||
# Update noise.
|
# Update noise.
|
||||||
ctrl_noise[i] = (rate * ctrl_noise[i] +
|
ctrl_noise[i] = rate * ctrl_noise[
|
||||||
scale * mujoco.mju_standardNormal(None))
|
i
|
||||||
|
] + scale * mujoco.mju_standardNormal(None)
|
||||||
|
|
||||||
# Apply noise.
|
# Apply noise.
|
||||||
d.ctrl[i] = ctrl_noise[i]
|
d.ctrl[i] = ctrl_noise[i]
|
||||||
@@ -291,12 +294,18 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
|
|||||||
slowdown = 100 / PERCENT_REALTIME[simulate.real_time_index]
|
slowdown = 100 / PERCENT_REALTIME[simulate.real_time_index]
|
||||||
|
|
||||||
# Misalignment: distance from target sim time > MAX_SYNC_MISALIGN.
|
# Misalignment: distance from target sim time > MAX_SYNC_MISALIGN.
|
||||||
misaligned = abs(elapsedcpu / slowdown -
|
misaligned = (
|
||||||
elapsedsim) > MAX_SYNC_MISALIGN
|
abs(elapsedcpu / slowdown - elapsedsim) > MAX_SYNC_MISALIGN
|
||||||
|
)
|
||||||
|
|
||||||
# Out-of-sync (for any reason): reset sync times, step.
|
# Out-of-sync (for any reason): reset sync times, step.
|
||||||
if (elapsedsim < 0 or elapsedcpu < 0 or synccpu == 0 or misaligned or
|
if (
|
||||||
simulate.speed_changed):
|
elapsedsim < 0
|
||||||
|
or elapsedcpu < 0
|
||||||
|
or synccpu == 0
|
||||||
|
or misaligned
|
||||||
|
or simulate.speed_changed
|
||||||
|
):
|
||||||
# Re-sync.
|
# Re-sync.
|
||||||
synccpu = startcpu
|
synccpu = startcpu
|
||||||
syncsim = d.time
|
syncsim = d.time
|
||||||
@@ -312,9 +321,9 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
|
|||||||
prevsim = d.time
|
prevsim = d.time
|
||||||
refreshtime = SIM_REFRESH_FRACTION / simulate.refresh_rate
|
refreshtime = SIM_REFRESH_FRACTION / simulate.refresh_rate
|
||||||
# Step while sim lags behind CPU and within refreshtime.
|
# Step while sim lags behind CPU and within refreshtime.
|
||||||
while (((d.time - syncsim) * slowdown <
|
while (
|
||||||
(time.time() - synccpu)) and
|
(d.time - syncsim) * slowdown < (time.time() - synccpu)
|
||||||
((time.time() - startcpu) < refreshtime)):
|
) and ((time.time() - startcpu) < refreshtime):
|
||||||
# Measure slowdown before first step.
|
# Measure slowdown before first step.
|
||||||
if not measured and elapsedsim:
|
if not measured and elapsedsim:
|
||||||
simulate.measured_slowdown = elapsedcpu / elapsedsim
|
simulate.measured_slowdown = elapsedcpu / elapsedsim
|
||||||
@@ -329,7 +338,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
|
|||||||
break
|
break
|
||||||
|
|
||||||
# save current state to history buffer
|
# save current state to history buffer
|
||||||
if (stepped):
|
if stepped:
|
||||||
simulate.add_to_history()
|
simulate.add_to_history()
|
||||||
|
|
||||||
else: # simulate.run is False: GUI is paused.
|
else: # simulate.run is False: GUI is paused.
|
||||||
@@ -355,7 +364,8 @@ def _launch_internal(
|
|||||||
raise ValueError('mjData is specified but mjModel is not')
|
raise ValueError('mjData is specified but mjModel is not')
|
||||||
elif callable(model) and data is not None:
|
elif callable(model) and data is not None:
|
||||||
raise ValueError(
|
raise ValueError(
|
||||||
'mjData should not be specified when an mjModel loader is used')
|
'mjData should not be specified when an mjModel loader is used'
|
||||||
|
)
|
||||||
elif loader is not None and model is not None:
|
elif loader is not None and model is not None:
|
||||||
raise ValueError('model and loader are both specified')
|
raise ValueError('model and loader are both specified')
|
||||||
elif run_physics_thread and handle_return is not None:
|
elif run_physics_thread and handle_return is not None:
|
||||||
@@ -398,14 +408,17 @@ def _launch_internal(
|
|||||||
|
|
||||||
if run_physics_thread:
|
if run_physics_thread:
|
||||||
side_thread = threading.Thread(
|
side_thread = threading.Thread(
|
||||||
target=_physics_loop, args=(simulate, loader))
|
target=_physics_loop, args=(simulate, loader)
|
||||||
|
)
|
||||||
else:
|
else:
|
||||||
side_thread = threading.Thread(
|
side_thread = threading.Thread(
|
||||||
target=_reload, args=(simulate, loader, notify_loaded))
|
target=_reload, args=(simulate, loader, notify_loaded)
|
||||||
|
)
|
||||||
|
|
||||||
def make_exit(simulate):
|
def make_exit(simulate):
|
||||||
def exit_simulate():
|
def exit_simulate():
|
||||||
simulate.exit()
|
simulate.exit()
|
||||||
|
|
||||||
return exit_simulate
|
return exit_simulate
|
||||||
|
|
||||||
exit_simulate = make_exit(simulate)
|
exit_simulate = make_exit(simulate)
|
||||||
@@ -456,8 +469,7 @@ def launch_passive(
|
|||||||
if not isinstance(data, mujoco.MjData):
|
if not isinstance(data, mujoco.MjData):
|
||||||
raise ValueError(f'`data` is not a mujoco.MjData: got {data!r}')
|
raise ValueError(f'`data` is not a mujoco.MjData: got {data!r}')
|
||||||
if key_callback is not None and not callable(key_callback):
|
if key_callback is not None and not callable(key_callback):
|
||||||
raise ValueError(
|
raise ValueError(f'`key_callback` is not callable: got {key_callback!r}')
|
||||||
f'`key_callback` is not callable: got {key_callback!r}')
|
|
||||||
|
|
||||||
mujoco.mj_forward(model, data)
|
mujoco.mj_forward(model, data)
|
||||||
handle_return = queue.Queue(1)
|
handle_return = queue.Queue(1)
|
||||||
@@ -480,7 +492,8 @@ def launch_passive(
|
|||||||
if not isinstance(_MJPYTHON, _MjPythonBase):
|
if not isinstance(_MJPYTHON, _MjPythonBase):
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
'`launch_passive` requires that the Python script be run under '
|
'`launch_passive` requires that the Python script be run under '
|
||||||
'`mjpython` on macOS')
|
'`mjpython` on macOS'
|
||||||
|
)
|
||||||
_MJPYTHON.launch_on_ui_thread(
|
_MJPYTHON.launch_on_ui_thread(
|
||||||
model,
|
model,
|
||||||
data,
|
data,
|
||||||
|
|||||||
@@ -65,3 +65,24 @@ usd = [
|
|||||||
"usd-core",
|
"usd-core",
|
||||||
"pillow"
|
"pillow"
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[tool.isort]
|
||||||
|
force_single_line = true
|
||||||
|
force_sort_within_sections = true
|
||||||
|
lexicographical = true
|
||||||
|
single_line_exclusions = ["typing"]
|
||||||
|
order_by_type = false
|
||||||
|
group_by_package = true
|
||||||
|
line_length = 120
|
||||||
|
use_parentheses = true
|
||||||
|
multi_line_output = 3
|
||||||
|
skip_glob = ["**/*.ipynb"]
|
||||||
|
|
||||||
|
[tool.pyink]
|
||||||
|
line-length = 80
|
||||||
|
unstable = true
|
||||||
|
pyink-indentation = 2
|
||||||
|
pyink-use-majority-quotes = true
|
||||||
|
extend-exclude = '''(
|
||||||
|
.ipynb$
|
||||||
|
)'''
|
||||||
|
|||||||
+63
-40
@@ -101,15 +101,15 @@ def tokenize_quoted_substr(input_string, quote_char, placeholders=None):
|
|||||||
placeholders = placeholders if placeholders is not None else dict()
|
placeholders = placeholders if placeholders is not None else dict()
|
||||||
prev_end = -1
|
prev_end = -1
|
||||||
for start, end in start_and_end(quote_positions):
|
for start, end in start_and_end(quote_positions):
|
||||||
output_string += input_string[prev_end+1:start]
|
output_string += input_string[prev_end + 1 : start]
|
||||||
while True:
|
while True:
|
||||||
placeholder = ''.join(random.choices(string.ascii_lowercase, k=5))
|
placeholder = ''.join(random.choices(string.ascii_lowercase, k=5))
|
||||||
if placeholder not in input_string and placeholder not in output_string:
|
if placeholder not in input_string and placeholder not in output_string:
|
||||||
break
|
break
|
||||||
output_string += placeholder
|
output_string += placeholder
|
||||||
placeholders[placeholder] = input_string[start+1:end]
|
placeholders[placeholder] = input_string[start + 1 : end]
|
||||||
prev_end = end
|
prev_end = end
|
||||||
output_string += input_string[prev_end+1:]
|
output_string += input_string[prev_end + 1 :]
|
||||||
|
|
||||||
return output_string, placeholders
|
return output_string, placeholders
|
||||||
|
|
||||||
@@ -145,15 +145,17 @@ class BuildCMakeExtension(build_ext.build_ext):
|
|||||||
"""Uses CMake to build extensions."""
|
"""Uses CMake to build extensions."""
|
||||||
|
|
||||||
def run(self):
|
def run(self):
|
||||||
self._is_apple = (platform.system() == 'Darwin')
|
self._is_apple = platform.system() == 'Darwin'
|
||||||
(self._mujoco_library_path,
|
(
|
||||||
self._mujoco_include_path,
|
self._mujoco_library_path,
|
||||||
self._mujoco_plugins_path,
|
self._mujoco_include_path,
|
||||||
self._mujoco_framework_path) = self._find_mujoco()
|
self._mujoco_plugins_path,
|
||||||
|
self._mujoco_framework_path,
|
||||||
|
) = self._find_mujoco()
|
||||||
self._configure_cmake()
|
self._configure_cmake()
|
||||||
for ext in self.extensions:
|
for ext in self.extensions:
|
||||||
assert ext.name.startswith(EXT_PREFIX)
|
assert ext.name.startswith(EXT_PREFIX)
|
||||||
assert '.' not in ext.name[len(EXT_PREFIX):]
|
assert '.' not in ext.name[len(EXT_PREFIX) :]
|
||||||
self.build_extension(ext)
|
self.build_extension(ext)
|
||||||
self._copy_external_libraries()
|
self._copy_external_libraries()
|
||||||
self._copy_mujoco_headers()
|
self._copy_mujoco_headers()
|
||||||
@@ -163,20 +165,22 @@ class BuildCMakeExtension(build_ext.build_ext):
|
|||||||
|
|
||||||
def _find_mujoco(self):
|
def _find_mujoco(self):
|
||||||
if MUJOCO_PATH not in os.environ:
|
if MUJOCO_PATH not in os.environ:
|
||||||
raise RuntimeError(
|
raise RuntimeError(f'{MUJOCO_PATH} environment variable is not set')
|
||||||
f'{MUJOCO_PATH} environment variable is not set')
|
|
||||||
if MUJOCO_PLUGIN_PATH not in os.environ:
|
if MUJOCO_PLUGIN_PATH not in os.environ:
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
f'{MUJOCO_PLUGIN_PATH} environment variable is not set')
|
f'{MUJOCO_PLUGIN_PATH} environment variable is not set'
|
||||||
|
)
|
||||||
library_path = None
|
library_path = None
|
||||||
include_path = None
|
include_path = None
|
||||||
plugin_path = os.environ[MUJOCO_PLUGIN_PATH]
|
plugin_path = os.environ[MUJOCO_PLUGIN_PATH]
|
||||||
for directory, subdirs, filenames in os.walk(os.environ[MUJOCO_PATH]):
|
for directory, subdirs, filenames in os.walk(os.environ[MUJOCO_PATH]):
|
||||||
if self._is_apple and 'mujoco.framework' in subdirs:
|
if self._is_apple and 'mujoco.framework' in subdirs:
|
||||||
return (os.path.join(directory, 'mujoco.framework/Versions/A'),
|
return (
|
||||||
os.path.join(directory, 'mujoco.framework/Headers'),
|
os.path.join(directory, 'mujoco.framework/Versions/A'),
|
||||||
plugin_path,
|
os.path.join(directory, 'mujoco.framework/Headers'),
|
||||||
directory)
|
plugin_path,
|
||||||
|
directory,
|
||||||
|
)
|
||||||
if fnmatch.filter(filenames, get_mujoco_lib_pattern()):
|
if fnmatch.filter(filenames, get_mujoco_lib_pattern()):
|
||||||
library_path = directory
|
library_path = directory
|
||||||
if os.path.exists(os.path.join(directory, 'mujoco/mujoco.h')):
|
if os.path.exists(os.path.join(directory, 'mujoco/mujoco.h')):
|
||||||
@@ -190,63 +194,78 @@ class BuildCMakeExtension(build_ext.build_ext):
|
|||||||
for directory, _, filenames in os.walk(os.environ[MUJOCO_PATH]):
|
for directory, _, filenames in os.walk(os.environ[MUJOCO_PATH]):
|
||||||
for pattern in get_external_lib_patterns():
|
for pattern in get_external_lib_patterns():
|
||||||
for filename in fnmatch.filter(filenames, pattern):
|
for filename in fnmatch.filter(filenames, pattern):
|
||||||
shutil.copyfile(os.path.join(directory, filename),
|
shutil.copyfile(
|
||||||
os.path.join(dst, filename))
|
os.path.join(directory, filename), os.path.join(dst, filename)
|
||||||
|
)
|
||||||
|
|
||||||
def _copy_plugin_libraries(self):
|
def _copy_plugin_libraries(self):
|
||||||
dst = os.path.join(
|
dst = os.path.join(
|
||||||
os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)),
|
os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)),
|
||||||
'plugin')
|
'plugin',
|
||||||
|
)
|
||||||
os.makedirs(dst)
|
os.makedirs(dst)
|
||||||
for directory, _, filenames in os.walk(self._mujoco_plugins_path):
|
for directory, _, filenames in os.walk(self._mujoco_plugins_path):
|
||||||
for pattern in get_plugin_lib_patterns():
|
for pattern in get_plugin_lib_patterns():
|
||||||
for filename in fnmatch.filter(filenames, pattern):
|
for filename in fnmatch.filter(filenames, pattern):
|
||||||
shutil.copyfile(os.path.join(directory, filename),
|
shutil.copyfile(
|
||||||
os.path.join(dst, filename))
|
os.path.join(directory, filename), os.path.join(dst, filename)
|
||||||
|
)
|
||||||
|
|
||||||
def _copy_mujoco_headers(self):
|
def _copy_mujoco_headers(self):
|
||||||
dst = os.path.join(
|
dst = os.path.join(
|
||||||
os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)),
|
os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)),
|
||||||
'include/mujoco')
|
'include/mujoco',
|
||||||
|
)
|
||||||
os.makedirs(dst)
|
os.makedirs(dst)
|
||||||
for directory, _, filenames in os.walk(self._mujoco_include_path):
|
for directory, _, filenames in os.walk(self._mujoco_include_path):
|
||||||
for filename in fnmatch.filter(filenames, '*.h'):
|
for filename in fnmatch.filter(filenames, '*.h'):
|
||||||
shutil.copyfile(os.path.join(directory, filename),
|
shutil.copyfile(
|
||||||
os.path.join(dst, filename))
|
os.path.join(directory, filename), os.path.join(dst, filename)
|
||||||
|
)
|
||||||
|
|
||||||
def _copy_mjpython(self):
|
def _copy_mjpython(self):
|
||||||
src_dir = os.path.join(os.path.dirname(__file__), 'mujoco/mjpython')
|
src_dir = os.path.join(os.path.dirname(__file__), 'mujoco/mjpython')
|
||||||
dst_contents_dir = os.path.join(
|
dst_contents_dir = os.path.join(
|
||||||
os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)),
|
os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)),
|
||||||
'MuJoCo_(mjpython).app/Contents')
|
'MuJoCo_(mjpython).app/Contents',
|
||||||
|
)
|
||||||
os.makedirs(dst_contents_dir)
|
os.makedirs(dst_contents_dir)
|
||||||
shutil.copyfile(os.path.join(src_dir, 'Info.plist'),
|
shutil.copyfile(
|
||||||
os.path.join(dst_contents_dir, 'Info.plist'))
|
os.path.join(src_dir, 'Info.plist'),
|
||||||
|
os.path.join(dst_contents_dir, 'Info.plist'),
|
||||||
|
)
|
||||||
|
|
||||||
dst_bin_dir = os.path.join(dst_contents_dir, 'MacOS')
|
dst_bin_dir = os.path.join(dst_contents_dir, 'MacOS')
|
||||||
os.makedirs(dst_bin_dir)
|
os.makedirs(dst_bin_dir)
|
||||||
shutil.copyfile(os.path.join(self.build_temp, 'mjpython'),
|
shutil.copyfile(
|
||||||
os.path.join(dst_bin_dir, 'mjpython'))
|
os.path.join(self.build_temp, 'mjpython'),
|
||||||
|
os.path.join(dst_bin_dir, 'mjpython'),
|
||||||
|
)
|
||||||
os.chmod(os.path.join(dst_bin_dir, 'mjpython'), 0o755)
|
os.chmod(os.path.join(dst_bin_dir, 'mjpython'), 0o755)
|
||||||
|
|
||||||
dst_resources_dir = os.path.join(dst_contents_dir, 'Resources')
|
dst_resources_dir = os.path.join(dst_contents_dir, 'Resources')
|
||||||
os.makedirs(dst_resources_dir)
|
os.makedirs(dst_resources_dir)
|
||||||
shutil.copyfile(os.path.join(src_dir, 'mjpython.icns'),
|
shutil.copyfile(
|
||||||
os.path.join(dst_resources_dir, 'mjpython.icns'))
|
os.path.join(src_dir, 'mjpython.icns'),
|
||||||
|
os.path.join(dst_resources_dir, 'mjpython.icns'),
|
||||||
|
)
|
||||||
|
|
||||||
def _configure_cmake(self):
|
def _configure_cmake(self):
|
||||||
"""Check for CMake."""
|
"""Check for CMake."""
|
||||||
cmake = os.environ.get(MUJOCO_CMAKE, 'cmake')
|
cmake = os.environ.get(MUJOCO_CMAKE, 'cmake')
|
||||||
build_cfg = 'Debug' if self.debug else 'Release'
|
build_cfg = 'Debug' if self.debug else 'Release'
|
||||||
cmake_module_path = os.path.join(
|
cmake_module_path = os.path.join(
|
||||||
os.path.dirname(__file__), 'mujoco', 'cmake')
|
os.path.dirname(__file__), 'mujoco', 'cmake'
|
||||||
|
)
|
||||||
cmake_args = [
|
cmake_args = [
|
||||||
f'-DPython3_ROOT_DIR:PATH={sys.prefix}',
|
f'-DPython3_ROOT_DIR:PATH={sys.prefix}',
|
||||||
f'-DPython3_EXECUTABLE:STRING={sys.executable}',
|
f'-DPython3_EXECUTABLE:STRING={sys.executable}',
|
||||||
f'-DCMAKE_MODULE_PATH:PATH={cmake_module_path}',
|
f'-DCMAKE_MODULE_PATH:PATH={cmake_module_path}',
|
||||||
f'-DCMAKE_BUILD_TYPE:STRING={build_cfg}',
|
f'-DCMAKE_BUILD_TYPE:STRING={build_cfg}',
|
||||||
f'-DCMAKE_LIBRARY_OUTPUT_DIRECTORY:PATH={self.build_temp}',
|
f'-DCMAKE_LIBRARY_OUTPUT_DIRECTORY:PATH={self.build_temp}',
|
||||||
f'-DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL={"OFF" if self.debug else "ON"}',
|
(
|
||||||
|
f'-DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL={"OFF" if self.debug else "ON"}'
|
||||||
|
),
|
||||||
'-DCMAKE_Fortran_COMPILER:STRING=',
|
'-DCMAKE_Fortran_COMPILER:STRING=',
|
||||||
'-DBUILD_TESTING:BOOL=OFF',
|
'-DBUILD_TESTING:BOOL=OFF',
|
||||||
]
|
]
|
||||||
@@ -284,14 +303,17 @@ class BuildCMakeExtension(build_ext.build_ext):
|
|||||||
for arg in cmake_args:
|
for arg in cmake_args:
|
||||||
print(f' {arg}')
|
print(f' {arg}')
|
||||||
subprocess.check_call(
|
subprocess.check_call(
|
||||||
[cmake] + cmake_args +
|
[cmake]
|
||||||
[os.path.join(os.path.dirname(__file__), 'mujoco')],
|
+ cmake_args
|
||||||
cwd=self.build_temp)
|
+ [os.path.join(os.path.dirname(__file__), 'mujoco')],
|
||||||
|
cwd=self.build_temp,
|
||||||
|
)
|
||||||
|
|
||||||
print('Building all extensions with CMake')
|
print('Building all extensions with CMake')
|
||||||
subprocess.check_call(
|
subprocess.check_call(
|
||||||
[cmake, '--build', '.', f'-j{os.cpu_count()}', '--config', build_cfg],
|
[cmake, '--build', '.', f'-j{os.cpu_count()}', '--config', build_cfg],
|
||||||
cwd=self.build_temp)
|
cwd=self.build_temp,
|
||||||
|
)
|
||||||
|
|
||||||
def build_extension(self, ext):
|
def build_extension(self, ext):
|
||||||
dest_path = self.get_ext_fullpath(ext.name)
|
dest_path = self.get_ext_fullpath(ext.name)
|
||||||
@@ -331,6 +353,7 @@ class InstallScripts(install_scripts.install_scripts):
|
|||||||
else:
|
else:
|
||||||
self.outfiles.append(oldfile)
|
self.outfiles.append(oldfile)
|
||||||
|
|
||||||
|
|
||||||
setuptools.setup(
|
setuptools.setup(
|
||||||
long_description=get_long_description(),
|
long_description=get_long_description(),
|
||||||
long_description_content_type='text/markdown',
|
long_description_content_type='text/markdown',
|
||||||
@@ -350,7 +373,7 @@ setuptools.setup(
|
|||||||
CMakeExtension('mujoco._specs'),
|
CMakeExtension('mujoco._specs'),
|
||||||
CMakeExtension('mujoco._structs'),
|
CMakeExtension('mujoco._structs'),
|
||||||
],
|
],
|
||||||
scripts=[
|
scripts=['mujoco/mjpython/mjpython.py']
|
||||||
'mujoco/mjpython/mjpython.py'
|
if platform.system() == 'Darwin'
|
||||||
] if platform.system() == 'Darwin' else [],
|
else [],
|
||||||
)
|
)
|
||||||
|
|||||||
Reference in New Issue
Block a user