def test_explicit_init(self):
"""Tests initialization with explicit values."""
scene = MockSimScene(nq=2,
nv=3,
nu=4,
ctrl_range=(-2, 2),
body_names=['test0'],
geom_names=['test0', 'test1'],
site_names=['test0', 'test1', 'test2'],
cam_names=['cam0'],
step_duration=0.5)
self.assertEqual(scene.data.qpos.size, 2)
self.assertEqual(scene.data.qvel.size, 3)
self.assertEqual(scene.data.qacc.size, 3)
self.assertEqual(scene.data.ctrl.size, 4)
np.testing.assert_array_equal(scene.model.actuator_ctrlrange,
[(-2, 2)] * 4)
np.testing.assert_array_equal(scene.model.body_pos, np.zeros((1, 3)))
np.testing.assert_array_equal(scene.model.geom_pos, np.zeros((2, 3)))
np.testing.assert_array_equal(scene.model.site_pos, np.zeros((3, 3)))
np.testing.assert_array_equal(scene.model.cam_pos, np.zeros((1, 3)))
self.assertEqual(scene.model.body_name2id('test0'), 0)
self.assertEqual(scene.model.geom_name2id('test1'), 1)
self.assertEqual(scene.model.site_name2id('test2'), 2)
self.assertEqual(scene.model.camera_name2id('cam0'), 0)
def test_get_state(self):
"""Tests querying the state of multiple groups."""
sim_scene = MockSimScene(nq=10) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 3, 5],
'motor_ids': [10, 20, 12, 21],
'calib_scale': [0.5] * 4,
'calib_offset': [1] * 4,
},
'b': {
'qpos_indices': [2, 6],
},
'c': {
'motor_ids': [22, 24],
},
},
device_path='test')
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test']
dxl.write_desired_pos([10, 12, 20, 21, 22, 24], [1, 2, 3, 4, 5, 6])
a_state, b_state, c_state = robot.get_state(['a', 'b', 'c'])
np.testing.assert_allclose(a_state.qpos, [1.5, 2.5, 2., 3.])
np.testing.assert_allclose(a_state.qvel, [0., 0., 0., 0.])
self.assertIsNone(b_state.qpos)
self.assertIsNone(b_state.qvel)
np.testing.assert_allclose(c_state.qpos, [5., 6.])
np.testing.assert_allclose(c_state.qvel, [0., 0.])
np.testing.assert_allclose(sim_scene.data.qpos,
[1.5, 2.5, 0, 2., 0, 3., 0, 0, 0, 0])
def test_engage_motors(self):
"""Tests engaging/disengaging subsets of motors."""
sim_scene = MockSimScene(nq=10) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'motor_ids': [10, 11, 12],
},
'b': {
'motor_ids': [13, 14, 15],
},
'c': {
'motor_ids': [12, 15],
}
},
device_path='test')
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test'] # type: Any
np.testing.assert_array_equal(dxl.enabled, [False] * 6)
robot.set_motors_engaged('a', True)
np.testing.assert_array_equal(dxl.enabled, [True] * 3 + [False] * 3)
robot.set_motors_engaged('c', False)
np.testing.assert_array_equal(dxl.enabled,
[True, True, False, False, False, False])
robot.set_motors_engaged(['a', 'b'], True)
np.testing.assert_array_equal(dxl.enabled, [True] * 6)
def test_qvel_out_of_range(self):
"""Ensures error when qvel indices are out of bounds."""
sim_scene = MockSimScene(nq=1, nv=3) # type: Any
with self.assertRaises(AssertionError):
RobotGroupConfig(sim_scene, qvel_indices=[3])
with self.assertRaises(AssertionError):
RobotGroupConfig(sim_scene, qvel_indices=[-4])
def test_step(self):
"""Tests stepping with an action for multiple groups."""
sim_scene = MockSimScene(nq=10, ctrl_range=[-1, 1]) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 2],
'motor_ids': [10, 11, 12],
'calib_offset': [1] * 3,
'calib_scale': [-1] * 3,
'qpos_range':
[(-0.5, 0.5)] * 3,
},
'b': {
'qpos_indices': [2, 3],
},
},
device_path='test')
with patch_time('robel.components.robot.hardware_robot.time'):
robot.step({
'a': np.array([.2, .4, .6]),
'b': np.array([.1, .3]),
})
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test'] # type: Any
np.testing.assert_allclose(dxl.qpos, [.9, .8, .7])
def test_qpos_range(self):
"""Checks presence of qpos_range when provided."""
sim_scene = MockSimScene(nq=2) # type: Any
config = RobotGroupConfig(sim_scene,
qpos_indices=[0, 1],
qpos_range=[(0, 1)] * 2)
np.testing.assert_array_equal(config.qpos_range,
np.array([(0, 1), (0, 1)]))
def test_qpos_invalid_range(self):
"""Ensures error when invalid qpos range is given."""
sim_scene = MockSimScene(nq=2) # type: Any
with self.assertRaises(AssertionError):
RobotGroupConfig(sim_scene,
qpos_indices=[0, 1],
qpos_range=[(0, 1)])
with self.assertRaises(AssertionError):
RobotGroupConfig(sim_scene, qpos_indices=[0], qpos_range=[(1, 0)])
def test_qvel_indices(self):
"""Checks defaults when nq == nu != nv."""
sim_scene = MockSimScene(nq=3, nv=5) # type: Any
config = RobotGroupConfig(sim_scene,
qpos_indices=[-1],
qvel_indices=[3, 4])
np.testing.assert_array_equal(config.qpos_indices, [-1])
np.testing.assert_array_equal(config.qvel_indices, [3, 4])
np.testing.assert_array_equal(config.actuator_indices, [-1])
def test_step_denormalize(self):
"""Tests denormalizing the actions to the sim control range."""
sim_scene = MockSimScene(nq=5, ctrl_range=[0, 10]) # type: Any
robot = RobotComponent(
sim_scene, groups={'a': {
'qpos_indices': [0, 1, 2, 3, 4],
}})
robot.step({
'a': np.array([-1, 1, -0.5, 0.5, 0]),
})
np.testing.assert_allclose(sim_scene.data.ctrl, [0, 10, 2.5, 7.5, 5])
def test_qvel_invalid_range(self):
"""Ensures error when invalid qvel range is given."""
sim_scene = MockSimScene(nq=2, nv=3) # type: Any
with self.assertRaises(AssertionError):
RobotGroupConfig(sim_scene,
qvel_indices=[0, 2],
qvel_range=[(-1, 1)] * 3)
with self.assertRaises(AssertionError):
RobotGroupConfig(sim_scene,
qvel_indices=[0],
qpos_range=[(-1, -2)])
def test_qpos_indices(self):
"""Checks defaults when nq == nv == nu."""
sim_scene = MockSimScene(nq=5, ctrl_range=(-1, 1)) # type: Any
config = RobotGroupConfig(sim_scene, qpos_indices=range(5))
result = np.array([0, 1, 2, 3, 4], dtype=int)
np.testing.assert_array_equal(config.qpos_indices, result)
np.testing.assert_array_equal(config.qvel_indices, result)
np.testing.assert_array_equal(config.actuator_indices, result)
np.testing.assert_array_equal(config.denormalize_center, np.zeros(5))
np.testing.assert_array_equal(config.denormalize_range, np.ones(5))
def test_decalibrate_qpos(self):
"""Converts a component state qpos to hardware space."""
sim_scene = MockSimScene(nq=1) # type: Any
robot = DummyHardwareRobotComponent(sim_scene,
groups={
'a': {
'calib_scale':
[0.5, -1, 1],
'calib_offset':
[2, 0, -0.5],
}
})
qpos = robot.decalibrate_qpos(np.array([1., 2., 3.]), 'a')
np.testing.assert_allclose(qpos, [-2, -2, 3.5])
def test_actuator_range(self):
"""Checks presence of actuator_range when provided."""
sim_scene = MockSimScene(nq=2, nu=3) # type: Any
config = RobotGroupConfig(
sim_scene,
qpos_indices=[0, 1],
qpos_range=[(0, 1)] * 2,
actuator_indices=[0, 1, 2],
actuator_range=[(-1, 3)] * 3,
)
np.testing.assert_array_equal(config.actuator_range, [(-1, 3)] * 3)
np.testing.assert_array_equal(config.denormalize_center, [1.] * 3)
np.testing.assert_array_equal(config.denormalize_range, [2.] * 3)
def test_step_position_control_bounds(self):
"""Tests action clamping when doing position control."""
sim_scene = MockSimScene(nq=5, ctrl_range=[-1, 1]) # type: Any
sim_scene.data.qpos[:] = [-0.4, -0.2, 0, 0.2, 0.4]
robot = RobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 2, 3, 4],
'qpos_range': [(-0.5, 0.5)] * 5,
'qvel_range': [(-0.2, 0.2)] * 5,
}
})
robot.step({'a': np.array([-1, -1, 0.2, 1, 1])})
np.testing.assert_allclose(sim_scene.data.ctrl,
[-0.5, -0.4, 0.1, 0.4, 0.5])
def test_step_velocity_control_bounds(self):
"""Tests action clamping when doing velocity control."""
sim_scene = MockSimScene(nq=3, ctrl_range=[-10, 10]) # type: Any
robot = RobotComponent(sim_scene,
groups={
'a': {
'control_mode':
ControlMode.JOINT_VELOCITY,
'qpos_indices': [0, 1, 2],
'qvel_range': [(-2, 2), (-1, 5),
(-7, -4)],
}
})
robot.step({'a': np.array([-0.5, 1, -1])})
np.testing.assert_allclose(sim_scene.data.ctrl, [-1, 5, -7])
def test_calibrate_state(self):
"""Converts a state to component space."""
sim_scene = MockSimScene(nq=1) # type: Any
robot = DummyHardwareRobotComponent(sim_scene,
groups={
'a': {
'calib_scale':
[0.5, -1, 1],
'calib_offset':
[2, 0, -0.5],
}
})
state = RobotState(qpos=np.array([1., 1., 1.]),
qvel=np.array([1., 1., 1.]))
robot.calibrate_state(state, 'a')
np.testing.assert_allclose(state.qpos, [2.5, -1, 0.5])
np.testing.assert_allclose(state.qvel, [0.5, -1, 1])
def test_timestep(self):
"""Tests advancement of time when doing timesteps."""
with patch_time('robel.components.robot.hardware_robot.time',
initial_time=0) as mock_time:
sim_scene = MockSimScene(nq=1, step_duration=0.5) # type: Any
robot = DummyHardwareRobotComponent(sim_scene, groups={})
self.assertEqual(robot.time, 0)
robot.do_timestep()
self.assertAlmostEqual(robot.time, 0.5)
mock_time.sleep(0.25)
robot.do_timestep()
self.assertAlmostEqual(robot.time, 1.0)
mock_time.sleep(0.6)
robot.do_timestep()
self.assertAlmostEqual(robot.time, 1.6)
robot.reset_time()
self.assertEqual(robot.time, 0)
def test_get_state(self):
"""Tests querying the state of multiple groups."""
sim_scene = MockSimScene(nq=10) # type: Any
sim_scene.data.qpos[:] = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
sim_scene.data.qvel[:] = [11, 12, 13, 14, 15, 16, 17, 18, 19, 20]
robot = RobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 3, 5],
},
'b': {
'qpos_indices': [2, 6],
'qvel_indices': [7, 8, 9],
},
})
a_state, b_state = robot.get_state(['a', 'b'])
np.testing.assert_array_equal(a_state.qpos, [1, 2, 4, 6])
np.testing.assert_array_equal(a_state.qvel, [11, 12, 14, 16])
np.testing.assert_array_equal(b_state.qpos, [3, 7])
np.testing.assert_array_equal(b_state.qvel, [18, 19, 20])
def test_step(self):
"""Tests stepping with an action for multiple groups."""
sim_scene = MockSimScene(nq=10, ctrl_range=[-1, 1]) # type: Any
robot = RobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 3, 5],
},
'b': {
'actuator_indices': [7, 8, 9],
},
})
robot.step({
'a': np.array([.2, .4, .6, .8]),
'b': np.array([.1, .3, .5])
})
np.testing.assert_allclose(sim_scene.data.ctrl,
[.2, .4, 0, .6, 0, .8, 0, .1, .3, .5])
self.assertEqual(sim_scene.advance.call_count, 1)
def test_set_state(self):
"""Tests stepping with an action for multiple groups."""
sim_scene = MockSimScene(nq=10) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 2],
'motor_ids': [10, 11, 12],
'calib_offset': [-1] * 3,
'qpos_range':
[(-2.5, 2.5)] * 3,
},
},
device_path='test')
with patch_time('robel.components.robot.hardware_robot.time'):
robot.set_state({
'a': RobotState(qpos=np.array([1, 2, 3])),
})
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test'] # type: Any
np.testing.assert_allclose(dxl.qpos, [2, 3, 3.5])
def test_defaults(self):
"""Tests default initialization of the sim scene."""
scene = MockSimScene(nq=2)
# Ensure that properties exist.
self.assertIsNotNone(scene.sim)
self.assertIsNotNone(scene.model)
self.assertIsNotNone(scene.data)
self.assertIsNotNone(scene.step_duration)
self.assertIsNotNone(scene.close)
self.assertIsNotNone(scene.advance)
np.testing.assert_array_equal(scene.model.actuator_ctrlrange,
[(-1, 1), (-1, 1)])
# Check that sizes are consistent.
self.assertEqual(scene.model.nq, 2)
self.assertEqual(scene.model.nv, 2)
self.assertEqual(scene.model.nu, 2)
self.assertEqual(scene.data.qpos.size, 2)
self.assertEqual(scene.data.qvel.size, 2)
self.assertEqual(scene.data.qacc.size, 2)
self.assertEqual(scene.data.ctrl.size, 2)
def __init__(self, **kwargs):
sim_scene = MockSimScene(nq=1) # type: Any
super().__init__(sim_scene=sim_scene, **kwargs)
def test_render_offscreen(self):
"""Tests mock rendering."""
scene = MockSimScene(nq=1)
image = scene.renderer.render_offscreen(16, 16)
self.assertEqual(image.shape, (16, 16))
def test_actuator_range_default(self):
"""Checks that actuator_range uses the simulation range by default."""
sim_scene = MockSimScene(nq=2) # type: Any
config = RobotGroupConfig(sim_scene, qpos_indices=[0, 1])
np.testing.assert_array_equal(config.actuator_range, [(-1, 1)] * 2)