def __init__(
self, arena, arm, hand, prop, obs_settings, workspace, control_timestep):
"""Initializes a new `Reach` task.
Args:
arena: `composer.Entity` instance.
arm: `robot_base.RobotArm` instance.
hand: `robot_base.RobotHand` instance.
prop: `composer.Entity` instance specifying the prop to reach to, or None
in which case the target is a fixed site whose position is specified by
the workspace.
obs_settings: `observations.ObservationSettings` instance.
workspace: `_ReachWorkspace` specifying the placement of the prop and TCP.
control_timestep: Float specifying the control timestep in seconds.
"""
self._arena = arena
self._arm = arm
self._hand = hand
self._arm.attach(self._hand)
self._arena.attach_offset(self._arm, offset=workspace.arm_offset)
self.control_timestep = control_timestep
self._tcp_initializer = initializers.ToolCenterPointInitializer(
self._hand, self._arm,
position=distributions.Uniform(*workspace.tcp_bbox),
quaternion=workspaces.DOWN_QUATERNION)
# Add custom camera observable.
self._task_observables = cameras.add_camera_observables(
arena, obs_settings, cameras.FRONT_CLOSE)
target_pos_distribution = distributions.Uniform(*workspace.target_bbox)
self._prop = prop
if prop:
# The prop itself is used to visualize the target location.
self._make_target_site(parent_entity=prop, visible=False)
self._target = self._arena.add_free_entity(prop)
self._prop_placer = initializers.PropPlacer(
props=[prop],
position=target_pos_distribution,
quaternion=workspaces.uniform_z_rotation,
settle_physics=True)
else:
self._target = self._make_target_site(parent_entity=arena, visible=True)
self._target_placer = target_pos_distribution
obs = observable.MJCFFeature('pos', self._target)
obs.configure(**obs_settings.prop_pose._asdict())
self._task_observables['target_position'] = obs
# Add sites for visualizing the prop and target bounding boxes.
workspaces.add_bbox_site(
body=self.root_entity.mjcf_model.worldbody,
lower=workspace.tcp_bbox.lower, upper=workspace.tcp_bbox.upper,
rgba=constants.GREEN, name='tcp_spawn_area')
workspaces.add_bbox_site(
body=self.root_entity.mjcf_model.worldbody,
lower=workspace.target_bbox.lower, upper=workspace.target_bbox.upper,
rgba=constants.BLUE, name='target_spawn_area')
def position(self):
"""Cartesian positions of all marker sites.
Returns:
An `observable.MJCFFeature` instance. When called with an instance of
`physics` as the argument, this will return a numpy float64 array of shape
(num_players * num_markers, 3) where each row contains the cartesian
position of a marker. Unplaced markers will have position (0, 0, 0).
"""
return observable.MJCFFeature(
'xpos', list(itertools.chain.from_iterable(self._entity.markers)))
def camera_joints_pos(self):
def _sin(value, random_state):
del random_state
return np.sin(value)
def _cos(value, random_state):
del random_state
return np.cos(value)
sin_rotation_joints = observable.MJCFFeature(
'qpos', self._entity.observable_camera_joints, corruptor=_sin)
cos_rotation_joints = observable.MJCFFeature(
'qpos', self._entity.observable_camera_joints, corruptor=_cos)
def _camera_joints(physics):
return np.concatenate([
sin_rotation_joints(physics),
cos_rotation_joints(physics)
], -1)
return observable.Generic(_camera_joints)
def sensors_gyro(self):
return observable.MJCFFeature('sensordata',
self._entity.mjcf_model.sensor.gyro)
def sensors_torque(self):
return observable.MJCFFeature('sensordata',
self._entity.mjcf_model.sensor.torque)
def actuator_activation(self):
return observable.MJCFFeature(
'act', self._entity.mjcf_model.find_all('actuator'))
def head_height(self):
return observable.MJCFFeature('xpos', self._entity.head)[2]
def __init__(self, arena, arm, hand, prop, obs_settings, workspace,
control_timestep, table_col_tag):
"""Initializes a new `Reach` task.
Args:
arena: `composer.Entity` instance.
arm: `robot_base.RobotArm` instance.
hand: `robot_base.RobotHand` instance.
prop: `composer.Entity` instance specifying the prop to reach to, or None
in which case the target is a fixed site whose position is specified by
the workspace.
obs_settings: `observations.ObservationSettings` instance.
workspace: `_ReachWorkspace` specifying the placement of the prop and TCP.
control_timestep: Float specifying the control timestep in seconds.
"""
self._arena = arena
self._arm = arm
self._hand = hand
self._arm.attach(self._hand)
self._arena.attach_offset(self._arm, offset=workspace.arm_offset)
self.control_timestep = control_timestep
self._tcp_initializer = initializers.ToolCenterPointInitializer(
self._hand,
self._arm,
position=distributions.Uniform(*workspace.tcp_bbox),
quaternion=workspaces.DOWN_QUATERNION)
# Add custom camera observable.
self._task_observables = cameras.add_camera_observables(
arena, obs_settings, cameras.FRONT_FAR)
target_pos_distribution = distributions.Uniform(*workspace.target_bbox)
self._prop = prop
if prop:
# The prop itself is used to visualize the target location.
self._make_target_site(parent_entity=prop, visible=False)
self._target = self._arena.add_free_entity(prop)
self._prop_placer = initializers.PropPlacer(
props=[prop],
position=target_pos_distribution,
quaternion=workspaces.uniform_z_rotation,
settle_physics=True)
else:
self._target = self._make_target_site(parent_entity=arena,
visible=True)
self._target_placer = target_pos_distribution
obs = observable.MJCFFeature('pos', self._target)
obs.configure(**obs_settings.prop_pose._asdict())
self._task_observables['target_position'] = obs
# Randomize the table surface
if table_col_tag == 0:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/robot_bw")
elif table_col_tag == 1:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/robot_bw")
elif table_col_tag == 2:
self.root_entity.mjcf_model.worldbody.add(
'geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/real_desk")
# Blue stary sky
self.root_entity.mjcf_model.asset.texture[0].mark = 'random'
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[.4, .6, .8])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
# How to remove the checkerboard groundplane ?
# For now this somehow sets it to a white plane!
self.root_entity.mjcf_model.asset.texture[1].width = 1
self.root_entity.mjcf_model.asset.texture[1].height = 1
# Add sites for visualizing the prop and target bounding boxes.
workspaces.add_bbox_site(body=self.root_entity.mjcf_model.worldbody,
lower=workspace.tcp_bbox.lower,
upper=workspace.tcp_bbox.upper,
rgba=constants.GREEN,
name='tcp_spawn_area')
workspaces.add_bbox_site(body=self.root_entity.mjcf_model.worldbody,
lower=workspace.target_bbox.lower,
upper=workspace.target_bbox.upper,
rgba=constants.BLUE,
name='target_spawn_area')
def torso_yvel(self):
return observable.MJCFFeature('subtree_linvel', self._entity.root_body)[1]
def pinch_site_rmat(self):
"""The rotation matrix of the pinch site in global coordinates."""
return observable.MJCFFeature('xmat', self._entity.pinch_site)
def pinch_site_pos(self):
"""The position of the pinch site, in global coordinates."""
return observable.MJCFFeature('xpos', self._entity.pinch_site)
def actuator_activation(self):
"""Observe the actuator activation."""
model = self._entity.mjcf_model
return observable.MJCFFeature('act', model.find_all('actuator'))
def tendons_vel(self):
return observable.MJCFFeature('velocity',
self._entity.observable_tendons)
def tendons_pos(self):
return observable.MJCFFeature('length',
self._entity.observable_tendons)
def head_height(self):
"""Observe the head height."""
return observable.MJCFFeature('xpos', self._entity.head)[2]
def gyro_rightward_roll(self):
return observable.MJCFFeature(
'sensordata', self._entity.mjcf_model.sensor.gyro)[1]
def gyro_anticlockwise_spin(self):
return observable.MJCFFeature(
'sensordata', self._entity.mjcf_model.sensor.gyro)[2]
def body_height(self):
return observable.MJCFFeature('xpos', self._entity.root_body)[2]
def __init__(self, arena, arm, hand, prop, obs_settings, workspace,
control_timestep, table_col_tag, sky_col_tag):
"""Initializes a new `Reach` task.
Args:
arena: `composer.Entity` instance.
arm: `robot_base.RobotArm` instance.
hand: `robot_base.RobotHand` instance.
prop: `composer.Entity` instance specifying the prop to reach to, or None
in which case the target is a fixed site whose position is specified by
the workspace.
obs_settings: `observations.ObservationSettings` instance.
workspace: `_ReachWorkspace` specifying the placement of the prop and TCP.
control_timestep: Float specifying the control timestep in seconds.
"""
self._arena = arena
self._arm = arm
self._hand = hand
self._arm.attach(self._hand)
self._arena.attach_offset(self._arm, offset=workspace.arm_offset)
self.control_timestep = control_timestep
self._tcp_initializer = initializers.ToolCenterPointInitializer(
self._hand,
self._arm,
position=distributions.Uniform(*workspace.tcp_bbox),
quaternion=workspaces.DOWN_QUATERNION)
# Add custom camera observable.
# Randomize camera viewing angle during training
# Experimental
randomize_cam = False
if randomize_cam:
camera_angle = random.choice([0, 1, 2, 3])
if camera_angle == 0:
self._task_observables = cameras.add_camera_observables(
arena, obs_settings, cameras.FRONT_FAR)
elif camera_angle == 1:
self._task_observables = cameras.add_camera_observables(
arena, obs_settings, cameras.FRONT_CLOSE)
elif camera_angle == 2:
self._task_observables = cameras.add_camera_observables(
arena, obs_settings, cameras.FRONT_CLOSE_TILT_UP)
elif camera_angle == 3:
self._task_observables = cameras.add_camera_observables(
arena, obs_settings, cameras.FRONT_CLOSE_TILT_DOWN)
else:
self._task_observables = cameras.add_camera_observables(
arena, obs_settings, cameras.FRONT_FAR)
target_pos_distribution = distributions.Uniform(*workspace.target_bbox)
self._prop = prop
if prop:
# The prop itself is used to visualize the target location.
self._make_target_site(parent_entity=prop, visible=False)
self._target = self._arena.add_free_entity(prop)
self._prop_placer = initializers.PropPlacer(
props=[prop],
position=target_pos_distribution,
quaternion=workspaces.uniform_z_rotation,
settle_physics=True)
else:
self._target = self._make_target_site(parent_entity=arena,
visible=True)
self._target_placer = target_pos_distribution
obs = observable.MJCFFeature('pos', self._target)
obs.configure(**obs_settings.prop_pose._asdict())
self._task_observables['target_position'] = obs
# Randomize the table surface
if table_col_tag == 0:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/darkwood")
elif table_col_tag == 1:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/marble")
elif table_col_tag == 2:
self.root_entity.mjcf_model.worldbody.add(
'geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/navy_blue")
elif table_col_tag == 3:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/tennis")
elif table_col_tag == 4:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/wood")
elif table_col_tag == 5:
self.root_entity.mjcf_model.worldbody.add(
'geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/wood_light")
elif table_col_tag == 6:
self.root_entity.mjcf_model.worldbody.add(
'geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/light_wood_v2")
elif table_col_tag == 7:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/metal")
elif table_col_tag == 8:
self.root_entity.mjcf_model.worldbody.add('geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/grass")
elif table_col_tag == 9:
self.root_entity.mjcf_model.worldbody.add(
'geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/blue_cloud")
elif table_col_tag == 10:
self.root_entity.mjcf_model.worldbody.add(
'geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/marble_v2")
elif table_col_tag == 11:
self.root_entity.mjcf_model.worldbody.add(
'geom',
type='plane',
pos="0 0 0.01",
size="0.6 0.6 0.5",
rgba=".6 .6 .5 1",
contype="1",
conaffinity="1",
friction="2 0.1 0.002",
material="j2s7/wood_gray")
# Sky change of colour
if sky_col_tag == 1:
# Red stary sky
self.root_entity.mjcf_model.asset.texture[0].mark = 'random'
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[.8, .1, .4])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
elif sky_col_tag == 2:
# Orange stary sky
self.root_entity.mjcf_model.asset.texture[0].mark = 'random'
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[.8, .5, .1])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
elif sky_col_tag == 3:
# Yellow stary sky
self.root_entity.mjcf_model.asset.texture[0].mark = 'random'
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[1, 1, .4])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
elif sky_col_tag == 4:
# Pink stary sky
self.root_entity.mjcf_model.asset.texture[0].mark = 'random'
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[1, .5, 1])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
elif sky_col_tag == 5:
# Amber stary sky
self.root_entity.mjcf_model.asset.texture[0].mark = 'random'
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[1, .6, .4])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
elif sky_col_tag == 6:
# White stary sky
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[1., 1., 1.])
self.root_entity.mjcf_model.asset.texture[0].rgb2 = np.array(
[1., 1., 1.])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
elif sky_col_tag == 7:
# Black stary sky
self.root_entity.mjcf_model.asset.texture[0].rgb1 = np.array(
[0, 0, 0])
self.root_entity.mjcf_model.asset.texture[0].width = 800
self.root_entity.mjcf_model.asset.texture[0].height = 800
elif sky_col_tag == 8:
# Leave the checkerboard
self.root_entity.mjcf_model.asset.texture[1].width = 1
self.root_entity.mjcf_model.asset.texture[1].height = 1
# TODO: Remove the checkerboard groundplane
# For now this somehow sets it to a white plane!
self.root_entity.mjcf_model.asset.texture[1].width = 1
self.root_entity.mjcf_model.asset.texture[1].height = 1
# Add sites for visualizing the prop and target bounding boxes.
workspaces.add_bbox_site(body=self.root_entity.mjcf_model.worldbody,
lower=workspace.tcp_bbox.lower,
upper=workspace.tcp_bbox.upper,
rgba=constants.GREEN,
name='tcp_spawn_area')
workspaces.add_bbox_site(body=self.root_entity.mjcf_model.worldbody,
lower=workspace.target_bbox.lower,
upper=workspace.target_bbox.upper,
rgba=constants.BLUE,
name='target_spawn_area')
def world_zaxis(self):
"""The world's z-vector in this Walker's torso frame."""
return observable.MJCFFeature('xmat', self._entity.root_body)[6:]
def sensors_framequat(self):
return observable.MJCFFeature('sensordata',
self._entity.mjcf_model.sensor.framequat)
def sensors_velocimeter(self):
return observable.MJCFFeature('sensordata',
self._entity.mjcf_model.sensor.velocimeter)
def sensors_torque(self):
return observable.MJCFFeature('sensordata',
self._entity.mjcf_model.sensor.torque,
corruptor=lambda v, random_state: np.
tanh(2 * v / _TORQUE_THRESHOLD))
def sensors_touch(self):
return observable.MJCFFeature(
'sensordata',
self._entity.mjcf_model.sensor.touch,
corruptor=
lambda v, random_state: np.array(v > _TOUCH_THRESHOLD, dtype=np.float))
def joints_vel(self):
return observable.MJCFFeature('qvel', self._entity.observable_joints)
def position(self):
return observable.MJCFFeature('xpos', self._entity.root_body)
def joints_pos(self):
return observable.MJCFFeature('qpos', self._entity.observable_joints)
def orientation(self):
return observable.MJCFFeature('xmat', self._entity.root_body)
def sensors_accelerometer(self):
return observable.MJCFFeature(
'sensordata', self._entity.mjcf_model.sensor.accelerometer)
def veloc_forward(self):
return observable.MJCFFeature(
'sensordata', self._entity.mjcf_model.sensor.velocimeter)[0]
