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)
