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]