Taylor Howell
5ae6b5fe31
PiperOrigin-RevId: 859599919 Change-Id: If785f36010a7437b354ec0ce7db30c2b7ec69fe6
1853 lines
64 KiB
Python
1853 lines
64 KiB
Python
# Copyright 2024 DeepMind Technologies Limited
|
|
#
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
# ==============================================================================
|
|
"""Tests for mjSpec bindings."""
|
|
|
|
import gc
|
|
import inspect
|
|
import math
|
|
import os
|
|
import textwrap
|
|
import typing
|
|
import zipfile # pylint: disable=unused-import
|
|
|
|
from absl.testing import absltest
|
|
from etils import epath
|
|
import mujoco
|
|
import numpy as np
|
|
|
|
|
|
def get_linenumber():
|
|
cf = inspect.currentframe()
|
|
return cf.f_back.f_lineno
|
|
|
|
|
|
class SpecsTest(absltest.TestCase):
|
|
|
|
def test_typing(self):
|
|
spec = mujoco.MjSpec()
|
|
self.assertIsInstance(spec, mujoco.MjSpec)
|
|
self.assertIsInstance(spec.worldbody, mujoco.MjsBody)
|
|
self.assertIsInstance(spec.worldbody, typing.get_args(mujoco.MjStruct))
|
|
|
|
def test_basic(self):
|
|
# Create a spec.
|
|
spec = mujoco.MjSpec()
|
|
|
|
# Check that euler sequence order is set correctly.
|
|
self.assertEqual(spec.compiler.eulerseq[0], 'x')
|
|
spec.compiler.eulerseq = ['z', 'y', 'x']
|
|
self.assertEqual(spec.compiler.eulerseq[0], 'z')
|
|
|
|
# Change single elements of euler sequence.
|
|
spec.compiler.eulerseq[0] = 'y'
|
|
spec.compiler.eulerseq[1] = 'z'
|
|
self.assertEqual(spec.compiler.eulerseq[0], 'y')
|
|
self.assertEqual(spec.compiler.eulerseq[1], 'z')
|
|
|
|
# eulerseq is iterable
|
|
self.assertEqual('yzx', ''.join(spec.compiler.eulerseq))
|
|
|
|
# supports `len`
|
|
self.assertLen(spec.compiler.eulerseq, 3)
|
|
|
|
# field checks for out-of-bound access on read and on write
|
|
with self.assertRaises(IndexError):
|
|
spec.compiler.eulerseq[3] = 'x'
|
|
|
|
with self.assertRaises(IndexError):
|
|
spec.compiler.eulerseq[-1] = 'x'
|
|
|
|
# Add a body, check that it has default orientation.
|
|
body = spec.worldbody.add_body()
|
|
self.assertEqual(body.name, '')
|
|
np.testing.assert_array_equal(body.quat, [1, 0, 0, 0])
|
|
|
|
# Change the name of the body and read it back twice.
|
|
body.name = 'foobar'
|
|
self.assertEqual(body.name, 'foobar')
|
|
body.name = 'baz'
|
|
self.assertEqual(body.name, 'baz')
|
|
|
|
# Change the position of the body and read it back.
|
|
body.pos = [4, 2, 3]
|
|
np.testing.assert_array_equal(body.pos, [4, 2, 3])
|
|
self.assertEqual(body.pos.shape, (3,))
|
|
|
|
# Change single element of position.
|
|
body.pos[0] = 1
|
|
np.testing.assert_array_equal(body.pos, [1, 2, 3])
|
|
|
|
# Change the orientation of the body and read it back.
|
|
body.quat = [0, 1, 0, 0]
|
|
np.testing.assert_array_equal(body.quat, [0, 1, 0, 0])
|
|
self.assertEqual(body.quat.shape, (4,))
|
|
|
|
# Add a site to the body with user data and read it back.
|
|
site = body.add_site()
|
|
site.name = 'sitename'
|
|
site.type = mujoco.mjtGeom.mjGEOM_BOX
|
|
site.userdata = [7, 2, 3, 4, 5, 6]
|
|
self.assertEqual(site.name, 'sitename')
|
|
self.assertEqual(site.type, mujoco.mjtGeom.mjGEOM_BOX)
|
|
np.testing.assert_array_equal(site.userdata, [7, 2, 3, 4, 5, 6])
|
|
|
|
# Modify a single element of userdata.
|
|
site.userdata[0] = 1
|
|
np.testing.assert_array_equal(site.userdata, [1, 2, 3, 4, 5, 6])
|
|
|
|
# Compile the spec and check for expected values in the model.
|
|
model = spec.compile()
|
|
data = mujoco.MjData(model)
|
|
mujoco.mj_forward(model, data)
|
|
self.assertEqual(model.nbody, 2) # 2 bodies, including the world body
|
|
np.testing.assert_array_equal(model.bind(body).pos, [1, 2, 3])
|
|
np.testing.assert_array_equal(model.bind(body).quat, [0, 1, 0, 0])
|
|
np.testing.assert_array_equal(data.bind(body).xpos, [1, 2, 3])
|
|
self.assertEqual(model.nsite, 1)
|
|
self.assertEqual(model.nuser_site, 6)
|
|
np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6])
|
|
|
|
xml = textwrap.dedent("""\
|
|
<mujoco model="MuJoCo Model">
|
|
<compiler angle="radian"/>
|
|
|
|
<size nuser_site="6"/>
|
|
|
|
<worldbody>
|
|
<body name="baz" pos="1 2 3" quat="0 1 0 0">
|
|
<site name="sitename" pos="0 0 0" type="box" user="1 2 3 4 5 6"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
""")
|
|
self.assertEqual(spec.to_xml(), xml)
|
|
|
|
def test_resolve_orientation(self):
|
|
spec = mujoco.MjSpec()
|
|
body = spec.worldbody.add_body(euler=[0, 0, 90])
|
|
quat = mujoco.MjSpec.resolve_orientation(
|
|
degree=spec.compiler.degree,
|
|
sequence=spec.compiler.eulerseq,
|
|
orientation=body.alt,
|
|
)
|
|
np.testing.assert_array_almost_equal(
|
|
quat, [math.sqrt(2) / 2, 0, 0, math.sqrt(2) / 2]
|
|
)
|
|
|
|
def test_kwarg(self):
|
|
# Create a spec.
|
|
spec = mujoco.MjSpec()
|
|
|
|
# Add material.
|
|
material = spec.add_material(texrepeat=[1, 2], emission=-1)
|
|
np.testing.assert_array_equal(material.texrepeat, [1, 2])
|
|
self.assertEqual(material.emission, -1)
|
|
|
|
# Add mesh.
|
|
mesh = spec.add_mesh(refpos=[1, 2, 3])
|
|
np.testing.assert_array_equal(mesh.refpos, [1, 2, 3])
|
|
|
|
# Add pair.
|
|
pair = spec.add_pair(gap=0.1)
|
|
self.assertEqual(pair.gap, 0.1)
|
|
|
|
# Add equality.
|
|
equality = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_SITE)
|
|
self.assertEqual(equality.objtype, mujoco.mjtObj.mjOBJ_SITE)
|
|
|
|
# Add tendon.
|
|
tendon = spec.add_tendon(stiffness=2, springlength=[0.1, 0.2])
|
|
self.assertEqual(tendon.stiffness, 2)
|
|
np.testing.assert_array_equal(tendon.springlength, [0.1, 0.2])
|
|
|
|
# Add actuator.
|
|
actuator = spec.add_actuator(actdim=10, ctrlrange=[-1, 10])
|
|
self.assertEqual(actuator.actdim, 10)
|
|
np.testing.assert_array_equal(actuator.ctrlrange, [-1, 10])
|
|
|
|
# Add skin.
|
|
skin = spec.add_skin(inflate=2.0, vertid=[[1, 1], [2, 2]])
|
|
self.assertEqual(skin.inflate, 2.0)
|
|
np.testing.assert_array_equal(skin.vertid, [[1, 1], [2, 2]])
|
|
|
|
# Add texture.
|
|
texture = spec.add_texture(builtin=0, nchannel=3)
|
|
self.assertEqual(texture.builtin, 0)
|
|
self.assertEqual(texture.nchannel, 3)
|
|
|
|
# Add text.
|
|
text = spec.add_text(data='data', info='info')
|
|
self.assertEqual(text.data, 'data')
|
|
self.assertEqual(text.info, 'info')
|
|
|
|
# Add tuple.
|
|
tuple_ = spec.add_tuple(objprm=[2.0, 3.0, 5.0], objname=['obj'])
|
|
np.testing.assert_array_equal(tuple_.objprm, [2.0, 3.0, 5.0])
|
|
self.assertEqual(tuple_.objname[0], 'obj')
|
|
|
|
# Add flex.
|
|
flex = spec.add_flex(friction=[1, 2, 3], texcoord=[1.0, 2.0, 3.0])
|
|
np.testing.assert_array_equal(flex.friction, [1, 2, 3])
|
|
np.testing.assert_array_equal(flex.texcoord, [1.0, 2.0, 3.0])
|
|
|
|
# Add hfield.
|
|
hfield = spec.add_hfield(nrow=2, content_type='type')
|
|
self.assertEqual(hfield.nrow, 2)
|
|
self.assertEqual(hfield.content_type, 'type')
|
|
|
|
# Add key.
|
|
key = spec.add_key(time=1.2, qpos=[1.0, 2.0])
|
|
self.assertEqual(key.time, 1.2)
|
|
np.testing.assert_array_equal(key.qpos, [1.0, 2.0])
|
|
|
|
# Add numeric.
|
|
numeric = spec.add_numeric(data=[1.0, 1.1, 1.2], size=2)
|
|
np.testing.assert_array_equal(numeric.data, [1.0, 1.1, 1.2])
|
|
self.assertEqual(numeric.size, 2)
|
|
|
|
# Add exclude.
|
|
exclude = spec.add_exclude(bodyname2='body2')
|
|
self.assertEqual(exclude.bodyname2, 'body2')
|
|
|
|
# Add sensor.
|
|
sensor = spec.add_sensor(
|
|
needstage=mujoco.mjtStage.mjSTAGE_ACC, objtype=mujoco.mjtObj.mjOBJ_SITE
|
|
)
|
|
self.assertEqual(sensor.needstage, mujoco.mjtStage.mjSTAGE_ACC)
|
|
self.assertEqual(sensor.objtype, mujoco.mjtObj.mjOBJ_SITE)
|
|
|
|
# Add plugin.
|
|
plugin = spec.add_plugin(
|
|
name='instance_name',
|
|
plugin_name='mujoco.plugin',
|
|
active=True,
|
|
info='info',
|
|
)
|
|
self.assertEqual(plugin.name, 'instance_name')
|
|
self.assertEqual(plugin.plugin_name, 'mujoco.plugin')
|
|
self.assertEqual(plugin.active, True)
|
|
self.assertEqual(plugin.info, 'info')
|
|
|
|
# Add a body.
|
|
body = spec.worldbody.add_body(
|
|
name='body', pos=[1, 2, 3], quat=[0, 0, 0, 1]
|
|
)
|
|
self.assertEqual(body.name, 'body')
|
|
np.testing.assert_array_equal(body.pos, [1, 2, 3])
|
|
np.testing.assert_array_equal(body.quat, [0, 0, 0, 1])
|
|
|
|
# Add a body with a plugin.
|
|
body_with_plugin = spec.worldbody.add_body(plugin=plugin)
|
|
self.assertEqual(body_with_plugin.plugin.name, 'instance_name')
|
|
self.assertEqual(body_with_plugin.plugin.plugin_name, 'mujoco.plugin')
|
|
self.assertEqual(body_with_plugin.plugin.active, True)
|
|
self.assertEqual(body_with_plugin.plugin.info, 'info')
|
|
|
|
# Add a geom.
|
|
geom = body.add_geom(
|
|
name='geom',
|
|
pos=[3, 2, 1],
|
|
fromto=[1, 2, 3, 4, 5, 6],
|
|
contype=3,
|
|
)
|
|
|
|
self.assertEqual(geom.name, 'geom')
|
|
np.testing.assert_array_equal(geom.pos, [3, 2, 1])
|
|
np.testing.assert_array_equal(geom.fromto, [1, 2, 3, 4, 5, 6])
|
|
self.assertEqual(geom.contype, 3)
|
|
|
|
# Add a site to the body with user data and read it back.
|
|
site = body.add_site(
|
|
name='sitename',
|
|
pos=[0, 1, 2],
|
|
quat=[1, 0, 0, 0],
|
|
fromto=[0, 1, 2, 3, 4, 5],
|
|
size=[3, 2, 1],
|
|
type=mujoco.mjtGeom.mjGEOM_BOX,
|
|
material='material',
|
|
group=7,
|
|
rgba=[1, 1, 1, 0.5],
|
|
userdata=[1, 2, 3, 4, 5, 6],
|
|
info='info',
|
|
)
|
|
self.assertEqual(site.name, 'sitename')
|
|
np.testing.assert_array_equal(site.pos, [0, 1, 2])
|
|
np.testing.assert_array_equal(site.quat, [1, 0, 0, 0])
|
|
np.testing.assert_array_equal(site.fromto, [0, 1, 2, 3, 4, 5])
|
|
np.testing.assert_array_equal(site.size, [3, 2, 1])
|
|
self.assertEqual(site.type, mujoco.mjtGeom.mjGEOM_BOX)
|
|
self.assertEqual(site.material, 'material')
|
|
self.assertEqual(site.group, 7)
|
|
np.testing.assert_array_equal(site.rgba, [1, 1, 1, 0.5])
|
|
np.testing.assert_array_equal(site.userdata, [1, 2, 3, 4, 5, 6])
|
|
self.assertEqual(site.info, 'info')
|
|
|
|
# Add camera.
|
|
cam = body.add_camera(
|
|
proj=mujoco.mjtProjection.mjPROJ_ORTHOGRAPHIC, resolution=[10, 20]
|
|
)
|
|
self.assertEqual(cam.proj, 1)
|
|
np.testing.assert_array_equal(cam.resolution, [10, 20])
|
|
|
|
# Add frame.
|
|
framea0 = body.add_frame(name='framea', pos=[1, 2, 3], quat=[0, 0, 0, 1])
|
|
np.testing.assert_array_equal(framea0.pos, [1, 2, 3])
|
|
np.testing.assert_array_equal(framea0.quat, [0, 0, 0, 1])
|
|
|
|
frameb0 = body.add_frame(
|
|
framea0, name='frameb', pos=[4, 5, 6], quat=[0, 1, 0, 0]
|
|
)
|
|
|
|
framea1 = body.first_frame()
|
|
frameb1 = body.next_frame(framea1)
|
|
|
|
self.assertEqual(framea1.name, framea0.name)
|
|
self.assertEqual(frameb1.name, frameb0.name)
|
|
np.testing.assert_array_equal(framea1.pos, framea0.pos)
|
|
np.testing.assert_array_equal(framea1.quat, framea0.quat)
|
|
np.testing.assert_array_equal(frameb1.pos, frameb0.pos)
|
|
np.testing.assert_array_equal(frameb1.quat, frameb0.quat)
|
|
|
|
# Add frame in frame.
|
|
framec0 = frameb0.add_frame(name='framec', pos=[7, 8, 9], quat=[0, 0, 0, 1])
|
|
self.assertEqual(framec0.name, 'framec')
|
|
self.assertEqual(framec0.parent, frameb0.parent)
|
|
self.assertEqual(framec0.frame, frameb0)
|
|
np.testing.assert_array_equal(framec0.pos, [7, 8, 9])
|
|
np.testing.assert_array_equal(framec0.quat, [0, 0, 0, 1])
|
|
|
|
# Add joint.
|
|
joint = body.add_joint(type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0])
|
|
self.assertEqual(joint.type, mujoco.mjtJoint.mjJNT_HINGE)
|
|
np.testing.assert_array_equal(joint.axis, [0, 1, 0])
|
|
|
|
# Add freejoint.
|
|
freejoint = body.add_freejoint()
|
|
self.assertEqual(freejoint.type, mujoco.mjtJoint.mjJNT_FREE)
|
|
freejoint_align = body.add_freejoint(align=True)
|
|
self.assertEqual(freejoint_align.align, True)
|
|
|
|
with self.assertRaises(TypeError) as cm:
|
|
body.add_freejoint(axis=[1, 2, 3])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'Invalid axis keyword argument. Valid options are: align, group, name.',
|
|
)
|
|
|
|
# Add light.
|
|
light = body.add_light(attenuation=[1, 2, 3])
|
|
np.testing.assert_array_equal(light.attenuation, [1, 2, 3])
|
|
|
|
# Add light in a frame.
|
|
light_in_frame = framea0.add_light(cutoff=10)
|
|
self.assertEqual(light_in_frame.cutoff, 10)
|
|
self.assertEqual(light_in_frame.parent, framea0.parent)
|
|
self.assertEqual(light_in_frame.frame, framea0)
|
|
|
|
# Invalid input for valid keyword argument.
|
|
with self.assertRaises(TypeError):
|
|
body.add_geom(pos='pos')
|
|
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(pos=[0, 1])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'pos should be a list/array of size 3.',
|
|
)
|
|
|
|
with self.assertRaises(TypeError):
|
|
body.add_geom(type='type')
|
|
|
|
with self.assertRaises(TypeError):
|
|
body.add_geom(userdata='')
|
|
|
|
# Invalid keyword argument.
|
|
with self.assertRaises(TypeError):
|
|
body.add_geom(vel='vel')
|
|
|
|
# Orientation keyword arguments.
|
|
geom_axisangle = body.add_geom(axisangle=[1, 2, 3, 4])
|
|
geom_xyaxes = body.add_geom(xyaxes=[1, 2, 3, 4, 5, 6])
|
|
geom_zaxis = body.add_geom(zaxis=[1, 2, 3])
|
|
geom_euler = body.add_geom(euler=[1, 2, 3])
|
|
|
|
self.assertEqual(
|
|
geom_axisangle.alt.type, mujoco.mjtOrientation.mjORIENTATION_AXISANGLE
|
|
)
|
|
self.assertEqual(
|
|
geom_xyaxes.alt.type, mujoco.mjtOrientation.mjORIENTATION_XYAXES
|
|
)
|
|
self.assertEqual(
|
|
geom_zaxis.alt.type, mujoco.mjtOrientation.mjORIENTATION_ZAXIS
|
|
)
|
|
self.assertEqual(
|
|
geom_euler.alt.type, mujoco.mjtOrientation.mjORIENTATION_EULER
|
|
)
|
|
np.testing.assert_array_equal(geom_axisangle.alt.axisangle, [1, 2, 3, 4])
|
|
np.testing.assert_array_equal(geom_xyaxes.alt.xyaxes, [1, 2, 3, 4, 5, 6])
|
|
np.testing.assert_array_equal(geom_zaxis.alt.zaxis, [1, 2, 3])
|
|
np.testing.assert_array_equal(geom_euler.alt.euler, [1, 2, 3])
|
|
|
|
body_iaxisangle = spec.worldbody.add_body(iaxisangle=[1, 2, 3, 4])
|
|
body_ixyaxes = spec.worldbody.add_body(ixyaxes=[1, 2, 3, 4, 5, 6])
|
|
body_izaxis = spec.worldbody.add_body(izaxis=[1, 2, 3])
|
|
body_ieuler = spec.worldbody.add_body(ieuler=[1, 2, 3])
|
|
body_euler_ieuler = spec.worldbody.add_body(
|
|
euler=[1, 2, 3], ieuler=[4, 5, 6]
|
|
)
|
|
np.testing.assert_array_equal(body_iaxisangle.ialt.axisangle, [1, 2, 3, 4])
|
|
np.testing.assert_array_equal(body_ixyaxes.ialt.xyaxes, [1, 2, 3, 4, 5, 6])
|
|
np.testing.assert_array_equal(body_izaxis.ialt.zaxis, [1, 2, 3])
|
|
np.testing.assert_array_equal(body_ieuler.ialt.euler, [1, 2, 3])
|
|
np.testing.assert_array_equal(body_euler_ieuler.alt.euler, [1, 2, 3])
|
|
np.testing.assert_array_equal(body_euler_ieuler.ialt.euler, [4, 5, 6])
|
|
|
|
# Test invalid orientation keyword arguments.
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(axisangle=[1, 2, 3])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'axisangle should be a list/array of size 4.',
|
|
)
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(xyaxes=[1, 2, 3, 4, 5])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'xyaxes should be a list/array of size 6.',
|
|
)
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(zaxis=[1, 2, 3, 4])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'zaxis should be a list/array of size 3.',
|
|
)
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(euler=[1])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'euler should be a list/array of size 3.',
|
|
)
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(axisangle=[1, 2, 3, 4], euler=[1, 2, 3])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'Only one of: axisangle, xyaxes, zaxis, or euler can be set.',
|
|
)
|
|
with self.assertRaises(ValueError) as cm:
|
|
spec.worldbody.add_body(iaxisangle=[1, 2, 3, 4], ieuler=[1, 2, 3])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'Only one of: iaxisangle, ixyaxes, izaxis, or ieuler can be set.',
|
|
)
|
|
|
|
def test_size_kwarg_variable_length(self):
|
|
spec = mujoco.MjSpec()
|
|
body = spec.worldbody.add_body()
|
|
|
|
geom_size1 = body.add_geom(size=[0.5])
|
|
np.testing.assert_array_equal(geom_size1.size, [0.5, 0, 0])
|
|
|
|
geom_size2 = body.add_geom(size=[0.5, 0.3])
|
|
np.testing.assert_array_equal(geom_size2.size, [0.5, 0.3, 0])
|
|
|
|
geom_size3 = body.add_geom(size=[0.5, 0.3, 0.1])
|
|
np.testing.assert_array_equal(geom_size3.size, [0.5, 0.3, 0.1])
|
|
|
|
site_size1 = body.add_site(size=[0.2])
|
|
np.testing.assert_array_equal(site_size1.size, [0.2, 0, 0])
|
|
|
|
site_size2 = body.add_site(size=[0.2, 0.1])
|
|
np.testing.assert_array_equal(site_size2.size, [0.2, 0.1, 0])
|
|
|
|
site_size3 = body.add_site(size=[0.2, 0.1, 0.05])
|
|
np.testing.assert_array_equal(site_size3.size, [0.2, 0.1, 0.05])
|
|
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(size=[])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'size should be a list/array of size 1, 2, or 3.',
|
|
)
|
|
|
|
with self.assertRaises(ValueError) as cm:
|
|
body.add_geom(size=[1, 2, 3, 4])
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'size should be a list/array of size 1, 2, or 3.',
|
|
)
|
|
|
|
def test_load_xml(self):
|
|
file_path = epath.resource_path("mujoco") / "testdata" / "model.xml"
|
|
filename = file_path.as_posix()
|
|
state_type = mujoco.mjtState.mjSTATE_INTEGRATION
|
|
|
|
# Load from file.
|
|
spec1 = mujoco.MjSpec.from_file(filename)
|
|
model1 = spec1.compile()
|
|
data1 = mujoco.MjData(model1)
|
|
mujoco.mj_step(model1, data1)
|
|
size1 = mujoco.mj_stateSize(model1, state_type)
|
|
state1 = np.empty(size1, np.float64)
|
|
mujoco.mj_getState(model1, data1, state1, state_type)
|
|
|
|
# Load from string.
|
|
with open(filename, 'r') as file:
|
|
spec2 = mujoco.MjSpec.from_string(file.read().rstrip())
|
|
model2 = spec2.compile()
|
|
data2 = mujoco.MjData(model2)
|
|
mujoco.mj_step(model2, data2)
|
|
size2 = mujoco.mj_stateSize(model2, state_type)
|
|
state2 = np.empty(size2, np.float64)
|
|
mujoco.mj_getState(model2, data2, state2, state_type)
|
|
|
|
# Check that the state is the same.
|
|
np.testing.assert_array_equal(state1, state2)
|
|
|
|
def test_parses_urdf(self):
|
|
file_path = epath.resource_path("mujoco") / "testdata" / "model.urdf"
|
|
filename = file_path.as_posix()
|
|
|
|
# Load from file.
|
|
spec1 = mujoco.MjSpec.from_file(filename)
|
|
self.assertIsNotNone(spec1)
|
|
|
|
def test_make_mesh(self):
|
|
spec = mujoco.MjSpec()
|
|
|
|
mesh = spec.add_mesh(name='wedge')
|
|
mesh.make_wedge(resolution=[25, 25], fov=[90, 45], gamma=0)
|
|
|
|
mesh = spec.add_mesh(name='prism')
|
|
mesh.make_cone(nedge=5, radius=1)
|
|
|
|
mesh = spec.add_mesh(name='cone')
|
|
mesh.make_cone(nedge=6, radius=0)
|
|
|
|
mesh = spec.add_mesh(name='hemisphere')
|
|
mesh.make_hemisphere(resolution=4)
|
|
|
|
mesh = spec.add_mesh(name='sphere')
|
|
mesh.make_sphere(subdivision=2)
|
|
|
|
mesh = spec.add_mesh(name='supertorus')
|
|
mesh.make_supertorus(resolution=10, radius=0.5, s=1, t=1)
|
|
|
|
mesh = spec.add_mesh(name='supersphere')
|
|
mesh.make_supersphere(resolution=20, e=2, n=1)
|
|
|
|
model = spec.compile()
|
|
self.assertEqual(model.nmesh, 7)
|
|
self.assertEqual(model.mesh_vertnum[0], 25 * 25)
|
|
self.assertEqual(model.mesh_vertnum[1], 10)
|
|
self.assertEqual(model.mesh_vertnum[2], 7)
|
|
self.assertEqual(model.mesh_vertnum[3], 2 * (4 + 1) * (4 + 2) + 2)
|
|
self.assertEqual(model.mesh_vertnum[4], 2 + 10 * 4**2)
|
|
self.assertEqual(model.mesh_vertnum[5], 100)
|
|
self.assertEqual(model.mesh_vertnum[6], 20 * (20 -1) + 2)
|
|
|
|
def test_compile_errors_with_line_info(self):
|
|
spec = mujoco.MjSpec()
|
|
|
|
added_on_line = get_linenumber() + 1
|
|
geom = spec.worldbody.add_geom()
|
|
geom.name = 'MyGeom'
|
|
geom.info = f'geom added on line {added_on_line}'
|
|
|
|
# Try to compile, get error.
|
|
expected_error = (
|
|
'Error: size 0 must be positive in geom\n'
|
|
+ f"Element name 'MyGeom', id 0, geom added on line {added_on_line}"
|
|
)
|
|
with self.assertRaisesRegex(ValueError, expected_error):
|
|
spec.compile()
|
|
|
|
def test_recompile(self):
|
|
# Create a spec.
|
|
spec = mujoco.MjSpec()
|
|
|
|
# Add movable body1.
|
|
body1 = spec.worldbody.add_body()
|
|
geom = body1.add_geom()
|
|
geom.size[0] = 1
|
|
geom.pos = [1, 1, 0]
|
|
joint = body1.add_joint()
|
|
joint.type = mujoco.mjtJoint.mjJNT_BALL
|
|
|
|
# Compile model, make data.
|
|
model = spec.compile()
|
|
data = mujoco.MjData(model)
|
|
|
|
# Simulate for 1 second.
|
|
while data.time < 1:
|
|
mujoco.mj_step(model, data)
|
|
|
|
# Add movable body2.
|
|
body2 = spec.worldbody.add_body()
|
|
body2.pos[1] = 3
|
|
geom = body2.add_geom()
|
|
geom.size[0] = 1
|
|
geom.pos = [0, 1, 0]
|
|
joint = body2.add_joint()
|
|
joint.type = mujoco.mjtJoint.mjJNT_BALL
|
|
|
|
# Recompile model and data while maintaining the state.
|
|
model_new, data_new = spec.recompile(model, data)
|
|
|
|
# Check that the state is preserved.
|
|
np.testing.assert_array_equal(model_new.body_pos[1], model.body_pos[1])
|
|
np.testing.assert_array_equal(data_new.qpos[:4], data.qpos)
|
|
np.testing.assert_array_equal(data_new.qvel[:3], data.qvel)
|
|
|
|
def test_uncompiled_spec_can_be_written(self):
|
|
spec = mujoco.MjSpec()
|
|
spec.to_xml()
|
|
|
|
def test_modelname_default_class(self):
|
|
xml = textwrap.dedent("""\
|
|
<mujoco model="test">
|
|
<compiler angle="radian"/>
|
|
|
|
<default>
|
|
<geom size="2 0 0"/>
|
|
<default class="def1">
|
|
<geom size="3 0 0"/>
|
|
</default>
|
|
</default>
|
|
|
|
<worldbody>
|
|
<geom class="def1"/>
|
|
<geom/>
|
|
</worldbody>
|
|
</mujoco>
|
|
""")
|
|
|
|
spec = mujoco.MjSpec()
|
|
spec.modelname = 'test'
|
|
|
|
main = spec.default
|
|
main.geom.size[0] = 2
|
|
|
|
def1 = spec.add_default('def1', main)
|
|
def1.geom.size[0] = 3
|
|
|
|
spec.worldbody.add_geom(def1)
|
|
spec.worldbody.add_geom(main)
|
|
|
|
spec.compile()
|
|
self.assertEqual(spec.to_xml(), xml)
|
|
spec = mujoco.MjSpec()
|
|
spec.modelname = 'test'
|
|
|
|
main = spec.default
|
|
main.geom.size[0] = 2
|
|
def1 = spec.add_default('def1', main)
|
|
def1.geom.size[0] = 3
|
|
|
|
geom1 = spec.worldbody.add_geom(def1)
|
|
geom2 = spec.worldbody.add_geom()
|
|
self.assertEqual(geom1.classname.name, 'def1')
|
|
self.assertEqual(geom2.classname.name, 'main')
|
|
|
|
spec.compile()
|
|
self.assertEqual(spec.to_xml(), xml)
|
|
|
|
spec = mujoco.MjSpec()
|
|
spec.modelname = 'test'
|
|
|
|
main = spec.default
|
|
main.geom.size[0] = 2
|
|
def1 = spec.add_default('def1', main)
|
|
def1.geom.size[0] = 3
|
|
|
|
geom1 = spec.worldbody.add_geom(size=[3, 0, 0])
|
|
geom2 = spec.worldbody.add_geom(size=[2, 0, 0])
|
|
geom1.classname = def1
|
|
geom2.classname = main # actually redundant, since main is always applied
|
|
|
|
spec.compile()
|
|
self.assertEqual(spec.to_xml(), xml)
|
|
|
|
# test delete default
|
|
def1 = spec.find_default('def1')
|
|
spec.delete(def1)
|
|
def1 = spec.find_default('def1')
|
|
self.assertIsNone(def1)
|
|
|
|
def test_element_list(self):
|
|
spec = mujoco.MjSpec()
|
|
sensor1 = spec.add_sensor()
|
|
sensor2 = spec.add_sensor()
|
|
sensor3 = spec.add_sensor()
|
|
actuator1 = spec.add_actuator()
|
|
actuator2 = spec.add_actuator()
|
|
actuator3 = spec.add_actuator()
|
|
sensor1.name = 'sensor1'
|
|
sensor2.name = 'sensor2'
|
|
sensor3.name = 'sensor3'
|
|
actuator1.name = 'actuator1'
|
|
actuator2.name = 'actuator2'
|
|
actuator3.name = 'actuator3'
|
|
self.assertLen(spec.sensors, 3)
|
|
self.assertLen(spec.actuators, 3)
|
|
self.assertEqual(spec.sensors[0].name, 'sensor1')
|
|
self.assertEqual(spec.sensors[1].name, 'sensor2')
|
|
self.assertEqual(spec.sensors[2].name, 'sensor3')
|
|
self.assertEqual(spec.actuators[0].name, 'actuator1')
|
|
self.assertEqual(spec.actuators[1].name, 'actuator2')
|
|
self.assertEqual(spec.actuators[2].name, 'actuator3')
|
|
self.assertEqual(spec.sensor('sensor1'), sensor1)
|
|
self.assertEqual(spec.sensor('sensor2'), sensor2)
|
|
self.assertEqual(spec.sensor('sensor3'), sensor3)
|
|
self.assertEqual(spec.actuator('actuator1'), actuator1)
|
|
self.assertEqual(spec.actuator('actuator2'), actuator2)
|
|
self.assertEqual(spec.actuator('actuator3'), actuator3)
|
|
|
|
def test_body_list(self):
|
|
main_xml = """
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="body1">
|
|
<site name="site1"/>
|
|
<body name="body2">
|
|
<site name="site4"/>
|
|
</body>
|
|
<geom name="geom1" size="1"/>
|
|
<geom name="geom2" size="1"/>
|
|
<site name="site2"/>
|
|
<site name="site3"/>
|
|
<geom name="geom3" size="1"/>
|
|
</body>
|
|
<body name="body3">
|
|
<site name="site5"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
"""
|
|
spec = mujoco.MjSpec.from_string(main_xml)
|
|
bodytype = mujoco.mjtObj.mjOBJ_BODY
|
|
self.assertLen(spec.bodies, 4)
|
|
self.assertLen(spec.sites, 5)
|
|
self.assertLen(spec.worldbody.find_all('body'), 3)
|
|
self.assertLen(spec.worldbody.find_all('site'), 5)
|
|
self.assertLen(spec.worldbody.find_all('geom'), 3)
|
|
self.assertEqual(spec.bodies[1].name, 'body1')
|
|
self.assertEqual(spec.bodies[2].name, 'body2')
|
|
self.assertEqual(spec.bodies[3].name, 'body3')
|
|
self.assertEqual(spec.bodies[1].parent, spec.bodies[0])
|
|
self.assertEqual(spec.bodies[2].parent, spec.bodies[1])
|
|
self.assertEqual(spec.bodies[3].parent, spec.bodies[0])
|
|
self.assertLen(spec.worldbody.find_all(bodytype), 3)
|
|
self.assertLen(spec.bodies[1].find_all(bodytype), 1)
|
|
self.assertEmpty(spec.bodies[3].find_all(bodytype))
|
|
self.assertEqual(spec.bodies[1].find_all('body')[0].name, 'body2')
|
|
self.assertEmpty(spec.bodies[3].find_all('body'))
|
|
self.assertEmpty(spec.bodies[2].find_all('body'))
|
|
for i, body in enumerate(spec.worldbody.find_all('body')):
|
|
self.assertEqual(body.name, 'body' + str(i + 1))
|
|
for i, site in enumerate(spec.worldbody.find_all('site')):
|
|
self.assertEqual(site.name, 'site' + str(i + 1))
|
|
self.assertLen(spec.bodies[1].sites, 3)
|
|
self.assertLen(spec.bodies[2].sites, 1)
|
|
self.assertLen(spec.bodies[3].sites, 1)
|
|
self.assertEqual(spec.bodies[1].sites[0].name, 'site1')
|
|
self.assertEqual(spec.bodies[1].sites[1].name, 'site2')
|
|
self.assertEqual(spec.bodies[1].sites[2].name, 'site3')
|
|
self.assertEqual(spec.bodies[2].sites[0].name, 'site4')
|
|
self.assertEqual(spec.bodies[3].sites[0].name, 'site5')
|
|
for body in spec.bodies:
|
|
for site in body.sites:
|
|
self.assertEqual(site.parent, body)
|
|
with self.assertRaises(ValueError) as cm:
|
|
spec.worldbody.find_all('actuator')
|
|
self.assertEqual(
|
|
str(cm.exception),
|
|
'body.find_all supports the types: body, frame, geom, site,'
|
|
' joint, light, camera.',
|
|
)
|
|
body3 = spec.worldbody.find_all('body')[2]
|
|
body3.name = 'body3_new'
|
|
self.assertEqual(spec.bodies[3].name, 'body3_new')
|
|
|
|
def test_geom_list(self):
|
|
main_xml = """
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="body1"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
"""
|
|
spec = mujoco.MjSpec.from_string(main_xml)
|
|
geom1 = spec.worldbody.add_geom(name='geom1')
|
|
geom2 = spec.worldbody.add_geom(name='geom2')
|
|
geom3 = spec.body('body1').add_geom(name='geom3')
|
|
|
|
self.assertEqual(spec.geoms, [geom1, geom2, geom3])
|
|
self.assertEqual(spec.geom('geom1'), geom1)
|
|
self.assertEqual(spec.geom('geom2'), geom2)
|
|
self.assertEqual(spec.geom('geom3'), geom3)
|
|
|
|
def test_iterators(self):
|
|
spec = mujoco.MjSpec()
|
|
geom1 = spec.worldbody.add_geom()
|
|
geom2 = spec.worldbody.add_geom()
|
|
geom3 = spec.worldbody.add_geom()
|
|
geom1.name = 'geom1'
|
|
geom2.name = 'geom2'
|
|
geom3.name = 'geom3'
|
|
geom = spec.worldbody.first_geom()
|
|
i = 1
|
|
while geom:
|
|
self.assertEqual(geom.name, 'geom' + str(i))
|
|
geom = spec.worldbody.next_geom(geom)
|
|
i += 1
|
|
|
|
def test_assets(self):
|
|
cube = """
|
|
v -1 -1 1
|
|
v 1 -1 1
|
|
v -1 1 1
|
|
v 1 1 1
|
|
v -1 1 -1
|
|
v 1 1 -1
|
|
v -1 -1 -1
|
|
v 1 -1 -1"""
|
|
spec = mujoco.MjSpec()
|
|
spec.modelname = 'test'
|
|
mesh = spec.add_mesh()
|
|
mesh.name = 'cube'
|
|
mesh.file = 'cube.obj'
|
|
geom = spec.worldbody.add_geom()
|
|
geom.type = mujoco.mjtGeom.mjGEOM_MESH
|
|
geom.meshname = 'cube'
|
|
spec.assets = {'cube.obj': cube}
|
|
model = spec.compile()
|
|
self.assertEqual(model.nmeshvert, 8)
|
|
self.assertEqual(spec.assets['cube.obj'], cube)
|
|
self.assertIs(
|
|
spec.assets['cube.obj'], cube,
|
|
'Asset dict should contain a reference, not a copy'
|
|
)
|
|
|
|
xml = """
|
|
<mujoco model="test">
|
|
<compiler angle="radian"/>
|
|
|
|
<asset>
|
|
<mesh name="cube" file="cube.obj"/>
|
|
</asset>
|
|
|
|
<worldbody>
|
|
<geom type="mesh" mesh="cube"/>
|
|
</worldbody>
|
|
</mujoco>
|
|
"""
|
|
assets = {'cube.obj': cube}
|
|
spec = mujoco.MjSpec.from_string(xml, assets=assets)
|
|
model = spec.compile()
|
|
self.assertEqual(model.nmeshvert, 8)
|
|
self.assertEqual(spec.assets['cube.obj'], cube)
|
|
self.assertIs(
|
|
spec.assets['cube.obj'], cube,
|
|
'Asset dict should contain a reference, not a copy'
|
|
)
|
|
del assets
|
|
gc.collect()
|
|
self.assertEqual(spec.assets['cube.obj'], cube)
|
|
|
|
def test_include(self):
|
|
included_xml = """
|
|
<mujoco>
|
|
<worldbody>
|
|
<body>
|
|
<geom type="box" size="1 1 1"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
"""
|
|
spec = mujoco.MjSpec.from_string(
|
|
textwrap.dedent("""
|
|
<mujoco model="MuJoCo Model">
|
|
<include file="included.xml"/>
|
|
</mujoco>
|
|
"""),
|
|
include={'included.xml': included_xml.encode('utf-8')},
|
|
)
|
|
self.assertEqual(
|
|
spec.worldbody.first_body().first_geom().type, mujoco.mjtGeom.mjGEOM_BOX
|
|
)
|
|
|
|
def test_delete(self):
|
|
file_path = epath.resource_path("mujoco") / "testdata" / "model.xml"
|
|
spec = mujoco.MjSpec.from_file(file_path.as_posix())
|
|
|
|
model = spec.compile()
|
|
self.assertIsNotNone(model)
|
|
self.assertEqual(model.nsite, 11)
|
|
self.assertEqual(model.nsensor, 11)
|
|
|
|
head = spec.body('head')
|
|
self.assertIsNotNone(head)
|
|
site = head.first_site()
|
|
self.assertIsNotNone(site)
|
|
self.assertEqual(site, spec.site('head'))
|
|
|
|
spec.delete(site)
|
|
spec.delete(spec.sensors[-1])
|
|
spec.delete(spec.sensors[-1])
|
|
|
|
model = spec.compile()
|
|
self.assertIsNotNone(model)
|
|
self.assertEqual(model.nsite, 10)
|
|
self.assertEqual(model.nsensor, 9)
|
|
|
|
def test_plugin(self):
|
|
spec = mujoco.MjSpec()
|
|
spec.activate_plugin('mujoco.elasticity.cable')
|
|
plugin = spec.add_plugin(
|
|
name='instance_name',
|
|
plugin_name='mujoco.elasticity.cable',
|
|
active=True,
|
|
info='info'
|
|
)
|
|
plugin.config = {'twist': '10', 'bend': '1'}
|
|
self.assertEqual(plugin.config, {'twist': '10', 'bend': '1'})
|
|
|
|
body = spec.worldbody.add_body()
|
|
body.plugin = plugin
|
|
body.plugin.name = 'instance_name'
|
|
body.plugin.active = True
|
|
|
|
geom = body.add_geom()
|
|
geom.type = mujoco.mjtGeom.mjGEOM_BOX
|
|
geom.size[0] = 1
|
|
geom.size[1] = 1
|
|
geom.size[2] = 1
|
|
|
|
model = spec.compile()
|
|
self.assertIsNotNone(model)
|
|
self.assertEqual(model.nplugin, 1)
|
|
self.assertEqual(model.npluginattr, 7)
|
|
self.assertEqual(model.body_plugin[1], 0)
|
|
attributes = (''.join([chr(i) for i in model.plugin_attr]).split(chr(0)))
|
|
self.assertEqual(attributes[:2], ['10', '1'])
|
|
|
|
copy = spec.copy() # before assigning the new config
|
|
wrong_config = {'wrong': '10', 'bend': '1'}
|
|
for s in [spec, copy]:
|
|
s.plugins[0].config = wrong_config
|
|
with self.assertRaisesRegex(
|
|
ValueError, "Error: unrecognized attribute 'plugin:wrong'"
|
|
):
|
|
s.compile()
|
|
|
|
def test_duplicate_name_error(self):
|
|
main_xml = """
|
|
<mujoco>
|
|
<worldbody>
|
|
<body>
|
|
<geom size="0.1"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
"""
|
|
|
|
spec = mujoco.MjSpec.from_string(main_xml)
|
|
spec.add_material().name = 'yellow'
|
|
with self.assertRaisesRegex(
|
|
ValueError, "Error: repeated name 'yellow' in material"
|
|
):
|
|
spec.add_material().name = 'yellow'
|
|
|
|
def test_delete_unused_plugin(self):
|
|
spec = mujoco.MjSpec.from_string("""
|
|
<mujoco model="MuJoCo Model">
|
|
<extension>
|
|
<plugin plugin="mujoco.pid">
|
|
<instance name="pid1">
|
|
<config key="kp" value="4.0"/>
|
|
</instance>
|
|
</plugin>
|
|
</extension>
|
|
|
|
<worldbody>
|
|
<body>
|
|
<geom size="1"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
""")
|
|
plugin = spec.plugins[0]
|
|
self.assertIsNotNone(plugin)
|
|
spec.delete(plugin)
|
|
|
|
model = spec.compile()
|
|
self.assertIsNotNone(model)
|
|
self.assertEqual(model.nplugin, 0)
|
|
|
|
def test_access_option_stat_visual(self):
|
|
spec = mujoco.MjSpec.from_string("""
|
|
<mujoco model="MuJoCo Model">
|
|
<option timestep="0.001"/>
|
|
<statistic meansize="0.05"/>
|
|
<visual>
|
|
<quality shadowsize="4096"/>
|
|
<headlight active="0"/>
|
|
<rgba camera="0 0 0 0"/>
|
|
</visual>
|
|
</mujoco>
|
|
""")
|
|
self.assertEqual(spec.option.timestep, 0.001)
|
|
self.assertEqual(spec.stat.meansize, 0.05)
|
|
self.assertEqual(spec.visual.quality.shadowsize, 4096)
|
|
self.assertEqual(spec.visual.headlight.active, 0)
|
|
self.assertEqual(spec.visual.global_, getattr(spec.visual, 'global'))
|
|
np.testing.assert_array_equal(spec.visual.rgba.camera, [0, 0, 0, 0])
|
|
|
|
spec.option.timestep = 0.002
|
|
spec.stat.meansize = 0.06
|
|
spec.visual.quality.shadowsize = 8192
|
|
spec.visual.headlight.active = 1
|
|
spec.visual.rgba.camera = [1, 1, 1, 1]
|
|
|
|
model = spec.compile()
|
|
|
|
self.assertEqual(model.opt.timestep, 0.002)
|
|
self.assertEqual(model.stat.meansize, 0.06)
|
|
self.assertEqual(model.vis.quality.shadowsize, 8192)
|
|
self.assertEqual(model.vis.headlight.active, 1)
|
|
np.testing.assert_array_equal(model.vis.rgba.camera, [1, 1, 1, 1])
|
|
|
|
def test_assign_list_element(self):
|
|
spec = mujoco.MjSpec()
|
|
material = spec.add_material()
|
|
texture_index = mujoco.mjtTextureRole.mjTEXROLE_RGB
|
|
|
|
# Assign a string to a list element.
|
|
material.textures[texture_index] = 'texture_name'
|
|
self.assertEqual(material.textures[texture_index], 'texture_name')
|
|
|
|
# Assign a complete list (must be mjNTEXROLE = 10 elements)
|
|
material.textures = ['', 'new_name', '', '', '', '', '', '', '', '']
|
|
self.assertEqual(material.textures[texture_index], 'new_name')
|
|
|
|
# textures is iterable
|
|
self.assertEqual('new_name', ''.join(material.textures))
|
|
|
|
# supports `len` - should always be mjNTEXROLE (10)
|
|
self.assertLen(material.textures, mujoco.mjtTextureRole.mjNTEXROLE)
|
|
|
|
# field checks for out-of-bound access on read and on write
|
|
with self.assertRaises(IndexError):
|
|
material.textures[10] = 'x'
|
|
|
|
with self.assertRaises(IndexError):
|
|
material.textures[-1] = 'x'
|
|
|
|
def test_textures(self):
|
|
"""Tests that partial texture list assignment raises ValueError."""
|
|
|
|
spec = mujoco.MjSpec()
|
|
material = spec.add_material(name='mat')
|
|
texture = spec.add_texture(
|
|
name='tex', builtin=mujoco.mjtBuiltin.mjBUILTIN_FLAT, width=2, height=2
|
|
)
|
|
|
|
# Should raise ValueError for incorrect size (only 1 element instead of 10)
|
|
with self.assertRaises(ValueError) as cm:
|
|
material.textures = ['tex']
|
|
self.assertIn('must have exactly 10 elements', str(cm.exception))
|
|
self.assertIn('got 1', str(cm.exception))
|
|
|
|
# Should succeed with correct size (mjNTEXROLE = 10)
|
|
material.textures = ['tex', '', '', '', '', '', '', '', '', '']
|
|
spec.worldbody.add_geom(size=[0.1, 0.1, 0.1], material='mat')
|
|
model = spec.compile()
|
|
self.assertIsNotNone(model)
|
|
|
|
def test_assign_texture(self):
|
|
spec = mujoco.MjSpec()
|
|
texture = spec.add_texture(name='texture', height=2, width=2)
|
|
texture.data = np.zeros((2, 2, 3), dtype=np.uint8).tobytes()
|
|
spec.compile()
|
|
|
|
def test_read_texture(self):
|
|
spec = mujoco.MjSpec()
|
|
texture = spec.add_texture(name='texture', height=1, width=2, nchannel=3)
|
|
texture.data = bytes([1, 2, 3, 4, 5, 6])
|
|
read_bytes = texture.data
|
|
self.assertEqual(read_bytes, bytes([1, 2, 3, 4, 5, 6]))
|
|
|
|
def test_modify_texture(self):
|
|
# Assign red, green and blue pixels, then make the first pixel yellow.
|
|
spec = mujoco.MjSpec()
|
|
texture = spec.add_texture(name='texture', height=1, width=3, nchannel=3)
|
|
texture.data = bytes([255, 0, 0, 0, 255, 0, 0, 0, 255])
|
|
data_array = bytearray(texture.data)
|
|
data_array[1] = 255
|
|
texture.data = bytes(data_array)
|
|
self.assertEqual(
|
|
texture.data, bytes([255, 255, 0, 0, 255, 0, 0, 0, 255])
|
|
)
|
|
|
|
# Assigning values outside the range [0, 255] should raise an error.
|
|
with self.assertRaises(ValueError):
|
|
data_array[0] = 256
|
|
with self.assertRaises(ValueError):
|
|
data_array[0] = -1
|
|
|
|
def test_find_unnamed_asset(self):
|
|
spec = mujoco.MjSpec()
|
|
texture_file = spec.add_texture(file='file.png')
|
|
texture_name = spec.add_texture(name='name')
|
|
mesh_file = spec.add_mesh(file='file.obj')
|
|
mesh_name = spec.add_mesh(name='mesh')
|
|
self.assertEqual(spec.texture('file'), texture_file)
|
|
self.assertEqual(spec.texture('name'), texture_name)
|
|
self.assertEqual(spec.mesh('file'), mesh_file)
|
|
self.assertEqual(spec.mesh('mesh'), mesh_name)
|
|
self.assertIsNone(spec.texture('none'))
|
|
self.assertIsNone(spec.mesh('none'))
|
|
|
|
def test_texture_gridlayout(self):
|
|
spec = mujoco.MjSpec()
|
|
|
|
texture = spec.add_texture(name='test', gridlayout='.U..LFRB.D..')
|
|
self.assertEqual(list(texture.gridlayout), list('.U..LFRB.D..'))
|
|
|
|
texture2 = spec.add_texture(name='test2', gridlayout=list('.U..LFRB.D..'))
|
|
self.assertEqual(list(texture2.gridlayout), list('.U..LFRB.D..'))
|
|
|
|
with self.assertRaises(ValueError) as cm:
|
|
spec.add_texture(name='test3', gridlayout='.U..')
|
|
self.assertIn('should have length 12', str(cm.exception))
|
|
|
|
with self.assertRaises(ValueError) as cm:
|
|
spec.add_texture(name='test4', gridlayout=['.', 'U', '.', '.'])
|
|
self.assertIn('should have length 12', str(cm.exception))
|
|
|
|
with self.assertRaises(ValueError) as cm:
|
|
spec.add_texture(name='test5', gridlayout=['..', 'U'] + ['.'] * 10)
|
|
self.assertIn('list elements must be single characters', str(cm.exception))
|
|
|
|
def test_attach_units(self):
|
|
child = mujoco.MjSpec()
|
|
parent = mujoco.MjSpec()
|
|
parent.compiler.degree = not child.compiler.degree
|
|
body = child.worldbody.add_body(euler=[90, 0, 0])
|
|
frame = parent.worldbody.add_frame(euler=[-mujoco.mjPI / 2, 0, 0])
|
|
frame.attach_body(body, prefix='child-')
|
|
model = parent.compile()
|
|
np.testing.assert_almost_equal(model.body_quat[1], [1, 0, 0, 0])
|
|
|
|
def test_attach_to_site(self):
|
|
parent = mujoco.MjSpec()
|
|
parent.assets = {'path/cube.obj': 'cube_content'}
|
|
site = parent.worldbody.add_site(pos=[1, 2, 3], quat=[0, 0, 0, 1])
|
|
site.name = 'site'
|
|
|
|
# Attach body to site and compile.
|
|
child1 = mujoco.MjSpec()
|
|
child1.assets = {'cube1.obj': 'cube1_content'}
|
|
body1 = child1.worldbody.add_body()
|
|
self.assertIs(body1, site.attach_body(body1, prefix='_'))
|
|
self.assertIsNotNone(child1.worldbody)
|
|
body1.pos = [1, 1, 1]
|
|
model1 = parent.compile()
|
|
self.assertIsNotNone(model1)
|
|
self.assertEqual(model1.nbody, 2)
|
|
np.testing.assert_array_equal(model1.body_pos[1], [0, 1, 4])
|
|
np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1])
|
|
self.assertEqual(parent.assets['path/cube.obj'], 'cube_content')
|
|
|
|
# Attach entire spec to site and compile again.
|
|
child2 = mujoco.MjSpec()
|
|
child2.assets = {'path/cube2.obj': 'cube2_content'}
|
|
body2 = child2.worldbody.add_body(name='body')
|
|
self.assertIsNotNone(parent.attach(child2, site=site, suffix='-child2'))
|
|
self.assertIsNotNone(child2.worldbody)
|
|
self.assertEqual(child2.parent, parent)
|
|
body2.pos = [-1, -1, -1]
|
|
model2 = parent.compile()
|
|
self.assertIsNotNone(model2)
|
|
self.assertEqual(model2.nbody, 3)
|
|
np.testing.assert_array_equal(model2.body_pos[1], [0, 1, 4])
|
|
np.testing.assert_array_equal(model2.body_pos[2], [2, 3, 2])
|
|
np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1])
|
|
np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1])
|
|
self.assertEqual(parent.assets['path/cube.obj'], 'cube_content')
|
|
self.assertEqual(parent.assets['path//cube2-child2.obj'], 'cube2_content')
|
|
|
|
# Attach another spec to site (referenced by name) and compile again.
|
|
child3 = mujoco.MjSpec()
|
|
child3.assets = {'path/cube3.obj': 'cube3_content'}
|
|
body3 = child3.worldbody.add_body(name='body')
|
|
self.assertIsNotNone(parent.attach(child3, site='site', prefix='child3-'))
|
|
self.assertIsNotNone(child3.worldbody)
|
|
self.assertEqual(child3.parent, parent)
|
|
body3.pos = [-2, -2, -2]
|
|
model3 = parent.compile()
|
|
self.assertIsNotNone(model3)
|
|
self.assertEqual(model3.nbody, 4)
|
|
np.testing.assert_array_equal(model3.body_pos[1], [0, 1, 4])
|
|
np.testing.assert_array_equal(model3.body_pos[2], [2, 3, 2])
|
|
np.testing.assert_array_equal(model3.body_pos[3], [3, 4, 1])
|
|
np.testing.assert_array_equal(model3.body_quat[1], [0, 0, 0, 1])
|
|
np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1])
|
|
np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1])
|
|
self.assertEqual(parent.assets['path/cube.obj'], 'cube_content')
|
|
self.assertEqual(parent.assets['path//cube2-child2.obj'], 'cube2_content')
|
|
self.assertEqual(parent.assets['path/child3-cube3.obj'], 'cube3_content')
|
|
|
|
# Fail to attach to a site that does not exist.
|
|
child4 = mujoco.MjSpec()
|
|
with self.assertRaisesRegex(ValueError, 'Site not found.'):
|
|
parent.attach(child4, site='invalid_site', prefix='child3-')
|
|
|
|
def test_body_to_frame(self):
|
|
spec = mujoco.MjSpec()
|
|
body = spec.worldbody.add_body(pos=[1, 2, 3])
|
|
spec.compile()
|
|
frame = body.to_frame()
|
|
np.testing.assert_array_equal(frame.pos, [1, 2, 3])
|
|
|
|
def test_get_frame(self):
|
|
spec = mujoco.MjSpec()
|
|
body = spec.worldbody.add_body()
|
|
frame = body.add_frame()
|
|
geom = body.add_geom()
|
|
geom.set_frame(frame)
|
|
self.assertIsNotNone(frame)
|
|
self.assertIs(geom.frame, frame)
|
|
|
|
def test_attach_to_frame(self):
|
|
parent = mujoco.MjSpec()
|
|
parent.assets = {'path/cube.obj': 'cube_content'}
|
|
frame = parent.worldbody.add_frame(pos=[1, 2, 3], quat=[0, 0, 0, 1])
|
|
frame.name = 'frame'
|
|
|
|
# Attach body to frame and compile.
|
|
child1 = mujoco.MjSpec()
|
|
child1.assets = {'path/cube1.obj': 'cube1_content'}
|
|
body1 = child1.worldbody.add_body()
|
|
self.assertIs(body1, frame.attach_body(body1, prefix='_'))
|
|
self.assertIsNotNone(child1.worldbody)
|
|
body1.pos = [1, 1, 1]
|
|
model1 = parent.compile()
|
|
self.assertIsNotNone(model1)
|
|
self.assertEqual(model1.nbody, 2)
|
|
np.testing.assert_array_equal(model1.body_pos[1], [0, 1, 4])
|
|
np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1])
|
|
self.assertEqual(parent.assets['path/cube.obj'], 'cube_content')
|
|
|
|
# Attach entire spec to frame and compile again.
|
|
child2 = mujoco.MjSpec()
|
|
child2.assets = {'path/cube2.obj': 'cube2_content'}
|
|
body2 = child2.worldbody.add_body(name='body')
|
|
body2.set_frame(child2.worldbody.add_frame(pos=[-1, -1, 1]))
|
|
self.assertIsNotNone(parent.attach(child2, frame=frame, suffix='-child'))
|
|
self.assertIsNotNone(child2.worldbody)
|
|
self.assertEqual(child2.parent, parent)
|
|
body2.pos = [-1, -1, -1]
|
|
model2 = parent.compile()
|
|
self.assertIsNotNone(model2)
|
|
self.assertEqual(model2.nbody, 3)
|
|
np.testing.assert_array_equal(model2.body_pos[1], [0, 1, 4])
|
|
np.testing.assert_array_equal(model2.body_pos[2], [3, 4, 3])
|
|
np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1])
|
|
np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1])
|
|
self.assertEqual(parent.assets['path/cube.obj'], 'cube_content')
|
|
self.assertEqual(parent.assets['path//cube2-child.obj'], 'cube2_content')
|
|
|
|
# Attach another spec to frame (referenced by name) and compile again.
|
|
child3 = mujoco.MjSpec()
|
|
child3.assets = {'path/cube2.obj': 'new_content'}
|
|
body3 = child3.worldbody.add_body(name='body')
|
|
body3.set_frame(child3.worldbody.add_frame(pos=[-1, -1, 1]))
|
|
self.assertIsNotNone(parent.attach(child3, frame='frame', prefix='child3-'))
|
|
self.assertIsNotNone(child3.worldbody)
|
|
self.assertEqual(child3.parent, parent)
|
|
body3.pos = [-2, -2, -2]
|
|
model3 = parent.compile()
|
|
self.assertIsNotNone(model3)
|
|
self.assertEqual(model3.nbody, 4)
|
|
np.testing.assert_array_equal(model3.body_pos[1], [0, 1, 4])
|
|
np.testing.assert_array_equal(model3.body_pos[2], [3, 4, 3])
|
|
np.testing.assert_array_equal(model3.body_pos[3], [4, 5, 2])
|
|
np.testing.assert_array_equal(model3.body_quat[1], [0, 0, 0, 1])
|
|
np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1])
|
|
np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1])
|
|
self.assertEqual(parent.assets['path/cube.obj'], 'cube_content')
|
|
self.assertEqual(parent.assets['path/child3-cube2.obj'], 'new_content')
|
|
|
|
# Fail to attach to a frame that does not exist.
|
|
child4 = mujoco.MjSpec()
|
|
with self.assertRaisesRegex(ValueError, 'Frame not found.'):
|
|
parent.attach(child4, frame='invalid_frame', prefix='child3-')
|
|
|
|
def test_attach_valid_child_lists(self):
|
|
xml1 = """
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="b1">
|
|
<geom name="g1"/>
|
|
<joint name="j1" type="hinge"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
"""
|
|
|
|
xml2 = """
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="b2">
|
|
<geom name="g2"/>
|
|
<joint name="j2" type="hinge"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
"""
|
|
|
|
parent = mujoco.MjSpec.from_string(xml1)
|
|
child = mujoco.MjSpec.from_string(xml2)
|
|
self.assertLen(child.joints, 1)
|
|
self.assertLen(child.geoms, 1)
|
|
frame = parent.worldbody.add_frame()
|
|
parent.attach(child, prefix='', frame=frame)
|
|
self.assertLen(child.joints, 1)
|
|
self.assertLen(child.geoms, 1)
|
|
|
|
def test_bind(self):
|
|
spec = mujoco.MjSpec.from_string("""
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="main">
|
|
<geom name="main" size="0.15 0.15 0.15" mass="1" type="box"/>
|
|
<freejoint/>
|
|
<body name="box">
|
|
<joint name="box" type="hinge" range="-1 +1"/>
|
|
<geom name="box" size="0.15 0.15 0.15" mass="1" type="box"/>
|
|
</body>
|
|
<body name="sphere">
|
|
<joint name="sphere" type="hinge" range="-1 +1"/>
|
|
<geom name="sphere" size="0.15 0.15 0.15" mass="1" type="box"/>
|
|
</body>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>
|
|
""")
|
|
joint_box = spec.joint('box')
|
|
joint_sphere = spec.joint('sphere')
|
|
joints = [joint_box, joint_sphere]
|
|
mj_model = spec.compile()
|
|
mj_data = mujoco.MjData(mj_model)
|
|
np.testing.assert_array_equal(mj_data.bind(joint_box).qpos, 0)
|
|
np.testing.assert_array_equal(mj_model.bind(joint_box).qposadr, 7)
|
|
np.testing.assert_array_equal(mj_data.bind(joints).qpos, [0, 0])
|
|
np.testing.assert_array_equal(mj_model.bind(joints).qposadr, [7, 8])
|
|
np.testing.assert_array_equal(mj_data.bind([]).qpos, [])
|
|
np.testing.assert_array_equal(mj_model.bind([]).qposadr, [])
|
|
mj_data.bind(joints).qpos = np.array([1, 2])
|
|
np.testing.assert_array_equal(mj_data.bind(joints).qpos, [1, 2])
|
|
with self.assertRaisesRegex(
|
|
AttributeError, "object has no attribute 'invalid'"
|
|
):
|
|
print(mj_model.bind(joints).invalid)
|
|
invalid_spec = mujoco.MjSpec()
|
|
invalid_spec.worldbody.add_body(name='main')
|
|
with self.assertRaisesRegex(
|
|
ValueError,
|
|
'The mjSpec does not match mjModel. Please recompile the mjSpec.',
|
|
):
|
|
print(mj_model.bind(invalid_spec.body('main')))
|
|
|
|
def test_incorrect_hfield_size(self):
|
|
nrow = 300
|
|
ncol = 400
|
|
hdata = np.random.uniform(size=(1, 1))
|
|
model_spec = mujoco.MjSpec()
|
|
model_spec.add_hfield(
|
|
name='hfield',
|
|
size=[1, 1, 1, 1e-3],
|
|
ncol=ncol,
|
|
nrow=nrow,
|
|
userdata=hdata.flatten(),
|
|
)
|
|
model_spec.worldbody.add_geom(
|
|
name='hfield',
|
|
type=mujoco.mjtGeom.mjGEOM_HFIELD,
|
|
pos=np.array([0, 0, 1]),
|
|
hfieldname='hfield',
|
|
)
|
|
with self.assertRaisesRegex(
|
|
ValueError, r"Error: elevation data length must match nrow\*ncol\n"
|
|
"Element name 'hfield', id 0",
|
|
):
|
|
model_spec.compile()
|
|
|
|
def test_address(self):
|
|
spec1 = mujoco.MjSpec()
|
|
spec2 = mujoco.MjSpec()
|
|
spec3 = spec1.copy()
|
|
|
|
self.assertGreater(spec1._address, 0)
|
|
self.assertGreater(spec2._address, 0)
|
|
self.assertGreater(spec3._address, 0)
|
|
self.assertLen({spec1._address, spec2._address, spec3._address}, 3)
|
|
|
|
def test_actuator_shortname(self):
|
|
spec = mujoco.MjSpec()
|
|
actuator = spec.add_actuator(
|
|
gainprm=np.zeros((10,)),
|
|
dyntype=mujoco.mjtDyn.mjDYN_FILTER,
|
|
gaintype=mujoco.mjtGain.mjGAIN_AFFINE,
|
|
biastype=mujoco.mjtBias.mjBIAS_AFFINE,
|
|
)
|
|
actuator.set_to_motor()
|
|
self.assertEqual(actuator.gainprm[0], 1)
|
|
self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE)
|
|
self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED)
|
|
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE)
|
|
|
|
actuator.set_to_position(kp=2.0, kv=3.0, timeconst=4.0, inheritrange=True)
|
|
self.assertEqual(actuator.gainprm[0], 2)
|
|
self.assertEqual(actuator.biasprm[1], -2)
|
|
self.assertEqual(actuator.biasprm[2], -3)
|
|
self.assertEqual(actuator.dynprm[0], 4)
|
|
self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_FILTEREXACT)
|
|
self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED)
|
|
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE)
|
|
self.assertEqual(actuator.inheritrange, True)
|
|
|
|
actuator.set_to_intvelocity(
|
|
kp=2.0, kv=3.0, timeconst=4.0, inheritrange=True
|
|
)
|
|
self.assertEqual(actuator.gainprm[0], 2)
|
|
self.assertEqual(actuator.biasprm[1], -2)
|
|
self.assertEqual(actuator.biasprm[2], -3)
|
|
self.assertEqual(actuator.dynprm[0], 4)
|
|
self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_INTEGRATOR)
|
|
self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED)
|
|
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE)
|
|
self.assertEqual(actuator.inheritrange, True)
|
|
|
|
actuator.set_to_velocity(kv=5.0)
|
|
self.assertEqual(actuator.gainprm[0], 5)
|
|
self.assertEqual(actuator.biasprm[2], -5)
|
|
self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE)
|
|
self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED)
|
|
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE)
|
|
|
|
actuator.set_to_damper(kv=6.0)
|
|
self.assertEqual(actuator.gainprm[0], 0)
|
|
self.assertEqual(actuator.gainprm[2], -6)
|
|
self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE)
|
|
self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_AFFINE)
|
|
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE)
|
|
|
|
actuator.set_to_adhesion(gain=7.0)
|
|
self.assertEqual(actuator.gainprm[0], 7)
|
|
self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE)
|
|
self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED)
|
|
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE)
|
|
|
|
def test_bad_contact_sensor(self):
|
|
test_cases = [
|
|
dict(
|
|
expected_error='num (intprm[2]) must be positive in sensor',
|
|
sensor_params=dict(
|
|
type=mujoco.mjtSensor.mjSENS_CONTACT,
|
|
objtype=mujoco.mjtObj.mjOBJ_GEOM,
|
|
objname='sphere1',
|
|
intprm=[1, 0, 0],
|
|
),
|
|
),
|
|
dict(
|
|
expected_error='data spec (intprm[0]) must be positive, got 0',
|
|
sensor_params=dict(
|
|
type=mujoco.mjtSensor.mjSENS_CONTACT,
|
|
objtype=mujoco.mjtObj.mjOBJ_GEOM,
|
|
objname='sphere1',
|
|
intprm=[0, 0, 1],
|
|
),
|
|
),
|
|
dict(
|
|
expected_error=(
|
|
'data spec intprm[0]=1024 must have at least one bit set of the'
|
|
' first mjNCONDATA bits'
|
|
),
|
|
sensor_params=dict(
|
|
type=mujoco.mjtSensor.mjSENS_CONTACT,
|
|
objtype=mujoco.mjtObj.mjOBJ_GEOM,
|
|
objname='sphere1',
|
|
intprm=[1 << 10, 0, 1],
|
|
),
|
|
),
|
|
dict(
|
|
expected_error=(
|
|
'data spec intprm[0]=1025 has bits set beyond the first'
|
|
' mjNCONDATA bits'
|
|
),
|
|
sensor_params=dict(
|
|
type=mujoco.mjtSensor.mjSENS_CONTACT,
|
|
objtype=mujoco.mjtObj.mjOBJ_GEOM,
|
|
objname='sphere1',
|
|
intprm=[(1 << 10) | 1, 0, 1],
|
|
),
|
|
),
|
|
dict(
|
|
expected_error='unknown reduction criterion. got 4',
|
|
sensor_params=dict(
|
|
type=mujoco.mjtSensor.mjSENS_CONTACT,
|
|
objtype=mujoco.mjtObj.mjOBJ_GEOM,
|
|
objname='sphere1',
|
|
intprm=[1, 4, 1],
|
|
),
|
|
),
|
|
dict(
|
|
expected_error=(
|
|
'first matching criterion: if set, must be'
|
|
' (x)body, geom or site'
|
|
),
|
|
sensor_params=dict(
|
|
type=mujoco.mjtSensor.mjSENS_CONTACT,
|
|
objtype=mujoco.mjtObj.mjOBJ_CAMERA,
|
|
objname='cam',
|
|
),
|
|
),
|
|
dict(
|
|
expected_error=(
|
|
'second matching criterion: if set, must be (x)body or geom'
|
|
),
|
|
sensor_params=dict(
|
|
type=mujoco.mjtSensor.mjSENS_CONTACT,
|
|
reftype=mujoco.mjtObj.mjOBJ_CAMERA,
|
|
refname='cam',
|
|
),
|
|
),
|
|
]
|
|
|
|
for params in test_cases:
|
|
expected_error = params.get('expected_error')
|
|
with self.subTest(expected_error):
|
|
spec = mujoco.MjSpec()
|
|
spec.worldbody.add_geom(name='sphere1', size=[.2, 0, 0], pos=[0, 0, 1])
|
|
body = spec.worldbody.add_body(name='body')
|
|
non_root = body.add_body(name='non_root')
|
|
non_root.add_geom(name='sphere3', size=[.3, 0, 0], pos=[1, 0, 1])
|
|
spec.worldbody.add_camera(name='cam')
|
|
spec.add_sensor(**params['sensor_params'])
|
|
error_predicate = lambda e, expected=expected_error: expected in str(e)
|
|
with self.assertRaisesWithPredicateMatch(ValueError, error_predicate):
|
|
spec.compile()
|
|
|
|
def test_sensor_data_size(self):
|
|
spec = mujoco.MjSpec()
|
|
quat = spec.add_sensor(
|
|
name='framequat',
|
|
type=mujoco.mjtSensor.mjSENS_FRAMEQUAT,
|
|
objtype=mujoco.mjtObj.mjOBJ_BODY,
|
|
objname='world',
|
|
)
|
|
self.assertEqual(quat.get_data_size(), 4)
|
|
clock = spec.add_sensor(
|
|
name='clock',
|
|
type=mujoco.mjtSensor.mjSENS_CLOCK,
|
|
)
|
|
self.assertEqual(clock.get_data_size(), 1)
|
|
mj_model = spec.compile()
|
|
self.assertEqual(mj_model.sensor_dim[0], 4)
|
|
self.assertEqual(mj_model.sensor_dim[1], 1)
|
|
|
|
def test_mesh_material(self):
|
|
spec = mujoco.MjSpec()
|
|
|
|
spec.add_material(name='red', rgba=(1, 0, 0, 1))
|
|
spec.add_material(name='green', rgba=(0, 1, 0, 1))
|
|
|
|
mesh = spec.add_mesh(name='sphere')
|
|
mesh.make_sphere(subdivision=1)
|
|
mesh.material = 'red'
|
|
|
|
geom = spec.worldbody.add_geom()
|
|
geom.type = mujoco.mjtGeom.mjGEOM_MESH
|
|
geom.meshname = 'sphere'
|
|
|
|
geom_2 = spec.worldbody.add_geom()
|
|
geom_2.type = mujoco.mjtGeom.mjGEOM_MESH
|
|
geom_2.meshname = 'sphere'
|
|
geom_2.material = 'green'
|
|
|
|
model = spec.compile()
|
|
|
|
self.assertEqual(model.geom_matid[0], 0)
|
|
self.assertEqual(model.geom_matid[1], 1)
|
|
|
|
mesh.material = 'green'
|
|
model = spec.compile()
|
|
|
|
self.assertEqual(model.geom_matid[0], 1)
|
|
|
|
def test_tendon_path(self):
|
|
spec = mujoco.MjSpec()
|
|
|
|
body = spec.worldbody.add_body(name='body')
|
|
|
|
body.add_geom(name='body_geom', pos=[0, 0, 0], size=[.1, 0, 0])
|
|
site1 = body.add_site(name='site1', pos=[0, 0, 0])
|
|
site2 = body.add_site(name='site2', pos=[0, 0, -1])
|
|
site3 = body.add_site(name='site3', pos=[0, 0, -4])
|
|
sidesite = body.add_site(name='sidesite', pos=[2, 0, -5])
|
|
site4 = body.add_site(name='site4', pos=[0, 1, -6])
|
|
|
|
sphere = spec.worldbody.add_geom(name='sphere', size=[.2, 0, 0], pos=[0, 0, -2])
|
|
|
|
cylinder = spec.worldbody.add_geom(
|
|
name='cylinder',
|
|
type=mujoco.mjtGeom.mjGEOM_CYLINDER,
|
|
size=[0.1, 0.2, 0.3],
|
|
pos=[0, 0, -5]
|
|
)
|
|
|
|
joint1 = body.add_joint(
|
|
name='joint1', type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]
|
|
)
|
|
|
|
body2 = spec.worldbody.add_body(name='body2', pos=[2, 0, 0])
|
|
body2.add_geom(name='body2_geom', pos=[0, 0, 0], size=[.1, 0, 0])
|
|
joint2 = body2.add_joint(
|
|
name='joint2', type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]
|
|
)
|
|
|
|
spatial_tendon = spec.add_tendon()
|
|
fixed_tendon = spec.add_tendon()
|
|
|
|
wrap_site1 = spatial_tendon.wrap_site('site1')
|
|
wrap_site2 = spatial_tendon.wrap_site('site2')
|
|
wrap_pulley1 = spatial_tendon.wrap_pulley(2.0)
|
|
wrap_site3_1 = spatial_tendon.wrap_site('site3')
|
|
wrap_sphere = spatial_tendon.wrap_geom('sphere', '')
|
|
wrap_site4_1 = spatial_tendon.wrap_site('site4')
|
|
wrap_pulley2 = spatial_tendon.wrap_pulley(2.0)
|
|
wrap_site3_2 = spatial_tendon.wrap_site('site3')
|
|
wrap_cylinder = spatial_tendon.wrap_geom('cylinder', 'sidesite')
|
|
wrap_site4_2 = spatial_tendon.wrap_site('site4')
|
|
|
|
wrap_joint1 = fixed_tendon.wrap_joint('joint1', 1.0)
|
|
wrap_joint2 = fixed_tendon.wrap_joint('joint2', 2.0)
|
|
|
|
self.assertListEqual(
|
|
list(spatial_tendon.path),
|
|
[
|
|
wrap_site1,
|
|
wrap_site2,
|
|
wrap_pulley1,
|
|
wrap_site3_1,
|
|
wrap_sphere,
|
|
wrap_site4_1,
|
|
wrap_pulley2,
|
|
wrap_site3_2,
|
|
wrap_cylinder,
|
|
wrap_site4_2,
|
|
],
|
|
)
|
|
self.assertListEqual(
|
|
[w.target for w in spatial_tendon.path],
|
|
[
|
|
site1,
|
|
site2,
|
|
None, # Pulley wraps have no targets
|
|
site3,
|
|
sphere,
|
|
site4,
|
|
None, # Pulley wraps have no targets
|
|
site3,
|
|
cylinder,
|
|
site4,
|
|
],
|
|
)
|
|
self.assertEqual(spatial_tendon.path[8].sidesite, sidesite)
|
|
self.assertIsNone(spatial_tendon.path[7].sidesite)
|
|
|
|
self.assertListEqual(list(fixed_tendon.path), [wrap_joint1, wrap_joint2])
|
|
self.assertListEqual(
|
|
[w.target for w in fixed_tendon.path],
|
|
[joint1, joint2]
|
|
)
|
|
|
|
# Wrap type for geom is only set during compilation.
|
|
spec.compile()
|
|
|
|
self.assertEqual(wrap_site1.type, mujoco.mjtWrap.mjWRAP_SITE)
|
|
self.assertEqual(wrap_site2.type, mujoco.mjtWrap.mjWRAP_SITE)
|
|
self.assertEqual(wrap_site3_1.type, mujoco.mjtWrap.mjWRAP_SITE)
|
|
self.assertEqual(wrap_site4_1.type, mujoco.mjtWrap.mjWRAP_SITE)
|
|
self.assertEqual(wrap_site3_2.type, mujoco.mjtWrap.mjWRAP_SITE)
|
|
self.assertEqual(wrap_site4_2.type, mujoco.mjtWrap.mjWRAP_SITE)
|
|
self.assertEqual(wrap_pulley1.type, mujoco.mjtWrap.mjWRAP_PULLEY)
|
|
self.assertEqual(wrap_pulley1.divisor, 2.0)
|
|
self.assertEqual(wrap_sphere.type, mujoco.mjtWrap.mjWRAP_SPHERE)
|
|
self.assertEqual(wrap_cylinder.type, mujoco.mjtWrap.mjWRAP_CYLINDER)
|
|
self.assertEqual(wrap_pulley2.type, mujoco.mjtWrap.mjWRAP_PULLEY)
|
|
self.assertEqual(wrap_pulley2.divisor, 2.0)
|
|
|
|
self.assertEqual(wrap_joint1.type, mujoco.mjtWrap.mjWRAP_JOINT)
|
|
self.assertEqual(wrap_joint1.coef, 1.0)
|
|
self.assertEqual(wrap_joint2.type, mujoco.mjtWrap.mjWRAP_JOINT)
|
|
self.assertEqual(wrap_joint2.coef, 2.0)
|
|
|
|
def test_from_zip(self):
|
|
"""Tests that the assets are correctly parsed from a zip file."""
|
|
model_path_root = (
|
|
epath.resource_path("mujoco") / "testdata" / "MJCF_Root.zip"
|
|
)
|
|
model_path_no_root = (
|
|
epath.resource_path("mujoco") / "testdata" / "MJCF_NoRoot.zip"
|
|
)
|
|
filenames = [model_path_root.as_posix(), model_path_no_root.as_posix()]
|
|
|
|
for filename in filenames:
|
|
with self.subTest(filename):
|
|
spec = mujoco.MjSpec.from_zip(filename)
|
|
spec.compile()
|
|
assets = spec.assets
|
|
xml_string = spec.to_xml()
|
|
string_spec = mujoco.MjSpec.from_string(xml_string, assets=assets)
|
|
string_spec.compile()
|
|
self.assertEqual(spec.to_xml(), string_spec.to_xml())
|
|
|
|
def test_rangefinder_sensor(self):
|
|
"""Test rangefinder sensor with mjSpec, iterative model building."""
|
|
# Raydata field enum values for dataspec bitfield
|
|
rd = mujoco.mjtRayDataField
|
|
dist_val = int(rd.mjRAYDATA_DIST)
|
|
dir_val = int(rd.mjRAYDATA_DIR)
|
|
origin_val = int(rd.mjRAYDATA_ORIGIN)
|
|
point_val = int(rd.mjRAYDATA_POINT)
|
|
normal_val = int(rd.mjRAYDATA_NORMAL)
|
|
depth_val = int(rd.mjRAYDATA_DEPTH)
|
|
|
|
# Step 1: Create a rangefinder sensor attached to a site, no dataspec set.
|
|
# Note: site goes on a child body because rangefinder excludes the site's
|
|
# parent body from ray casting.
|
|
spec = mujoco.MjSpec()
|
|
sensor_body = spec.worldbody.add_body(name='sensor_body', pos=[0, 0, 1])
|
|
sensor_body.add_site(name='rf_site', zaxis=[0, 0, -1])
|
|
rf_sensor = spec.add_sensor(
|
|
name='rf',
|
|
type=mujoco.mjtSensor.mjSENS_RANGEFINDER,
|
|
objtype=mujoco.mjtObj.mjOBJ_SITE,
|
|
objname='rf_site',
|
|
)
|
|
|
|
# This should fail: data spec (intprm[0]) must be positive
|
|
with self.assertRaisesWithPredicateMatch(
|
|
ValueError,
|
|
lambda e: 'data spec (intprm[0]) must be positive' in str(e)
|
|
):
|
|
spec.compile()
|
|
|
|
# Step 2: Set dataspec to just mjRAYDATA_DIST
|
|
rf_sensor.intprm[0] = 1 << dist_val
|
|
model = spec.compile()
|
|
data = mujoco.MjData(model)
|
|
mujoco.mj_forward(model, data)
|
|
|
|
# With no geometry, the ray should miss: dist = -1
|
|
self.assertEqual(model.nsensordata, 1)
|
|
self.assertEqual(data.bind(rf_sensor).data[0], -1)
|
|
|
|
# Step 3: Add all raydata fields and check no-hit values
|
|
all_fields = (
|
|
(1 << dist_val) | (1 << dir_val) | (1 << origin_val) |
|
|
(1 << point_val) | (1 << normal_val) | (1 << depth_val)
|
|
)
|
|
rf_sensor.intprm[0] = all_fields
|
|
model = spec.compile()
|
|
data = mujoco.MjData(model)
|
|
mujoco.mj_forward(model, data)
|
|
|
|
# Expected size: dist(1) + dir(3) + origin(3) + point(3) + normal(3) +
|
|
# depth(1) = 14
|
|
self.assertEqual(model.nsensordata, 14)
|
|
|
|
# No-hit values
|
|
sd = data.bind(rf_sensor).data
|
|
self.assertEqual(sd[0], -1) # dist
|
|
np.testing.assert_allclose(sd[1:4], [0, 0, 0]) # dir
|
|
np.testing.assert_allclose(sd[4:7], [0, 0, 1]) # origin
|
|
np.testing.assert_allclose(sd[7:10], [0, 0, 0]) # point
|
|
np.testing.assert_allclose(sd[10:13], [0, 0, 0]) # normal
|
|
self.assertEqual(sd[13], -1) # depth
|
|
|
|
# Step 4: Add a floor plane, now the ray should hit
|
|
spec.worldbody.add_geom(
|
|
name='floor',
|
|
type=mujoco.mjtGeom.mjGEOM_PLANE,
|
|
size=[10, 10, 0.1],
|
|
)
|
|
model = spec.compile()
|
|
data = mujoco.MjData(model)
|
|
mujoco.mj_forward(model, data)
|
|
|
|
# Ray starts at z=1 pointing down, hits floor at z=0
|
|
# For site sensor, depth = dist
|
|
sd = data.bind(rf_sensor).data
|
|
self.assertAlmostEqual(sd[0], 1.0, places=6) # dist
|
|
np.testing.assert_allclose(sd[1:4], [0, 0, -1], atol=1e-10) # dir
|
|
np.testing.assert_allclose(sd[4:7], [0, 0, 1], atol=1e-10) # origin
|
|
np.testing.assert_allclose(sd[7:10], [0, 0, 0], atol=1e-10) # point
|
|
np.testing.assert_allclose(sd[10:13], [0, 0, 1], atol=1e-10) # normal
|
|
self.assertAlmostEqual(sd[13], 1.0, places=6) # depth
|
|
|
|
# Step 5: Add a camera-based rangefinder sensor
|
|
# Camera also on child body so it doesn't exclude the floor
|
|
cam_body = spec.worldbody.add_body(name='cam_body', pos=[0, 0, 2])
|
|
cam_body.add_camera(
|
|
name='rf_cam',
|
|
xyaxes=[1, 0, 0, 0, 1, 0], # z=[0,0,1], looks along -z (down)
|
|
resolution=[3, 3],
|
|
fovy=90,
|
|
)
|
|
cam_sensor = spec.add_sensor(
|
|
name='rf_cam_sensor',
|
|
type=mujoco.mjtSensor.mjSENS_RANGEFINDER,
|
|
objtype=mujoco.mjtObj.mjOBJ_CAMERA,
|
|
objname='rf_cam',
|
|
intprm=[(1 << dist_val) | (1 << depth_val), 0, 0],
|
|
)
|
|
|
|
model = spec.compile()
|
|
data = mujoco.MjData(model)
|
|
mujoco.mj_forward(model, data)
|
|
|
|
# Site sensor: 14 values, Camera sensor: (1+1)*9 = 18 values
|
|
self.assertEqual(model.nsensordata, 14 + 18)
|
|
|
|
# Check camera sensor data using bind
|
|
cam_sd = data.bind(cam_sensor).data
|
|
stride = 2 # dist + depth per pixel
|
|
center_pixel = 4 # center of 3x3 = row 1, col 1
|
|
|
|
# Center pixel: ray straight down from z=2 to z=0
|
|
self.assertAlmostEqual(cam_sd[center_pixel * stride], 2.0, places=6)
|
|
self.assertAlmostEqual(cam_sd[center_pixel * stride + 1], 2.0, places=6)
|
|
|
|
# Corner pixel: off-axis ray, dist > depth
|
|
self.assertGreater(cam_sd[0], cam_sd[1]) # dist > depth
|
|
self.assertAlmostEqual(cam_sd[1], 2.0, places=6) # depth is still 2.0
|
|
|
|
if __name__ == '__main__':
|
|
absltest.main()
|