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 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 step(self, n_sub_steps=1):
"""Advances physics with up-to-date position and velocity dependent fields.
The actuation can be updated by calling the `set_control` function first.
Args:
n_sub_steps: Optional number of times to advance the physics. Default 1.
"""
# In the case of Euler integration we assume mj_step1 has already been
# called for this state, finish the step with mj_step2 and then update all
# position and velocity related fields with mj_step1. This ensures that
# (most of) mjData is in sync with qpos and qvel. In the case of non-Euler
# integrators (e.g. RK4) an additional mj_step1 must be called after the
# last mj_step to ensure mjData syncing.
for _ in xrange(n_sub_steps):
if self.model.opt.integrator == enums.mjtIntegrator.mjINT_EULER:
mjlib.mj_step2(self.model.ptr, self.data.ptr)
mjlib.mj_step1(self.model.ptr, self.data.ptr)
else:
mjlib.mj_step(self.model.ptr, self.data.ptr)
if self.model.opt.integrator != enums.mjtIntegrator.mjINT_EULER:
mjlib.mj_step1(self.model.ptr, self.data.ptr)
self.check_divergence()
def testNanControl(self):
with self._physics.reset_context():
self._physics.data.ctrl[0] = float('nan')
# Apply the controls.
mjlib.mj_step(self._physics.model.ptr, self._physics.data.ptr)
with self.assertRaisesRegexp(control.PhysicsError, 'mjWARN_BADCTRL'):
self._physics.check_invalid_state()
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 step(self):
"""Advances physics with up-to-date position and velocity dependent fields.
The actuation can be updated by calling the `set_control` function first.
"""
# In the case of Euler integration we assume mj_step1 has already been
# called for this state, finish the step with mj_step2 and then update all
# position and velocity related fields with mj_step1. This ensures that
# (most of) mjData is in sync with qpos and qvel. In the case of non-Euler
# integrators (e.g. RK4) an additional mj_step1 must be called after the
# last mj_step to ensure mjData syncing.
# print("applying", self.named.data.ctrl)
with self.check_invalid_state():
if self.model.opt.integrator == enums.mjtIntegrator.mjINT_EULER:
mjlib.mj_step2(self.model.ptr, self.data.ptr)
else:
mjlib.mj_step(self.model.ptr, self.data.ptr)
mjlib.mj_step1(self.model.ptr, self.data.ptr)
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 testCopyOrPickleData(self, func):
for _ in xrange(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 xrange(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)
def testCopyOrPickleStructs(self, func):
for _ in xrange(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
data2 = func(self.data)
self.assertNotEqual(data2.ptr, self.data.ptr)
for name in ["warning", "timer", "solver"]:
self._assert_structs_equal(getattr(self.data, name),
getattr(data2, name))
for _ in xrange(10):
mjlib.mj_step(self.model.ptr, self.data.ptr)
mjlib.mj_step(data2.model.ptr, data2.ptr)
for expected, actual in zip(self.data.timer, data2.timer):
self._assert_structs_equal(expected, actual)
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 xrange(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 xrange(10):
mjlib.mj_step(model.ptr, data.ptr)
self.assertLess(data.qvel[0], -0.1)
def _take_steps(self, n=5):
for _ in xrange(n):
mjlib.mj_step(self.model.ptr, self.data.ptr)
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.assert_(np.all(np.isfinite(self.data.qpos[:])))
self.assert_(np.all(np.isfinite(self.data.qvel[:])))
