def get_observation(self) -> Observation:
joint_forces = None
if self._obs_config.joint_forces:
fs = self._snake_robot.get_joint_forces()
vels = self._snake_robot.get_joint_target_velocities()
joint_forces = self._obs_config.joint_forces_noise.apply(
np.array([-f if v < 0 else f for f, v in zip(fs, vels)]))
hc_ob = self._obs_config.head_camera
obs = Observation(
head_camera_rgb=(hc_ob.rgb_noise.apply(
self._head_camera.capture_rgb()) if hc_ob.rgb else None),
head_camera_depth=(hc_ob.depth_noise.apply(
self._head_camera.capture_depth()) if hc_ob.depth else None),
head_camera_mask=(self._head_camera.mask().capture_rgb()
if hc_ob.mask else None),
joint_velocities=(self._obs_config.joint_velocities_noise.apply(
np.array(self._snake_robot.get_joint_velocities()))
if self._obs_config.joint_velocities else None),
joint_positions=(self._obs_config.joint_positions_noise.apply(
np.array(self._snake_robot.get_joint_positions()))
if self._obs_config.joint_positions else None),
joint_forces=joint_forces,
robot_pos=(np.array(self._snake_robot.get_snake_head_pos())
if self._obs_config.robot_pos else None),
target_pos=(np.array(self._active_task.get_target_pos())
if self._obs_config.target_pos else None),
target_angle=(np.array(self._active_task.get_target_angle())
if self._obs_config.target_angle else None),
robot_angle=(np.array(self._active_task.get_robot_angle())
if self._obs_config.robot_angle else None),
desired_goal=(np.array(self._active_task.get_target_pos())
if self._obs_config.desired_goal else None),
achieved_goal=(np.array(self._snake_robot.get_snake_head_pos())
if self._obs_config.achieved_goal else None))
obs = self._active_task.decorate_observation(obs)
# self.normalize_observation(obs)
return obs
def get_observation(self) -> Observation:
tip = self._robot.arm.get_tip()
joint_forces = None
if self._obs_config.joint_forces:
fs = self._robot.arm.get_joint_forces()
vels = self._robot.arm.get_joint_target_velocities()
joint_forces = self._obs_config.joint_forces_noise.apply(
np.array([-f if v < 0 else f for f, v in zip(fs, vels)]))
ee_forces_flat = None
if self._obs_config.gripper_touch_forces:
ee_forces = self._robot.gripper.get_touch_sensor_forces()
ee_forces_flat = []
for eef in ee_forces:
ee_forces_flat.extend(eef)
ee_forces_flat = np.array(ee_forces_flat)
lsc_ob = self._obs_config.left_shoulder_camera
rsc_ob = self._obs_config.right_shoulder_camera
wc_ob = self._obs_config.wrist_camera
fc_ob = self._obs_config.front_camera
lsc_mask_fn = (rgb_handles_to_mask
if lsc_ob.masks_as_one_channel else lambda x: x)
rsc_mask_fn = (rgb_handles_to_mask
if rsc_ob.masks_as_one_channel else lambda x: x)
wc_mask_fn = (rgb_handles_to_mask
if wc_ob.masks_as_one_channel else lambda x: x)
fc_mask_fn = (rgb_handles_to_mask
if fc_ob.masks_as_one_channel else lambda x: x)
def get_rgb_depth(sensor: VisionSensor, get_rgb: bool, get_depth: bool,
rgb_noise: NoiseModel, depth_noise: NoiseModel,
depth_in_meters: bool):
rgb = depth = None
if sensor is not None and (get_rgb or get_depth):
sensor.handle_explicitly()
if get_rgb:
rgb = sensor.capture_rgb()
if rgb_noise is not None:
rgb = rgb_noise.apply(rgb)
if get_depth:
depth = sensor.capture_depth(depth_in_meters)
if depth_noise is not None:
depth = depth_noise.apply(depth)
return rgb, depth
def get_mask(sensor: VisionSensor, mask_fn):
mask = None
if sensor is not None:
sensor.handle_explicitly()
mask = mask_fn(sensor.capture_rgb())
return mask
left_shoulder_rgb, left_shoulder_depth = get_rgb_depth(
self._cam_over_shoulder_left, lsc_ob.rgb, lsc_ob.depth,
lsc_ob.rgb_noise, lsc_ob.depth_noise, lsc_ob.depth_in_meters)
right_shoulder_rgb, right_shoulder_depth = get_rgb_depth(
self._cam_over_shoulder_right, rsc_ob.rgb, rsc_ob.depth,
rsc_ob.rgb_noise, rsc_ob.depth_noise, rsc_ob.depth_in_meters)
wrist_rgb, wrist_depth = get_rgb_depth(self._cam_wrist, wc_ob.rgb,
wc_ob.depth, wc_ob.rgb_noise,
wc_ob.depth_noise,
wc_ob.depth_in_meters)
front_rgb, front_depth = get_rgb_depth(self._cam_front, fc_ob.rgb,
fc_ob.depth, fc_ob.rgb_noise,
fc_ob.depth_noise,
fc_ob.depth_in_meters)
left_shoulder_mask = get_mask(self._cam_over_shoulder_left_mask,
lsc_mask_fn) if lsc_ob.mask else None
right_shoulder_mask = get_mask(self._cam_over_shoulder_right_mask,
rsc_mask_fn) if rsc_ob.mask else None
wrist_mask = get_mask(self._cam_wrist_mask,
wc_mask_fn) if wc_ob.mask else None
front_mask = get_mask(self._cam_front_mask,
fc_mask_fn) if fc_ob.mask else None
obs = Observation(
left_shoulder_rgb=left_shoulder_rgb,
left_shoulder_depth=left_shoulder_depth,
right_shoulder_rgb=right_shoulder_rgb,
right_shoulder_depth=right_shoulder_depth,
wrist_rgb=wrist_rgb,
wrist_depth=wrist_depth,
front_rgb=front_rgb,
front_depth=front_depth,
left_shoulder_mask=left_shoulder_mask,
right_shoulder_mask=right_shoulder_mask,
wrist_mask=wrist_mask,
front_mask=front_mask,
joint_velocities=(self._obs_config.joint_velocities_noise.apply(
np.array(self._robot.arm.get_joint_velocities()))
if self._obs_config.joint_velocities else None),
joint_positions=(self._obs_config.joint_positions_noise.apply(
np.array(self._robot.arm.get_joint_positions()))
if self._obs_config.joint_positions else None),
joint_forces=(joint_forces
if self._obs_config.joint_forces else None),
gripper_open=(self._robot.gripper.if_open()
if self._obs_config.gripper_open else None),
gripper_pose=(np.array(tip.get_pose())
if self._obs_config.gripper_pose else None),
gripper_matrix=(np.reshape(tip.get_matrix(), (3, 4))
if self._obs_config.gripper_matrix else None),
gripper_touch_forces=(ee_forces_flat
if self._obs_config.gripper_touch_forces else
None),
gripper_joint_positions=(
np.array(self._robot.gripper.get_joint_positions())
if self._obs_config.gripper_joint_positions else None),
wrist_camera_matrix=(np.reshape(self._cam_wrist.get_matrix(),
(3, 4))
if self._cam_wrist.still_exists() else None),
task_low_dim_state=(self._active_task.get_low_dim_state() if
self._obs_config.task_low_dim_state else None))
obs = self._active_task.decorate_observation(obs)
return obs
def get_observation(self) -> Observation:
tip = self._robot.arm.get_tip()
joint_forces = None
if self._obs_config.joint_forces:
fs = self._robot.arm.get_joint_forces()
vels = self._robot.arm.get_joint_target_velocities()
joint_forces = self._obs_config.joint_forces_noise.apply(
np.array([-f if v < 0 else f for f, v in zip(fs, vels)]))
ee_forces_flat = None
if self._obs_config.gripper_touch_forces:
ee_forces = self._robot.gripper.get_touch_sensor_forces()
ee_forces_flat = []
for eef in ee_forces:
ee_forces_flat.extend(eef)
ee_forces_flat = np.array(ee_forces_flat)
lsc_ob = self._obs_config.left_shoulder_camera
rsc_ob = self._obs_config.right_shoulder_camera
wc_ob = self._obs_config.wrist_camera
obs = Observation(
left_shoulder_rgb=(lsc_ob.rgb_noise.apply(
self._cam_over_shoulder_left.capture_rgb())
if lsc_ob.rgb else None),
left_shoulder_depth=(lsc_ob.depth_noise.apply(
self._cam_over_shoulder_left.capture_depth())
if lsc_ob.depth else None),
right_shoulder_rgb=(rsc_ob.rgb_noise.apply(
self._cam_over_shoulder_right.capture_rgb())
if rsc_ob.rgb else None),
right_shoulder_depth=(rsc_ob.depth_noise.apply(
self._cam_over_shoulder_right.capture_depth())
if rsc_ob.depth else None),
wrist_rgb=(wc_ob.rgb_noise.apply(self._cam_wrist.capture_rgb())
if wc_ob.rgb else None),
wrist_depth=(wc_ob.depth_noise.apply(
self._cam_wrist.capture_depth()) if wc_ob.depth else None),
left_shoulder_mask=(
self._cam_over_shoulder_left_mask.capture_rgb()
if lsc_ob.mask else None),
right_shoulder_mask=(
self._cam_over_shoulder_right_mask.capture_rgb()
if rsc_ob.mask else None),
wrist_mask=(self._cam_wrist_mask.capture_rgb()
if wc_ob.mask else None),
joint_velocities=(self._obs_config.joint_velocities_noise.apply(
np.array(self._robot.arm.get_joint_velocities()))
if self._obs_config.joint_velocities else None),
joint_positions=(self._obs_config.joint_positions_noise.apply(
np.array(self._robot.arm.get_joint_positions()))
if self._obs_config.joint_positions else None),
joint_forces=(joint_forces
if self._obs_config.joint_forces else None),
gripper_open_amount=((
1.0 if self._robot.gripper.get_open_amount()[0] > 0.9 else 0.0)
if self._obs_config.gripper_open_amount else
None),
gripper_pose=(np.array(tip.get_pose())
if self._obs_config.gripper_pose else None),
gripper_touch_forces=(ee_forces_flat
if self._obs_config.gripper_touch_forces else
None),
gripper_joint_positions=(
np.array(self._robot.gripper.get_joint_positions())
if self._obs_config.gripper_joint_positions else None),
task_low_dim_state=(self._active_task.get_low_dim_state() if
self._obs_config.task_low_dim_state else None))
obs = self._active_task.decorate_observation(obs)
return obs