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[:])))