def testSceneGeomsAttribute(self):
scene = core.MjvScene(model=self.model)
self.assertEqual(scene.ngeom, 0)
self.assertEmpty(scene.geoms)
geom_types = (enums.mjtObj.mjOBJ_BODY, enums.mjtObj.mjOBJ_GEOM,
enums.mjtObj.mjOBJ_SITE)
for geom_type in geom_types:
scene.ngeom += 1
scene.ptr.contents.geoms[scene.ngeom - 1].objtype = geom_type
self.assertLen(scene.geoms, len(geom_types))
np.testing.assert_array_equal(scene.geoms.objtype, geom_types)
class CoreTest(parameterized.TestCase):
def setUp(self):
self.model = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
self.data = core.MjData(self.model)
def _assert_attributes_equal(self, actual_obj, expected_obj,
attr_to_compare):
for name in attr_to_compare:
actual_value = getattr(actual_obj, name)
expected_value = getattr(expected_obj, name)
try:
if isinstance(expected_value, np.ndarray):
np.testing.assert_array_equal(actual_value, expected_value)
else:
self.assertEqual(actual_value, expected_value)
except AssertionError as e:
self.fail(
"Attribute '{}' differs from expected value: {}".format(
name, str(e)))
def testLoadXML(self):
with open(HUMANOID_XML_PATH, "r") as f:
xml_string = f.read()
model = core.MjModel.from_xml_string(xml_string)
core.MjData(model)
with self.assertRaises(TypeError):
core.MjModel()
with self.assertRaises(core.Error):
core.MjModel.from_xml_path("/path/to/nonexistent/model/file.xml")
xml_with_warning = """
<mujoco>
<size njmax='2'/>
<worldbody>
<body pos='0 0 0'>
<geom type='box' size='.1 .1 .1'/>
</body>
<body pos='0 0 0'>
<joint type='slide' axis='1 0 0'/>
<geom type='box' size='.1 .1 .1'/>
</body>
</worldbody>
</mujoco>"""
# This model should compile successfully, but raise a warning on the first
# simulation step.
model = core.MjModel.from_xml_string(xml_with_warning)
data = core.MjData(model)
with mock.patch.object(core, "logging") as mock_logging:
mjlib.mj_step(model.ptr, data.ptr)
mock_logging.warn.assert_called_once_with(
"Pre-allocated constraint buffer is full. Increase njmax above 2. "
"Time = 0.0000.")
def testLoadXMLWithAssetsFromString(self):
core.MjModel.from_xml_string(MODEL_WITH_ASSETS, assets=ASSETS)
with self.assertRaises(core.Error):
# Should fail to load without the assets
core.MjModel.from_xml_string(MODEL_WITH_ASSETS)
def testVFSFilenameTooLong(self):
limit = core._MAX_VFS_FILENAME_CHARACTERS
contents = "fake contents"
valid_filename = "a" * limit
with core._temporary_vfs({valid_filename: contents}):
pass
invalid_filename = "a" * (limit + 1)
expected_message = core._VFS_FILENAME_TOO_LONG.format(
length=(limit + 1), limit=limit, filename=invalid_filename)
with self.assertRaisesWithLiteralMatch(ValueError, expected_message):
with core._temporary_vfs({invalid_filename: contents}):
pass
def testSaveLastParsedModelToXML(self):
save_xml_path = os.path.join(OUT_DIR, "tmp_humanoid.xml")
not_last_parsed = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
last_parsed = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
# Modify the model before saving it in order to confirm that the changes are
# written to the XML.
last_parsed.geom_pos.flat[:] = np.arange(last_parsed.geom_pos.size)
core.save_last_parsed_model_to_xml(save_xml_path,
check_model=last_parsed)
loaded = core.MjModel.from_xml_path(save_xml_path)
self._assert_attributes_equal(last_parsed, loaded, ["geom_pos"])
core.MjData(loaded)
# Test that `check_model` results in a ValueError if it is not the most
# recently parsed model.
with self.assertRaisesWithLiteralMatch(ValueError,
core._NOT_LAST_PARSED_ERROR):
core.save_last_parsed_model_to_xml(save_xml_path,
check_model=not_last_parsed)
def testBinaryIO(self):
bin_path = os.path.join(OUT_DIR, "tmp_humanoid.mjb")
self.model.save_binary(bin_path)
core.MjModel.from_binary_path(bin_path)
byte_string = self.model.to_bytes()
core.MjModel.from_byte_string(byte_string)
def testDimensions(self):
self.assertEqual(self.data.qpos.shape[0], self.model.nq)
self.assertEqual(self.data.qvel.shape[0], self.model.nv)
self.assertEqual(self.model.body_pos.shape, (self.model.nbody, 3))
def testStep(self):
t0 = self.data.time
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(self.data.time, t0 + self.model.opt.timestep)
self.assertTrue(np.all(np.isfinite(self.data.qpos[:])))
self.assertTrue(np.all(np.isfinite(self.data.qvel[:])))
def testMultipleData(self):
data2 = core.MjData(self.model)
self.assertNotEqual(self.data.ptr, data2.ptr)
t0 = self.data.time
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(self.data.time, t0 + self.model.opt.timestep)
self.assertEqual(data2.time, 0)
def testMultipleModel(self):
model2 = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
self.assertNotEqual(self.model.ptr, model2.ptr)
self.model.opt.timestep += 0.001
self.assertEqual(self.model.opt.timestep, model2.opt.timestep + 0.001)
def testModelName(self):
self.assertEqual(self.model.name, "humanoid")
@parameterized.named_parameters(
("_copy", lambda x: x.copy()),
("_pickle_unpickle", lambda x: cPickle.loads(cPickle.dumps(x))),
)
def testCopyOrPickleModel(self, func):
timestep = 0.12345
self.model.opt.timestep = timestep
body_pos = self.model.body_pos + 1
self.model.body_pos[:] = body_pos
model2 = func(self.model)
self.assertNotEqual(model2.ptr, self.model.ptr)
self.assertEqual(model2.opt.timestep, timestep)
np.testing.assert_array_equal(model2.body_pos, body_pos)
@parameterized.named_parameters(
("_copy", lambda x: x.copy()),
("_pickle_unpickle", lambda x: cPickle.loads(cPickle.dumps(x))),
)
def testCopyOrPickleData(self, func):
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
data2 = func(self.data)
attr_to_compare = ("time", "energy", "qpos", "xpos")
self.assertNotEqual(data2.ptr, self.data.ptr)
self._assert_attributes_equal(data2, self.data, attr_to_compare)
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
mjlib.mj_step(data2.model.ptr, data2.ptr)
self._assert_attributes_equal(data2, self.data, attr_to_compare)
@parameterized.named_parameters(
("_copy", lambda x: x.copy()),
("_pickle_unpickle", lambda x: cPickle.loads(cPickle.dumps(x))),
)
def testCopyOrPickleStructs(self, func):
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
data2 = func(self.data)
self.assertNotEqual(data2.ptr, self.data.ptr)
attr_to_compare = ("warning", "timer", "solver")
self._assert_attributes_equal(self.data, data2, attr_to_compare)
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
mjlib.mj_step(data2.model.ptr, data2.ptr)
self._assert_attributes_equal(self.data, data2, attr_to_compare)
@parameterized.parameters(("right_foot", "body", 6),
("right_foot", enums.mjtObj.mjOBJ_BODY, 6),
("left_knee", "joint", 11),
("left_knee", enums.mjtObj.mjOBJ_JOINT, 11))
def testNamesIds(self, name, object_type, object_id):
output_id = self.model.name2id(name, object_type)
self.assertEqual(object_id, output_id)
output_name = self.model.id2name(object_id, object_type)
self.assertEqual(name, output_name)
def testNamesIdsExceptions(self):
with self.assertRaisesRegexp(core.Error, "does not exist"):
self.model.name2id("nonexistent_body_name", "body")
with self.assertRaisesRegexp(core.Error, "is not a valid object type"):
self.model.name2id("right_foot", "nonexistent_type_name")
def testNamelessObject(self):
# The model in humanoid.xml contains a single nameless camera.
name = self.model.id2name(0, "camera")
self.assertEqual("", name)
def testWarningCallback(self):
self.data.qpos[0] = np.inf
with mock.patch.object(core, "logging") as mock_logging:
mjlib.mj_step(self.model.ptr, self.data.ptr)
mock_logging.warn.assert_called_once_with(
"Nan, Inf or huge value in QPOS at DOF 0. The simulation is unstable. "
"Time = 0.0000.")
def testErrorCallback(self):
with mock.patch.object(core, "logging") as mock_logging:
mjlib.mj_activate(b"nonexistent_activation_key")
mock_logging.fatal.assert_called_once_with(
"Could not open activation key file nonexistent_activation_key")
def testSingleCallbackContext(self):
callback_was_called = [False]
def callback(unused_model, unused_data):
callback_was_called[0] = True
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertFalse(callback_was_called[0])
class DummyError(RuntimeError):
pass
try:
with core.callback_context("mjcb_passive", callback):
# Stepping invokes the `mjcb_passive` callback.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertTrue(callback_was_called[0])
# Exceptions should not prevent `mjcb_passive` from being reset.
raise DummyError("Simulated exception.")
except DummyError:
pass
# `mjcb_passive` should have been reset to None.
callback_was_called[0] = False
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertFalse(callback_was_called[0])
def testNestedCallbackContexts(self):
last_called = [None]
outer_called = "outer called"
inner_called = "inner called"
def outer(unused_model, unused_data):
last_called[0] = outer_called
def inner(unused_model, unused_data):
last_called[0] = inner_called
with core.callback_context("mjcb_passive", outer):
# This should execute `outer` a few times.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(last_called[0], outer_called)
with core.callback_context("mjcb_passive", inner):
# This should execute `inner` a few times.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(last_called[0], inner_called)
# When we exit the inner context, the `mjcb_passive` callback should be
# reset to `outer`.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(last_called[0], outer_called)
# When we exit the outer context, the `mjcb_passive` callback should be
# reset to None, and stepping should not affect `last_called`.
last_called[0] = None
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertIsNone(last_called[0])
def testDisableFlags(self):
xml_string = """
<mujoco>
<option gravity="0 0 -9.81"/>
<worldbody>
<geom name="floor" type="plane" pos="0 0 0" size="10 10 0.1"/>
<body name="cube" pos="0 0 0.1">
<geom type="box" size="0.1 0.1 0.1" mass="1"/>
<site name="cube_site" type="box" size="0.1 0.1 0.1"/>
<joint type="slide"/>
</body>
</worldbody>
<sensor>
<touch name="touch_sensor" site="cube_site"/>
</sensor>
</mujoco>
"""
model = core.MjModel.from_xml_string(xml_string)
data = core.MjData(model)
for _ in range(100): # Let the simulation settle for a while.
mjlib.mj_step(model.ptr, data.ptr)
# With gravity and contact enabled, the cube should be stationary and the
# touch sensor should give a reading of ~9.81 N.
self.assertAlmostEqual(data.qvel[0], 0, places=4)
self.assertAlmostEqual(data.sensordata[0], 9.81, places=2)
# If we disable both contacts and gravity then the cube should remain
# stationary and the touch sensor should read zero.
with model.disable("contact", "gravity"):
mjlib.mj_step(model.ptr, data.ptr)
self.assertAlmostEqual(data.qvel[0], 0, places=4)
self.assertEqual(data.sensordata[0], 0)
# If we disable contacts but not gravity then the cube should fall through
# the floor.
with model.disable(enums.mjtDisableBit.mjDSBL_CONTACT):
for _ in range(10):
mjlib.mj_step(model.ptr, data.ptr)
self.assertLess(data.qvel[0], -0.1)
def testDisableFlagsExceptions(self):
with self.assertRaisesRegexp(ValueError, "not a valid flag name"):
with self.model.disable("invalid_flag_name"):
pass
with self.assertRaisesRegexp(ValueError,
"not a value in `enums.mjtDisableBit`"):
with self.model.disable(-99):
pass
@parameterized.named_parameters(
("MjModel", lambda _: core.MjModel.from_xml_path(HUMANOID_XML_PATH),
"mj_deleteModel"),
("MjData", lambda self: core.MjData(self.model), "mj_deleteData"),
("MjvScene", lambda _: core.MjvScene(), "mjv_freeScene"))
def testFree(self, constructor, destructor_name):
for _ in range(5):
destructor = getattr(mjlib, destructor_name)
with mock.patch.object(core.mjlib,
destructor_name,
wraps=destructor) as mock_destructor:
wrapper = constructor(self)
expected_address = ctypes.addressof(wrapper.ptr.contents)
wrapper.free()
self.assertIsNone(wrapper.ptr)
mock_destructor.assert_called_once()
pointer = mock_destructor.call_args[0][0]
actual_address = ctypes.addressof(pointer.contents)
self.assertEqual(expected_address, actual_address)
# Explicit freeing should not break any automatic GC triggered later.
del wrapper
gc.collect()
def testFreeMjrContext(self):
for _ in range(5):
renderer = render.Renderer(640, 480)
with mock.patch.object(
core.mjlib, "mjr_freeContext",
wraps=mjlib.mjr_freeContext) as mock_destructor:
mjr_context = core.MjrContext(self.model, renderer)
expected_address = ctypes.addressof(mjr_context.ptr.contents)
mjr_context.free()
self.assertIsNone(mjr_context.ptr)
mock_destructor.assert_called_once()
pointer = mock_destructor.call_args[0][0]
actual_address = ctypes.addressof(pointer.contents)
self.assertEqual(expected_address, actual_address)
# Explicit freeing should not break any automatic GC triggered later.
del mjr_context
renderer.free()
del renderer
gc.collect()
class CoreTest(parameterized.TestCase):
def setUp(self):
super().setUp()
self.model = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
self.data = core.MjData(self.model)
def _assert_attributes_equal(self, actual_obj, expected_obj,
attr_to_compare):
for name in attr_to_compare:
actual_value = getattr(actual_obj, name)
expected_value = getattr(expected_obj, name)
try:
if isinstance(expected_value, np.ndarray):
np.testing.assert_array_equal(actual_value, expected_value)
else:
self.assertEqual(actual_value, expected_value)
except AssertionError as e:
self.fail(
"Attribute '{}' differs from expected value: {}".format(
name, str(e)))
def testLoadXML(self):
with open(HUMANOID_XML_PATH, "r") as f:
xml_string = f.read()
model = core.MjModel.from_xml_string(xml_string)
core.MjData(model)
with self.assertRaises(TypeError):
core.MjModel()
with self.assertRaises(core.Error):
core.MjModel.from_xml_path("/path/to/nonexistent/model/file.xml")
xml_with_warning = """
<mujoco>
<size njmax='2'/>
<worldbody>
<body pos='0 0 0'>
<geom type='box' size='.1 .1 .1'/>
</body>
<body pos='0 0 0'>
<joint type='slide' axis='1 0 0'/>
<geom type='box' size='.1 .1 .1'/>
</body>
</worldbody>
</mujoco>"""
# This model should compile successfully, but raise a warning on the first
# simulation step.
model = core.MjModel.from_xml_string(xml_with_warning)
data = core.MjData(model)
with mock.patch.object(core, "logging") as mock_logging:
mjlib.mj_step(model.ptr, data.ptr)
mock_logging.warning.assert_called_once_with(
"Pre-allocated constraint buffer is full. Increase njmax above 2. "
"Time = 0.0000.")
def testLoadXMLWithAssetsFromString(self):
core.MjModel.from_xml_string(MODEL_WITH_ASSETS, assets=ASSETS)
with self.assertRaises(core.Error):
# Should fail to load without the assets
core.MjModel.from_xml_string(MODEL_WITH_ASSETS)
def testVFSFilenameTooLong(self):
limit = core._MAX_VFS_FILENAME_CHARACTERS
contents = "fake contents"
valid_filename = "a" * limit
with core._temporary_vfs({valid_filename: contents}):
pass
invalid_filename = "a" * (limit + 1)
expected_message = core._VFS_FILENAME_TOO_LONG.format(
length=(limit + 1), limit=limit, filename=invalid_filename)
with self.assertRaisesWithLiteralMatch(ValueError, expected_message):
with core._temporary_vfs({invalid_filename: contents}):
pass
def testSaveLastParsedModelToXML(self):
save_xml_path = os.path.join(OUT_DIR, "tmp_humanoid.xml")
not_last_parsed = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
last_parsed = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
# Modify the model before saving it in order to confirm that the changes are
# written to the XML.
last_parsed.geom_pos.flat[:] = np.arange(last_parsed.geom_pos.size)
core.save_last_parsed_model_to_xml(save_xml_path,
check_model=last_parsed)
loaded = core.MjModel.from_xml_path(save_xml_path)
self._assert_attributes_equal(last_parsed, loaded, ["geom_pos"])
core.MjData(loaded)
# Test that `check_model` results in a ValueError if it is not the most
# recently parsed model.
with self.assertRaisesWithLiteralMatch(ValueError,
core._NOT_LAST_PARSED_ERROR):
core.save_last_parsed_model_to_xml(save_xml_path,
check_model=not_last_parsed)
def testBinaryIO(self):
bin_path = os.path.join(OUT_DIR, "tmp_humanoid.mjb")
self.model.save_binary(bin_path)
core.MjModel.from_binary_path(bin_path)
byte_string = self.model.to_bytes()
core.MjModel.from_byte_string(byte_string)
def testDimensions(self):
self.assertEqual(self.data.qpos.shape[0], self.model.nq)
self.assertEqual(self.data.qvel.shape[0], self.model.nv)
self.assertEqual(self.model.body_pos.shape, (self.model.nbody, 3))
def testStep(self):
t0 = self.data.time
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(self.data.time, t0 + self.model.opt.timestep)
self.assertTrue(np.all(np.isfinite(self.data.qpos[:])))
self.assertTrue(np.all(np.isfinite(self.data.qvel[:])))
def testMultipleData(self):
data2 = core.MjData(self.model)
self.assertNotEqual(self.data.ptr, data2.ptr)
t0 = self.data.time
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(self.data.time, t0 + self.model.opt.timestep)
self.assertEqual(data2.time, 0)
def testMultipleModel(self):
model2 = core.MjModel.from_xml_path(HUMANOID_XML_PATH)
self.assertNotEqual(self.model.ptr, model2.ptr)
self.model.opt.timestep += 0.001
self.assertEqual(self.model.opt.timestep, model2.opt.timestep + 0.001)
def testModelName(self):
self.assertEqual(self.model.name, "humanoid")
@parameterized.named_parameters(
("_copy", lambda x: x.copy()),
("_pickle_unpickle", lambda x: pickle.loads(pickle.dumps(x))),
)
def testCopyOrPickleModel(self, func):
timestep = 0.12345
self.model.opt.timestep = timestep
body_pos = self.model.body_pos + 1
self.model.body_pos[:] = body_pos
model2 = func(self.model)
self.assertNotEqual(model2.ptr, self.model.ptr)
self.assertEqual(model2.opt.timestep, timestep)
np.testing.assert_array_equal(model2.body_pos, body_pos)
@parameterized.named_parameters(
("_copy", lambda x: x.copy()),
("_pickle_unpickle", lambda x: pickle.loads(pickle.dumps(x))),
)
def testCopyOrPickleData(self, func):
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
data2 = func(self.data)
attr_to_compare = ("time", "energy", "qpos", "xpos")
self.assertNotEqual(data2.ptr, self.data.ptr)
self._assert_attributes_equal(data2, self.data, attr_to_compare)
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
mjlib.mj_step(data2.model.ptr, data2.ptr)
self._assert_attributes_equal(data2, self.data, attr_to_compare)
@parameterized.named_parameters(
("_copy", lambda x: x.copy()),
("_pickle_unpickle", lambda x: pickle.loads(pickle.dumps(x))),
)
def testCopyOrPickleStructs(self, func):
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
data2 = func(self.data)
self.assertNotEqual(data2.ptr, self.data.ptr)
attr_to_compare = ("warning", "timer", "solver")
self._assert_attributes_equal(self.data, data2, attr_to_compare)
for _ in range(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
mjlib.mj_step(data2.model.ptr, data2.ptr)
self._assert_attributes_equal(self.data, data2, attr_to_compare)
@parameterized.parameters(("right_foot", "body", 6),
("right_foot", enums.mjtObj.mjOBJ_BODY, 6),
("left_knee", "joint", 11),
("left_knee", enums.mjtObj.mjOBJ_JOINT, 11))
def testNamesIds(self, name, object_type, object_id):
output_id = self.model.name2id(name, object_type)
self.assertEqual(object_id, output_id)
output_name = self.model.id2name(object_id, object_type)
self.assertEqual(name, output_name)
def testNamesIdsExceptions(self):
with self.assertRaisesRegex(core.Error, "does not exist"):
self.model.name2id("nonexistent_body_name", "body")
with self.assertRaisesRegex(core.Error, "is not a valid object type"):
self.model.name2id("right_foot", "nonexistent_type_name")
def testNamelessObject(self):
# The model in humanoid.xml contains a single nameless camera.
name = self.model.id2name(0, "camera")
self.assertEqual("", name)
def testWarningCallback(self):
self.data.qpos[0] = np.inf
with mock.patch.object(core, "logging") as mock_logging:
mjlib.mju_warning(b"some warning message")
mock_logging.warning.assert_called_once_with("some warning message")
def testErrorCallback(self):
with mock.patch.object(core, "logging") as mock_logging:
mjlib.mju_error(b"some error message")
mock_logging.fatal.assert_called_once_with("some error message")
def testSingleCallbackContext(self):
callback_was_called = [False]
def callback(unused_model, unused_data):
callback_was_called[0] = True
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertFalse(callback_was_called[0])
class DummyError(RuntimeError):
pass
try:
with core.callback_context("mjcb_passive", callback):
# Stepping invokes the `mjcb_passive` callback.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertTrue(callback_was_called[0])
# Exceptions should not prevent `mjcb_passive` from being reset.
raise DummyError("Simulated exception.")
except DummyError:
pass
# `mjcb_passive` should have been reset to None.
callback_was_called[0] = False
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertFalse(callback_was_called[0])
def testNestedCallbackContexts(self):
last_called = [None]
outer_called = "outer called"
inner_called = "inner called"
def outer(unused_model, unused_data):
last_called[0] = outer_called
def inner(unused_model, unused_data):
last_called[0] = inner_called
with core.callback_context("mjcb_passive", outer):
# This should execute `outer` a few times.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(last_called[0], outer_called)
with core.callback_context("mjcb_passive", inner):
# This should execute `inner` a few times.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(last_called[0], inner_called)
# When we exit the inner context, the `mjcb_passive` callback should be
# reset to `outer`.
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertEqual(last_called[0], outer_called)
# When we exit the outer context, the `mjcb_passive` callback should be
# reset to None, and stepping should not affect `last_called`.
last_called[0] = None
mjlib.mj_step(self.model.ptr, self.data.ptr)
self.assertIsNone(last_called[0])
def testDisableFlags(self):
xml_string = """
<mujoco>
<option gravity="0 0 -9.81"/>
<worldbody>
<geom name="floor" type="plane" pos="0 0 0" size="10 10 0.1"/>
<body name="cube" pos="0 0 0.1">
<geom type="box" size="0.1 0.1 0.1" mass="1"/>
<site name="cube_site" type="box" size="0.1 0.1 0.1"/>
<joint type="slide"/>
</body>
</worldbody>
<sensor>
<touch name="touch_sensor" site="cube_site"/>
</sensor>
</mujoco>
"""
model = core.MjModel.from_xml_string(xml_string)
data = core.MjData(model)
for _ in range(100): # Let the simulation settle for a while.
mjlib.mj_step(model.ptr, data.ptr)
# With gravity and contact enabled, the cube should be stationary and the
# touch sensor should give a reading of ~9.81 N.
self.assertAlmostEqual(data.qvel[0], 0, places=4)
self.assertAlmostEqual(data.sensordata[0], 9.81, places=2)
# If we disable both contacts and gravity then the cube should remain
# stationary and the touch sensor should read zero.
with model.disable("contact", "gravity"):
mjlib.mj_step(model.ptr, data.ptr)
self.assertAlmostEqual(data.qvel[0], 0, places=4)
self.assertEqual(data.sensordata[0], 0)
# If we disable contacts but not gravity then the cube should fall through
# the floor.
with model.disable(enums.mjtDisableBit.mjDSBL_CONTACT):
for _ in range(10):
mjlib.mj_step(model.ptr, data.ptr)
self.assertLess(data.qvel[0], -0.1)
def testDisableFlagsExceptions(self):
with self.assertRaisesRegex(ValueError, "not a valid flag name"):
with self.model.disable("invalid_flag_name"):
pass
with self.assertRaisesRegex(ValueError,
"not a value in `enums.mjtDisableBit`"):
with self.model.disable(-99):
pass
@parameterized.named_parameters(
("MjModel", lambda _: core.MjModel.from_xml_path(HUMANOID_XML_PATH),
"mj_deleteModel"),
("MjData", lambda self: core.MjData(self.model), "mj_deleteData"),
("MjvScene", lambda _: core.MjvScene(), "mjv_freeScene"))
def testFree(self, constructor, destructor_name):
for _ in range(5):
destructor = getattr(mjlib, destructor_name)
with mock.patch.object(core.mjlib,
destructor_name,
wraps=destructor) as mock_destructor:
wrapper = constructor(self)
expected_address = ctypes.addressof(wrapper.ptr.contents)
wrapper.free()
self.assertIsNone(wrapper.ptr)
mock_destructor.assert_called_once()
pointer = mock_destructor.call_args[0][0]
actual_address = ctypes.addressof(pointer.contents)
self.assertEqual(expected_address, actual_address)
# Explicit freeing should not break any automatic GC triggered later.
del wrapper
gc.collect()
@parameterized.parameters(
# The tip is .5 meters from the cart so we expect its horizontal velocity
# to be 1m/s + .5m*1rad/s = 1.5m/s.
dict(
qpos=[0., 0.], # Pole pointing upwards.
qvel=[1., 1.],
expected_linvel=[1.5, 0., 0.],
expected_angvel=[0., 1., 0.],
),
# For the same velocities but with the pole pointing down, we expect the
# velocities to cancel, making the global tip velocity now equal to
# 1m/s - 0.5m*1rad/s = 0.5m/s.
dict(
qpos=[0., np.pi], # Pole pointing downwards.
qvel=[1., 1.],
expected_linvel=[0.5, 0., 0.],
expected_angvel=[0., 1., 0.],
),
# In the site's local frame, which is now flipped w.r.t the world, the
# velocity is in the negative x direction.
dict(
qpos=[0., np.pi], # Pole pointing downwards.
qvel=[1., 1.],
expected_linvel=[-0.5, 0., 0.],
expected_angvel=[0., 1., 0.],
local=True,
),
)
def testObjectVelocity(self,
qpos,
qvel,
expected_linvel,
expected_angvel,
local=False):
cartpole = """
<mujoco>
<worldbody>
<body name='cart'>
<joint type='slide' axis='1 0 0'/>
<geom name='cart' type='box' size='0.2 0.2 0.2'/>
<body name='pole'>
<joint name='hinge' type='hinge' axis='0 1 0'/>
<geom name='mass' pos='0 0 .5' size='0.04'/>
</body>
</body>
</worldbody>
</mujoco>
"""
model = core.MjModel.from_xml_string(cartpole)
data = core.MjData(model)
data.qpos[:] = qpos
data.qvel[:] = qvel
mjlib.mj_step1(model.ptr, data.ptr)
linvel, angvel = data.object_velocity("mass",
"geom",
local_frame=local)
np.testing.assert_array_almost_equal(linvel, expected_linvel)
np.testing.assert_array_almost_equal(angvel, expected_angvel)
def testContactForce(self):
box_on_floor = """
<mujoco>
<worldbody>
<geom name='floor' type='plane' size='1 1 1'/>
<body name='box' pos='0 0 .1'>
<freejoint/>
<geom name='box' type='box' size='.1 .1 .1'/>
</body>
</worldbody>
</mujoco>
"""
model = core.MjModel.from_xml_string(box_on_floor)
data = core.MjData(model)
# Settle for 500 timesteps (1 second):
for _ in range(500):
mjlib.mj_step(model.ptr, data.ptr)
normal_force = 0.
for contact_id in range(data.ncon):
force = data.contact_force(contact_id)
normal_force += force[0, 0]
box_id = 1
box_weight = -model.opt.gravity[2] * model.body_mass[box_id]
self.assertAlmostEqual(normal_force, box_weight)
# Test raising of out-of-range errors:
bad_ids = [-1, data.ncon]
for bad_id in bad_ids:
with self.assertRaisesWithLiteralMatch(
ValueError,
core._CONTACT_ID_OUT_OF_RANGE.format(max_valid=data.ncon -
1,
actual=bad_id)):
data.contact_force(bad_id)
@parameterized.parameters(
dict(
condim=3, # Only sliding friction.
expected_torques=[False, False, False], # No torques.
),
dict(
condim=4, # Sliding and torsional friction.
expected_torques=[True, False, False], # Only torsional torque.
),
dict(
condim=6, # Sliding, torsional and rolling.
expected_torques=[True, True, True], # All torques are nonzero.
),
)
def testContactTorque(self, condim, expected_torques):
ball_on_floor = """
<mujoco>
<worldbody>
<geom name='floor' type='plane' size='1 1 1'/>
<body name='ball' pos='0 0 .1'>
<freejoint/>
<geom name='ball' size='.1' friction='1 .1 .1'/>
</body>
</worldbody>
</mujoco>
"""
model = core.MjModel.from_xml_string(ball_on_floor)
data = core.MjData(model)
model.geom_condim[:] = condim
data.qvel[3:] = np.array((1., 1., 1.))
# Settle for 10 timesteps (20 milliseconds):
for _ in range(10):
mjlib.mj_step(model.ptr, data.ptr)
contact_id = 0 # This model has only one contact.
_, torque = data.contact_force(contact_id)
nonzero_torques = torque != 0
np.testing.assert_array_equal(nonzero_torques,
np.array((expected_torques)))
def testFreeMjrContext(self):
for _ in range(5):
renderer = _render.Renderer(640, 480)
with mock.patch.object(
core.mjlib, "mjr_freeContext",
wraps=mjlib.mjr_freeContext) as mock_destructor:
mjr_context = core.MjrContext(self.model, renderer)
expected_address = ctypes.addressof(mjr_context.ptr.contents)
mjr_context.free()
self.assertIsNone(mjr_context.ptr)
mock_destructor.assert_called_once()
pointer = mock_destructor.call_args[0][0]
actual_address = ctypes.addressof(pointer.contents)
self.assertEqual(expected_address, actual_address)
# Explicit freeing should not break any automatic GC triggered later.
del mjr_context
renderer.free()
del renderer
gc.collect()
def testSceneGeomsAttribute(self):
scene = core.MjvScene(model=self.model)
self.assertEqual(scene.ngeom, 0)
self.assertEmpty(scene.geoms)
geom_types = (enums.mjtObj.mjOBJ_BODY, enums.mjtObj.mjOBJ_GEOM,
enums.mjtObj.mjOBJ_SITE)
for geom_type in geom_types:
scene.ngeom += 1
scene.ptr.contents.geoms[scene.ngeom - 1].objtype = geom_type
self.assertLen(scene.geoms, len(geom_types))
np.testing.assert_array_equal(scene.geoms.objtype, geom_types)
def testInvalidFontScale(self):
invalid_font_scale = 99
with self.assertRaisesWithLiteralMatch(
ValueError,
core._INVALID_FONT_SCALE.format(invalid_font_scale)):
core.MjrContext(
model=self.model,
gl_context=None, # Don't need a context for this test.
font_scale=invalid_font_scale)