def test_termination_and_discount(self): walker = cmu_humanoid.CMUHumanoid() arena = corridor_arenas.EmptyCorridor() task = corridor_tasks.RunThroughCorridor(walker, arena) random_state = np.random.RandomState(12345) env = composer.Environment(task, random_state=random_state) env.reset() zero_action = np.zeros_like(env.physics.data.ctrl) # Walker starts in upright position. # Should not trigger failure termination in the first few steps. for _ in range(5): env.step(zero_action) self.assertFalse(task.should_terminate_episode(env.physics)) self.assertEqual(task.get_discount(env.physics), 1) # Rotate the walker upside down and run the physics until it makes contact. current_time = env.physics.data.time walker.shift_pose(env.physics, position=(0, 0, 10), quaternion=(0, 1, 0, 0)) env.physics.forward() while env.physics.data.ncon == 0: env.physics.step() env.physics.data.time = current_time # Should now trigger a failure termination. env.step(zero_action) self.assertTrue(task.should_terminate_episode(env.physics)) self.assertEqual(task.get_discount(env.physics), 0)
def test_reward_fixed_target(self): walker = cmu_humanoid.CMUHumanoid() arena = floors.Floor() task = go_to_target.GoToTarget(walker=walker, arena=arena, moving_target=False) random_state = np.random.RandomState(12345) env = composer.Environment(task, random_state=random_state) env.reset() target_position = task.target_position(env.physics) zero_action = np.zeros_like(env.physics.data.ctrl) for _ in range(2): timestep = env.step(zero_action) self.assertEqual(timestep.reward, 0) walker_pos = env.physics.bind(walker.root_body).xpos walker.set_pose( env.physics, position=[target_position[0], target_position[1], walker_pos[2]]) env.physics.forward() # Receive reward while the agent remains at that location. timestep = env.step(zero_action) self.assertEqual(timestep.reward, 1) # Target position should not change. np.testing.assert_array_equal(target_position, task.target_position(env.physics))
def test_cmu_humanoid_position_controlled_has_correct_actuators(self): walker_torque = cmu_humanoid.CMUHumanoid() walker_pos = cmu_humanoid.CMUHumanoidPositionControlled() actuators_torque = walker_torque.mjcf_model.find_all('actuator') actuators_pos = walker_pos.mjcf_model.find_all('actuator') actuator_pos_params = { params.name: params for params in cmu_humanoid._POSITION_ACTUATORS } self.assertEqual(len(actuators_torque), len(actuators_pos)) for actuator_torque, actuator_pos in zip(actuators_torque, actuators_pos): self.assertEqual(actuator_pos.name, actuator_torque.name) self.assertEqual(actuator_pos.joint.full_identifier, actuator_torque.joint.full_identifier) self.assertEqual(actuator_pos.tag, 'general') self.assertEqual(actuator_pos.ctrllimited, 'true') np.testing.assert_array_equal(actuator_pos.ctrlrange, (-1, 1)) expected_params = actuator_pos_params[actuator_pos.name] self.assertEqual(actuator_pos.biasprm[1], -expected_params.kp) np.testing.assert_array_equal(actuator_pos.forcerange, expected_params.forcerange)
def test_termination_and_discount(self): walker = cmu_humanoid.CMUHumanoid() # Build a maze with rooms and targets. skybox_texture = labmaze_textures.SkyBox(style='sky_03') wall_textures = labmaze_textures.WallTextures(style='style_01') floor_textures = labmaze_textures.FloorTextures(style='style_01') arena = mazes.RandomMazeWithTargets( x_cells=11, y_cells=11, xy_scale=3, max_rooms=4, room_min_size=4, room_max_size=5, spawns_per_room=1, targets_per_room=3, skybox_texture=skybox_texture, wall_textures=wall_textures, floor_textures=floor_textures, ) task = random_goal_maze.ManyGoalsMaze( walker=walker, maze_arena=arena, target_builder=functools.partial(target_sphere.TargetSphere, radius=0.4, rgb1=(0, 0, 0.4), rgb2=(0, 0, 0.7)), control_timestep=.03, physics_timestep=.005, ) random_state = np.random.RandomState(12345) env = composer.Environment(task, random_state=random_state) env.reset() zero_action = np.zeros_like(env.physics.data.ctrl) # Walker starts in upright position. # Should not trigger failure termination in the first few steps. for _ in range(5): env.step(zero_action) self.assertFalse(task.should_terminate_episode(env.physics)) np.testing.assert_array_equal(task.get_discount(env.physics), 1) # Rotate the walker upside down and run the physics until it makes contact. current_time = env.physics.data.time walker.shift_pose(env.physics, position=(0, 0, 10), quaternion=(0, 1, 0, 0)) env.physics.forward() while env.physics.data.ncon == 0: env.physics.step() env.physics.data.time = current_time # Should now trigger a failure termination. env.step(zero_action) self.assertTrue(task.should_terminate_episode(env.physics))
def test_walker_is_correctly_reinitialized( self, position_offset, rotate_180_degrees, use_variations): walker_spawn_position = position_offset if not rotate_180_degrees: walker_spawn_rotation = None else: walker_spawn_rotation = np.pi if use_variations: walker_spawn_position = deterministic.Constant(position_offset) walker_spawn_rotation = deterministic.Constant(walker_spawn_rotation) walker = cmu_humanoid.CMUHumanoid() arena = corridor_arenas.EmptyCorridor() task = corridor_tasks.RunThroughCorridor( walker=walker, arena=arena, walker_spawn_position=walker_spawn_position, walker_spawn_rotation=walker_spawn_rotation) # Randomize the initial pose and joint positions in order to check that they # are set correctly by `initialize_episode`. random_state = np.random.RandomState(12345) task.initialize_episode_mjcf(random_state) physics = mjcf.Physics.from_mjcf_model(task.root_entity.mjcf_model) walker_joints = walker.mjcf_model.find_all('joint') physics.bind(walker_joints).qpos = random_state.uniform( size=len(walker_joints)) walker.set_pose(physics, position=random_state.uniform(size=3), quaternion=rotations.UniformQuaternion()(random_state)) task.initialize_episode(physics, random_state) physics.forward() with self.subTest('Correct joint positions'): walker_qpos = physics.bind(walker_joints).qpos np.testing.assert_array_equal(walker_qpos, walker.upright_pose.qpos) walker_xpos, walker_xquat = walker.get_pose(physics) with self.subTest('Correct position'): expected_xpos = walker.upright_pose.xpos + np.array(position_offset) np.testing.assert_array_equal(walker_xpos, expected_xpos) with self.subTest('Correct orientation'): upright_xquat = walker.upright_pose.xquat.copy() upright_xquat /= np.linalg.norm(walker.upright_pose.xquat) if rotate_180_degrees: expected_xquat = (-upright_xquat[3], -upright_xquat[2], upright_xquat[1], upright_xquat[0]) else: expected_xquat = upright_xquat np.testing.assert_allclose(walker_xquat, expected_xquat)
def test_observables(self): walker = cmu_humanoid.CMUHumanoid() arena = floors.Floor() task = go_to_target.GoToTarget(walker=walker, arena=arena, moving_target=False) random_state = np.random.RandomState(12345) env = composer.Environment(task, random_state=random_state) timestep = env.reset() self.assertIn('walker/target', timestep.observation)
def test_actuator_to_mocap_joint_mapping(self): walker = cmu_humanoid.CMUHumanoid() with self.subTest('Forward mapping'): for actuator_num, cmu_mocap_joint_num in enumerate(walker.actuator_order): self.assertEqual(walker.actuator_to_joint_order[cmu_mocap_joint_num], actuator_num) with self.subTest('Inverse mapping'): for cmu_mocap_joint_num, actuator_num in enumerate( walker.actuator_to_joint_order): self.assertEqual(walker.actuator_order[actuator_num], cmu_mocap_joint_num)
def test_target_position_randomized_on_reset(self): walker = cmu_humanoid.CMUHumanoid() arena = floors.Floor() task = go_to_target.GoToTarget(walker=walker, arena=arena, moving_target=False) random_state = np.random.RandomState(12345) env = composer.Environment(task, random_state=random_state) env.reset() first_target_position = task.target_position(env.physics) env.reset() second_target_position = task.target_position(env.physics) self.assertFalse( np.all(first_target_position == second_target_position), 'Target positions are unexpectedly identical.')
def test_observables(self): walker = cmu_humanoid.CMUHumanoid() # Build a maze with rooms and targets. skybox_texture = labmaze_textures.SkyBox(style='sky_03') wall_textures = labmaze_textures.WallTextures(style='style_01') floor_textures = labmaze_textures.FloorTextures(style='style_01') arena = mazes.RandomMazeWithTargets( x_cells=11, y_cells=11, xy_scale=3, max_rooms=4, room_min_size=4, room_max_size=5, spawns_per_room=1, targets_per_room=3, skybox_texture=skybox_texture, wall_textures=wall_textures, floor_textures=floor_textures, ) task = random_goal_maze.ManyGoalsMaze( walker=walker, maze_arena=arena, target_builder=functools.partial(target_sphere.TargetSphere, radius=0.4, rgb1=(0, 0, 0.4), rgb2=(0, 0, 0.7)), control_timestep=.03, physics_timestep=.005, ) random_state = np.random.RandomState(12345) env = composer.Environment(task, random_state=random_state) timestep = env.reset() self.assertIn('walker/joints_pos', timestep.observation)
def _build_task(**task_kwargs): walker = cmu_humanoid.CMUHumanoid() arena = floors.Floor() task = go_to_target.GoToTarget(walker=walker, arena=arena, **task_kwargs) return task
def test_get_element_property(self, name): attribute_value = getattr(cmu_humanoid.CMUHumanoid(), name) self.assertIsInstance(attribute_value, mjcf.Element)
def test_proprioception(self): walker = cmu_humanoid.CMUHumanoid() for item in walker.observables.proprioception: self.assertIsInstance(item, observable_base.Observable)
def test_evaluate_observable(self, name): walker = cmu_humanoid.CMUHumanoid() observable = getattr(walker.observables, name) physics = mjcf.Physics.from_mjcf_model(walker.mjcf_model) observation = observable(physics) self.assertIsInstance(observation, (float, np.ndarray))
def test_set_marker_rgba(self): marker_rgba = (1., 0., 1., 0.5) walker = cmu_humanoid.CMUHumanoid(marker_rgba=marker_rgba) for marker_geom in walker.marker_geoms: np.testing.assert_array_equal(marker_geom.rgba, marker_rgba)
def test_set_name(self): name = 'fred' walker = cmu_humanoid.CMUHumanoid(name=name) self.assertEqual(walker.mjcf_model.model, name)
def test_get_element_tuple_property(self, name): attribute_value = getattr(cmu_humanoid.CMUHumanoid(), name) self.assertNotEmpty(attribute_value) for item in attribute_value: self.assertIsInstance(item, mjcf.Element)