예제 #1
0
    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)
예제 #2
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])
예제 #3
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)
예제 #4
0
 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])
예제 #5
0
    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])
예제 #6
0
    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)]))
예제 #7
0
 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)])
예제 #8
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])
예제 #9
0
 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])
예제 #10
0
 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)])
예제 #11
0
    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))
예제 #12
0
 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])
예제 #13
0
    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)
예제 #14
0
 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])
예제 #15
0
 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])
예제 #16
0
 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])
예제 #17
0
    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)
예제 #18
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])
예제 #19
0
    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)
예제 #20
0
    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])
예제 #21
0
    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)
예제 #22
0
 def __init__(self, **kwargs):
     sim_scene = MockSimScene(nq=1)  # type: Any
     super().__init__(sim_scene=sim_scene, **kwargs)
예제 #23
0
 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))
예제 #24
0
    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)