class RattlerCamera(object):
def __init__(self):
self._head_camera = VisionSensor('rattler_eye')
self._head_camera_mask = None
self._head_camera_state = 1
def get_camera_state(self) -> int:
return self._head_camera_state
def set_camera_state(self, state: int) -> None:
self._head_camera_state = state
def set_camera_properties(self, conf: CameraConfig) -> None:
if not (conf.rgb or conf.depth):
self._head_camera.remove()
else:
self._head_camera.set_resolution(conf.image_size)
self._head_camera.set_render_mode(conf.render_mode)
if conf.mask:
self._head_camera_mask = self._head_camera.copy()
self._head_camera_mask.set_render_mode(
RenderMode.OPENGL_COLOR_CODED)
def capture_rgb(self) -> np.ndarray:
return self._head_camera.capture_rgb()
def capture_depth(self) -> np.ndarray:
return self._head_camera.capture_depth()
def mask(self) -> VisionSensor:
return self._head_camera_mask
class ArmECM(PyRep):
def __init__(self, pr):
#super(ArmECM, self).__init__(scenePath)
"""self.pr = PyRep()
self.pr.launch(scenePath)
self.pr.start()
self.pr.step()"""
self.pr = pr
self.base_handle = Shape('L0_respondable_ECM')
self.j1_handle = Joint('J1_ECM')
self.j2_handle = Joint('J2_ECM')
self.j3_handle = Joint('J3_ECM')
self.j4_handle = Joint('J4_ECM')
self.left_cam = VisionSensor('Vision_sensor_left')
self.right_cam = VisionSensor('Vision_sensor_right')
def getJointAngles(self):
pos1 = self.j1_handle.get_joint_position()
pos2 = self.j2_handle.get_joint_position()
pos3 = self.j3_handle.get_joint_position()
pos4 = self.j4_handle.get_joint_position()
return np.array([pos1, pos2, pos3, pos4])
def setJointAngles(self, jointAngles):
self.j1_handle.set_joint_position(jointAngles[0])
self.j2_handle.set_joint_position(jointAngles[1])
self.j3_handle.set_joint_position(jointAngles[2])
self.j4_handle.set_joint_position(jointAngles[3])
def getCurrentPose(self):
return self.left_cam.get_pose(relative_to=self.base_handle)
def getStereoImagePairs(self):
return self.left_cam.capture_rgb(), self.right_cam.capture_rgb()
def getDepthImagePairs(self):
return self.left_cam.capture_depth(True), self.right_cam.capture_depth(
True)
"""def stopSim(self):
class Plane3D:
def __init__(self, headless=False):
self._pr = PyRep()
self._pr.launch(scene_file=scene_file, headless=headless)
self._pr.start()
self.workspace_base = Shape("workspace")
self.workspace = self._get_worksapce()
self.camera = VisionSensor("camera")
self.obstacles = []
self.velocity_scale = 0
self.repiration_cycle = 0
def _get_worksapce(self):
base_pos = self.workspace_base.get_position()
bbox = self.workspace_base.get_bounding_box()
min_pos = [bbox[2 * i] + base_pos[i] for i in range(3)]
max_pos = [bbox[2 * i + 1] + base_pos[i] for i in range(3)]
return [min_pos, max_pos]
def reset(self, obstacle_num, velocity_scale, respiration_cycle=0):
for obstacle in self.obstacles:
obstacle.remove()
self._pr.step()
self.velocity_scale = velocity_scale
self.repiration_cycle = respiration_cycle
self.obstacles = []
for i in range(obstacle_num):
obs = Obstacle2D.create_random_obstacle(
workspace=self.workspace,
velocity_scale=velocity_scale,
respiration_cycle=respiration_cycle)
self._pr.step()
self.obstacles.append(obs)
def get_image(self):
rgb = self.camera.capture_rgb()
return rgb
def step(self):
# update config
for obs in self.obstacles:
if self.repiration_cycle > 0:
obs.respire()
if self.velocity_scale > 0:
obs.keep_velocity()
self._pr.step()
def get_rgb_depth(sensor: VisionSensor, get_rgb: bool, get_depth: bool,
rgb_noise: NoiseModel, depth_noise: NoiseModel):
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()
if depth_noise is not None:
depth = depth_noise.apply(depth)
return rgb, depth
class TestVisionSensors(TestCore):
def setUp(self):
super().setUp()
[self.pyrep.step() for _ in range(10)]
self.cam = VisionSensor('cam0')
def test_capture_rgb(self):
img = self.cam.capture_rgb()
self.assertEqual(img.shape, (16, 16, 3))
# Check that it's not a blank image
self.assertFalse(img.min() == img.max() == 0.0)
def test_capture_depth(self):
img = self.cam.capture_depth()
self.assertEqual(img.shape, (16, 16))
# Check that it's not a blank depth map
self.assertFalse(img.min() == img.max() == 1.0)
path_model = os.path.join(
os.path.expanduser('~'),
'robotics_drl/data/youbot_all_final_21-08-2019_22-32-00/model.pkl')
actor.load_state_dict(torch.load(path_model))
actor.eval()
state = env.reset()
for i in range(2):
state = env.reset()
steps = 0
while True:
steps += 1
with torch.no_grad():
# state['low'] = state['low'].to(device)
# state['high'] = state['high'].to(device)
img = resize(camera_side.capture_rgb())
save_image(img, 'y_%s.png' % steps)
action, _, gate = actor(state.cuda(),
log_prob=False,
deterministic=True)
print(
sqrt(
np.sum(
np.array(
env.target_base.get_position(
relative_to=env.tip))**2)), gate)
state, reward, done = env.step(action.squeeze().cpu().numpy())
if reward == env.reward_termination:
print('Task %s completed' % (i))
break
class LoCoBotCamera(Camera):
"""docstring for SimpleCamera"""
def __init__(self, configs, simulator):
self.sim = simulator.sim
self.rgb_cam = VisionSensor("kinect_rgb")
self.depth_cam = VisionSensor("kinect_depth")
self.rgb_cam.set_render_mode(RenderMode.OPENGL3)
self.depth_cam.set_render_mode(RenderMode.OPENGL3)
# Pan and tilt related variables.
self.pan_joint = Joint("LoCoBot_head_pan_joint")
self.tilt_joint = Joint("LoCoBot_head_tilt_joint")
def get_rgb(self):
return self.rgb_cam.capture_rgb()
def get_depth(self):
return self.depth_cam.capture_depth()
def get_rgb_depth(self):
return self.get_rgb(), self.get_depth()
def get_intrinsics(self):
# Todo: Remove this after we fix intrinsics
raise NotImplementedError
"""
Returns the instrinsic matrix of the camera
:return: the intrinsic matrix (shape: :math:`[3, 3]`)
:rtype: np.ndarray
"""
# fx = self.configs['Camera.fx']
# fy = self.configs['Camera.fy']
# cx = self.configs['Camera.cx']
# cy = self.configs['Camera.cy']
Itc = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
return Itc
def pix_to_3dpt(self, rs, cs, in_cam=False):
"""
Get the 3D points of the pixels in RGB images.
:param rs: rows of interest in the RGB image.
It can be a list or 1D numpy array
which contains the row indices.
The default value is None,
which means all rows.
:param cs: columns of interest in the RGB image.
It can be a list or 1D numpy array
which contains the column indices.
The default value is None,
which means all columns.
:param in_cam: return points in camera frame,
otherwise, return points in base frame
:type rs: list or np.ndarray
:type cs: list or np.ndarray
:type in_cam: bool
:returns: tuple (pts, colors)
pts: point coordinates in world frame
(shape: :math:`[N, 3]`)
colors: rgb values for pts_in_cam
(shape: :math:`[N, 3]`)
:rtype: tuple(np.ndarray, np.ndarray)
"""
raise NotImplementedError
def get_current_pcd(self, in_cam=True):
"""
Return the point cloud at current time step (one frame only)
:param in_cam: return points in camera frame,
otherwise, return points in base frame
:type in_cam: bool
:returns: tuple (pts, colors)
pts: point coordinates in world frame (shape: :math:`[N, 3]`)
colors: rgb values for pts_in_cam (shape: :math:`[N, 3]`)
:rtype: tuple(np.ndarray, np.ndarray)
"""
raise NotImplementedError
@property
def state(self):
"""
Return the current pan and tilt joint angles of the robot camera.
:return:
pan_tilt: A list the form [pan angle, tilt angle]
:rtype: list
"""
return self.get_state()
def get_state(self):
"""
Return the current pan and tilt joint angles of the robot camera.
:return:
pan_tilt: A list the form [pan angle, tilt angle]
:rtype: list
"""
return [self.get_pan(), self.get_tilt()]
def get_pan(self):
"""
Return the current pan joint angle of the robot camera.
:return:
pan: Pan joint angle
:rtype: float
"""
return self.pan_joint.get_joint_position()
def get_tilt(self):
"""
Return the current tilt joint angle of the robot camera.
:return:
tilt: Tilt joint angle
:rtype: float
"""
return self.tilt_joint.get_joint_position()
def set_pan(self, pan, wait=True):
"""
Sets the pan joint angle to the specified value.
:param pan: value to be set for pan joint
:param wait: wait until the pan angle is set to
the target angle.
:type pan: float
:type wait: bool
"""
self.pan_joint.set_joint_position(pan)
# [self.sim.step() for _ in range(50)]
def set_tilt(self, tilt, wait=True):
"""
Sets the tilt joint angle to the specified value.
:param tilt: value to be set for the tilt joint
:param wait: wait until the tilt angle is set to
the target angle.
:type tilt: float
:type wait: bool
"""
self.tilt_joint.set_joint_position(tilt)
def set_pan_tilt(self, pan, tilt, wait=True):
"""
Sets both the pan and tilt joint angles to the specified values.
:param pan: value to be set for pan joint
:param tilt: value to be set for the tilt joint
:param wait: wait until the pan and tilt angles are set to
the target angles.
:type pan: float
:type tilt: float
:type wait: bool
"""
self.set_pan(pan)
self.set_tilt(tilt)
def reset(self):
"""
This function resets the pan and tilt joints by actuating
them to their home configuration.
"""
self.set_pan_tilt(self.configs.CAMERA.RESET_PAN, self.configs.CAMERA.RESET_TILT)
class Scene(object):
"""Controls what is currently in the vrep scene. This is used for making
sure that the tasks are easily reachable. This may be just replaced by
environment. Responsible for moving all the objects. """
def __init__(self,
pyrep: PyRep,
robot: Robot,
obs_config=ObservationConfig()):
self._pyrep = pyrep
self._robot = robot
self._obs_config = obs_config
self._active_task = None
self._inital_task_state = None
self._start_arm_joint_pos = robot.arm.get_joint_positions()
self._starting_gripper_joint_pos = robot.gripper.get_joint_positions()
self._workspace = Shape('workspace')
self._workspace_boundary = SpawnBoundary([self._workspace])
self._cam_over_shoulder_left = VisionSensor('cam_over_shoulder_left')
self._cam_over_shoulder_right = VisionSensor('cam_over_shoulder_right')
self._cam_wrist = VisionSensor('cam_wrist')
self._cam_over_shoulder_left_mask = VisionSensor(
'cam_over_shoulder_left_mask')
self._cam_over_shoulder_right_mask = VisionSensor(
'cam_over_shoulder_right_mask')
self._cam_wrist_mask = VisionSensor('cam_wrist_mask')
self._has_init_task = self._has_init_episode = False
self._variation_index = 0
self._initial_robot_state = (robot.arm.get_configuration_tree(),
robot.gripper.get_configuration_tree())
# Set camera properties from observation config
self._set_camera_properties()
def load(self, task: Task) -> None:
"""Loads the task and positions at the centre of the workspace.
:param task: The task to load in the scene.
"""
task.load() # Load the task in to the scene
# Set at the centre of the workspace
task.get_base().set_position(self._workspace.get_position())
self._inital_task_state = task.get_state()
self._active_task = task
self._initial_task_pose = task.boundary_root().get_orientation()
self._has_init_task = self._has_init_episode = False
self._variation_index = 0
def unload(self) -> None:
"""Clears the scene. i.e. removes all tasks. """
if self._active_task is not None:
self._robot.gripper.release()
if self._has_init_task:
self._active_task.cleanup_()
self._active_task.unload()
self._active_task = None
self._variation_index = 0
def init_task(self) -> None:
self._active_task.init_task()
self._has_init_task = True
self._variation_index = 0
def init_episode(self,
index: int,
randomly_place: bool = True,
max_attempts: int = 5) -> List[str]:
"""Calls the task init_episode and puts randomly in the workspace.
"""
self._variation_index = index
if not self._has_init_task:
self.init_task()
# Try a few times to init and place in the workspace
attempts = 0
descriptions = None
while attempts < max_attempts:
descriptions = self._active_task.init_episode(index)
try:
if (randomly_place
and not self._active_task.is_static_workspace()):
self._place_task()
self._active_task.validate()
break
except (BoundaryError, WaypointError) as e:
self._active_task.cleanup_()
self._active_task.restore_state(self._inital_task_state)
attempts += 1
if attempts >= max_attempts:
raise e
# Let objects come to rest
[self._pyrep.step() for _ in range(STEPS_BEFORE_EPISODE_START)]
self._has_init_episode = True
return descriptions
def reset(self) -> None:
"""Resets the joint angles. """
self._robot.gripper.release()
arm, gripper = self._initial_robot_state
self._pyrep.set_configuration_tree(arm)
self._pyrep.set_configuration_tree(gripper)
self._robot.arm.set_joint_positions(self._start_arm_joint_pos)
self._robot.arm.set_joint_target_velocities(
[0] * len(self._robot.arm.joints))
self._robot.gripper.set_joint_positions(
self._starting_gripper_joint_pos)
self._robot.gripper.set_joint_target_velocities(
[0] * len(self._robot.gripper.joints))
if self._active_task is not None and self._has_init_task:
self._active_task.cleanup_()
self._active_task.restore_state(self._inital_task_state)
[self._pyrep.step_ui() for _ in range(20)]
self._active_task.set_initial_objects_in_scene()
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
def step(self):
self._pyrep.step()
self._active_task.step()
def get_demo(self,
record: bool = True,
func=None,
randomly_place: bool = True) -> List[Observation]:
"""Returns a demo (list of observations)"""
if not self._has_init_task:
self.init_task()
if not self._has_init_episode:
self.init_episode(self._variation_index,
randomly_place=randomly_place)
self._has_init_episode = False
waypoints = self._active_task.get_waypoints()
if len(waypoints) == 0:
raise NoWaypointsError('No waypoints were found.',
self._active_task)
demo = []
if record:
self._pyrep.step() # Need this here or get_force doesn't work...
demo.append(self.get_observation())
while True:
success = False
for i, point in enumerate(waypoints):
point.start_of_path()
try:
path = point.get_path()
except ConfigurationPathError as e:
raise DemoError(
'Could not get a path for waypoint %d.' % i,
self._active_task) from e
ext = point.get_ext()
path.visualize()
done = False
success = False
while not done:
done = path.step()
self.step()
self._demo_record_step(demo, record, func)
success, term = self._active_task.success()
if success:
break
point.end_of_path()
path.clear_visualization()
if success:
# We can quit early because we have finished the task
break
# TODO: need to decide how I do the gripper actions
if len(ext) > 0:
contains_param = False
start_of_bracket = -1
gripper = self._robot.gripper
if 'open_gripper(' in ext:
gripper.release()
start_of_bracket = ext.index('open_gripper(') + 13
contains_param = ext[start_of_bracket] != ')'
if not contains_param:
done = False
while not done:
done = gripper.actuate(1.0, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(demo, record, func)
elif 'close_gripper(' in ext:
start_of_bracket = ext.index('close_gripper(') + 14
contains_param = ext[start_of_bracket] != ')'
if not contains_param:
done = False
while not done:
done = gripper.actuate(0.0, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(demo, record, func)
if contains_param:
rest = ext[start_of_bracket:]
num = float(rest[:rest.index(')')])
done = False
while not done:
done = gripper.actuate(num, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(demo, record, func)
if 'close_gripper(' in ext:
for g_obj in self._active_task.get_graspable_objects():
gripper.grasp(g_obj)
self._demo_record_step(demo, record, func)
if not self._active_task.should_repeat_waypoints() or success:
break
# Some tasks may need additional physics steps
# (e.g. ball rowling to goal)
if not success:
for _ in range(10):
self._pyrep.step()
self._active_task.step()
self._demo_record_step(demo, record, func)
success, term = self._active_task.success()
if success:
break
success, term = self._active_task.success()
if not success:
raise DemoError('Demo was completed, but was not successful.',
self._active_task)
return demo
def get_observation_config(self) -> ObservationConfig:
return self._obs_config
def _demo_record_step(self, demo_list, record, func):
if record:
demo_list.append(self.get_observation())
if func is not None:
func()
def _set_camera_properties(self) -> None:
def _set_rgb_props(rgb_cam: VisionSensor, rgb: bool, depth: bool,
conf: CameraConfig):
if not (rgb or depth):
rgb_cam.remove()
else:
rgb_cam.set_resolution(conf.image_size)
rgb_cam.set_render_mode(conf.render_mode)
def _set_mask_props(mask_cam: VisionSensor, mask: bool,
conf: CameraConfig):
if not mask:
mask_cam.remove()
else:
mask_cam.set_resolution(conf.image_size)
_set_rgb_props(self._cam_over_shoulder_left,
self._obs_config.left_shoulder_camera.rgb,
self._obs_config.left_shoulder_camera.depth,
self._obs_config.left_shoulder_camera)
_set_rgb_props(self._cam_over_shoulder_right,
self._obs_config.right_shoulder_camera.rgb,
self._obs_config.right_shoulder_camera.depth,
self._obs_config.right_shoulder_camera)
_set_rgb_props(self._cam_wrist, self._obs_config.wrist_camera.rgb,
self._obs_config.wrist_camera.depth,
self._obs_config.wrist_camera)
_set_mask_props(self._cam_over_shoulder_left_mask,
self._obs_config.left_shoulder_camera.mask,
self._obs_config.left_shoulder_camera)
_set_mask_props(self._cam_over_shoulder_right_mask,
self._obs_config.right_shoulder_camera.mask,
self._obs_config.right_shoulder_camera)
_set_mask_props(self._cam_wrist_mask,
self._obs_config.wrist_camera.mask,
self._obs_config.wrist_camera)
def _place_task(self) -> None:
self._workspace_boundary.clear()
# Find a place in the robot workspace for task
self._active_task.boundary_root().set_orientation(
self._initial_task_pose)
min_rot, max_rot = self._active_task.base_rotation_bounds()
self._workspace_boundary.sample(self._active_task.boundary_root(),
min_rotation=min_rot,
max_rotation=max_rot)
class SmartBot(NonHolonomicBase):
velocity_limit = np.array([0.0, 15.0])
nb_ultrasonic_sensor = 5
ultrasonic_sensor_bound = np.array([0.02, 2.0])
def __init__(self, count: int = 0, enable_vision: bool = False):
super().__init__(count, 2, 'smartbot')
self._enable_vision = enable_vision
self._camera = VisionSensor('{}_camera'.format(self.get_name()))
self._gyroscope = Gyroscope('{}_gyro_sensor'.format(self.get_name()))
self._accelerometer = Accelerometer(
'{}_accelerometer'.format(self.get_name()))
self._ultrasonic_values = 2 * np.ones(self.nb_ultrasonic_sensor)
self._ultrasonic_sensors = [ProximitySensor(
'{}_ultrasonic_sensor_{}'.format(self.get_name(), i + 1))
for i in range(self.nb_ultrasonic_sensor)]
self._previous_joint_positions = self.get_joint_positions()
self.initial_configuration = self.get_configuration_tree()
@property
def ultrasonic_distances(self) -> np.ndarray:
"""Reads proximity sensors values from robot.
Returns:
proximity_sensor_values: Array of proximity sensor values.
"""
for i in range(self.nb_ultrasonic_sensor):
dist = self._ultrasonic_sensors[i].read()
if dist == -1.0:
self._ultrasonic_values[i - 1] = self.ultrasonic_sensor_bound[1]
else:
self._ultrasonic_values[i - 1] = dist
return self._ultrasonic_values
@property
def image(self) -> np.ndarray:
"""Reads image from camera mounted to robot.
Returns:
img: Image received from robot
"""
return self._camera.capture_rgb()
@property
def accelerations(self) -> np.ndarray:
"""Reads accelerometer values from robot.
Returns:
accelerometer_values: Array of values received from accelerometer.
"""
values = self._accelerometer.read()
return np.round(values, 4)
@property
def angular_velocities(self) -> np.ndarray:
"""Reads gyroscope values from robot.
Returns:
gyroscope_values: Array of values received from gyroscope.
"""
values = self._gyroscope.read()
return np.round(values, 4)
@property
def encoder_ticks(self) -> np.ndarray:
"""Reads encoders ticks from robot.
Returns:
encoder_ticks: Current values of encoders ticks.
"""
dphi = np.asarray(self.get_joint_positions()) \
- np.asarray(self._previous_joint_positions)
self._previous_joint_positions = self.get_joint_positions()
def correct_angle(angle):
new_angle = None
if angle >= 0:
new_angle = np.fmod((angle + np.pi), (2 * np.pi)) - np.pi
if angle < 0:
new_angle = np.fmod((angle - np.pi), (2 * np.pi)) + np.pi
new_angle = np.round(angle, 3)
return new_angle
return np.asarray([correct_angle(angle) for angle in dphi])
class StereoVisionRobot:
def __init__(self,
min_distance,
max_distance,
default_joint_limit_type="none"):
self.cam_left = VisionSensor("vs_cam_left#")
self.cam_right = VisionSensor("vs_cam_right#")
self.tilt_left = Joint("vs_eye_tilt_left#")
self.tilt_right = Joint("vs_eye_tilt_right#")
self.pan_left = Joint("vs_eye_pan_left#")
self.pan_right = Joint("vs_eye_pan_right#")
self.min_distance = min_distance
self.max_distance = max_distance
self.default_joint_limit_type = default_joint_limit_type
self._pan_max = rad(10)
self._tilt_max = rad(10)
self._tilt_speed = 0
self._pan_speed = 0
self.episode_reset()
# def episode_reset(self, vergence=True):
def episode_reset(self):
### reset joints position / speeds
self.reset_speed()
self.tilt_right.set_joint_position(0)
self.tilt_left.set_joint_position(0)
fixation_distance = np.random.uniform(self.min_distance,
self.max_distance)
random_vergence = rad(to_angle(fixation_distance))
self.pan_left.set_joint_position(random_vergence / 2)
self.pan_right.set_joint_position(-random_vergence / 2)
def reset_speed(self):
self._tilt_speed = 0
self._pan_speed = 0
def _check_pan_limit(self, left, right, joint_limit_type=None):
joint_limit_type = self.default_joint_limit_type if joint_limit_type is None else joint_limit_type
if joint_limit_type == "stick":
return np.clip(left, 0,
self._pan_max), np.clip(right, -self._pan_max, 0)
if joint_limit_type == "none":
return left, right
def _check_tilt_limit(self, left, right, joint_limit_type=None):
joint_limit_type = self.default_joint_limit_type if joint_limit_type is None else joint_limit_type
if joint_limit_type == "stick":
return np.clip(left, -self._tilt_max,
self._tilt_max), np.clip(right, -self._tilt_max,
self._tilt_max)
if joint_limit_type == "none":
return left, right
def reset_vergence_position(self, joint_limit_type=None):
mean = (self.pan_right.get_joint_position() +
self.pan_left.get_joint_position()) / 2
left, right = self._check_pan_limit(mean, mean, joint_limit_type)
self.pan_left.set_joint_position(left)
self.pan_right.set_joint_position(right)
def set_vergence_position(self, alpha, joint_limit_type=None):
rad_alpha = rad(alpha)
mean = (self.pan_right.get_joint_position() +
self.pan_left.get_joint_position()) / 2
left, right = self._check_pan_limit(mean + rad_alpha / 2,
mean - rad_alpha / 2,
joint_limit_type)
self.pan_left.set_joint_position(left)
self.pan_right.set_joint_position(right)
def set_delta_vergence_position(self, delta, joint_limit_type=None):
rad_delta = rad(delta)
left, right = self._check_pan_limit(
self.pan_left.get_joint_position() + rad_delta / 2,
self.pan_right.get_joint_position() - rad_delta / 2,
joint_limit_type)
self.pan_left.set_joint_position(left)
self.pan_right.set_joint_position(right)
def set_pan_position(self, alpha, joint_limit_type=None):
rad_alpha = rad(alpha)
vergence = self.pan_left.get_joint_position(
) - self.pan_right.get_joint_position()
left, right = self._check_pan_limit((vergence / 2) + rad_alpha,
-(vergence / 2) + rad_alpha,
joint_limit_type)
self.pan_left.set_joint_position(left)
self.pan_right.set_joint_position(right)
def set_delta_pan_speed(self, delta, joint_limit_type=None):
self._pan_speed += rad(delta)
left, right = self._check_pan_limit(
self.pan_left.get_joint_position() + self._pan_speed,
self.pan_right.get_joint_position() + self._pan_speed,
joint_limit_type)
self.pan_left.set_joint_position(left)
self.pan_right.set_joint_position(right)
def set_tilt_position(self, alpha, joint_limit_type=None):
rad_alpha = rad(alpha)
left, right = self._check_tilt_limit(rad_alpha, rad_alpha,
joint_limit_type)
self.tilt_left.set_joint_position(left)
self.tilt_right.set_joint_position(right)
def set_delta_tilt_speed(self, delta, joint_limit_type=None):
self._tilt_speed += rad(delta)
left, right = self._check_tilt_limit(
self.tilt_left.get_joint_position() -
self._tilt_speed, # minus to get natural upward movement
self.tilt_right.get_joint_position() -
self._tilt_speed, # minus to get natural upward movement
joint_limit_type)
self.tilt_left.set_joint_position(left)
self.tilt_right.set_joint_position(right)
def get_tilt_position(self):
return deg(self.tilt_right.get_joint_position())
def get_pan_position(self):
return deg((self.pan_right.get_joint_position() +
self.pan_left.get_joint_position()) / 2)
def get_vergence_position(self):
return deg(self.pan_left.get_joint_position() -
self.pan_right.get_joint_position())
def get_position(self):
return self.tilt_position, self.pan_position, self.vergence_position
def get_vergence_error(self, other_distance):
return self.vergence_position - to_angle(other_distance)
def get_tilt_speed(self):
return deg(self._tilt_speed)
def get_pan_speed(self):
return deg(self._pan_speed)
def get_speed(self):
return self.tilt_speed, self.pan_speed
def set_action(self, action, joint_limit_type=None):
self.set_delta_tilt_speed(float(action[0]), joint_limit_type)
self.set_delta_pan_speed(float(action[1]), joint_limit_type)
self.set_delta_vergence_position(float(action[2]), joint_limit_type)
def set_position(self, position, joint_limit_type):
self.set_tilt_position(position[0], joint_limit_type)
self.set_pan_position(position[1], joint_limit_type)
self.set_vergence_position(position[2], joint_limit_type)
def get_vision(self):
return self.cam_left.capture_rgb(), self.cam_right.capture_rgb()
tilt_position = property(get_tilt_position)
pan_position = property(get_pan_position)
vergence_position = property(get_vergence_position)
position = property(get_position)
tilt_speed = property(get_tilt_speed)
pan_speed = property(get_pan_speed)
speed = property(get_speed)
class EnvPollos(Env):
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
# 5 points in 3D space forming the trajectory
self.action_space = Box(low=-0.3, high=0.3, shape=(5,3), dtype=np.float32)
self.observation_space = Box(low=np.array([-0.1, -0.2, 0, -1.5, 0.0, 0.0, -0.5, -0.2, -0.2]),
high=np.array([0.1, 1.7, 2.5, 0, 0.0, 0.0, 0.5, 0.2, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1.2
self.joints = [Joint('UR10_joint1'), Joint('UR10_joint2'), Joint('UR10_joint3'), Joint('UR10_joint4'), Joint('UR10_joint5'), Joint('UR10_joint6')]
self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
for j in self.joints]
print(self.joints_limits)
self.agent_hand = Shape('hand')
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip().get_quaternion()
self.target = Dummy('UR10_target')
self.initial_joint_positions = self.agent.get_joint_positions()
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
self.table_target = Dummy('table_target')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
self.table_target = Dummy('table_target')
# objects
self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')]
#self.agent_tip = self.agent.get_tip()
self.initial_distance = np.linalg.norm(np.array(self.initial_pollo_position)-np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(self.camera.get_handle(),-1)
self.np_camera_matrix_extrinsics = np.delete(np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width/2.0) / math.tan(angle/2.0)
focaly_px = (height/2.0) / math.tan(angle/2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def reset(self):
pos = list(np.random.uniform( [-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) + self.initial_pollo_position)
self.pollo.set_position(pos)
self.pollo.set_quaternion(self.initial_pollo_orientation)
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_epoch_time = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where( np.fabs(a) < 0.1, False, True )):
break
return self._get_state()
def step(self, action):
print(action.shape, action)
if action is None:
self.pr.step()
return self._get_state(), 0.0, False, {}
if np.any(np.isnan(action)):
print(action)
return self._get_state(), -1.0, False, {}
# create a path with tip and pollo at its endpoints and 5 adjustable points in the middle
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
np_robot_tip_orientation = np.array(self.agent.get_tip().get_orientation())
c_path = CartesianPath.create()
c_path.insert_control_points(action[4]) # point after pollo to secure the grasp
c_path.insert_control_points([list(np_pollo_target) + list(np_robot_tip_orientation)]) # pollo
c_path.insert_control_points(action[0:3])
c_path.insert_control_points([list(np_robot_tip_position) + list(np_robot_tip_orientation)]) # robot hand
try:
self.path = self.agent.get_path_from_cartesian_path(c_path)
except IKError as e:
print('Agent::grasp Could not find joint values')
return self._get_state(), -1, True, {}
# go through path
reloj = time.time()
while self.path.step():
self.pr.step() # Step the physics simulation
if (time.time()-reloj) > 4:
return self._get_state(), -10.0, True, {} # Too much time
if any((self.agent_hand.check_collision(obj) for obj in self.scene_objects)): # colisión con la mesa
return self._get_state(), -10.0, True, {}
# path ok, compute reward
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
dist = np.linalg.norm(np_pollo_target-np_robot_tip_position)
altura = np.clip((np_pollo_target[2] - self.initial_pollo_position[2]), 0, 0.5)
if altura > 0.3: # pollo en mano
return self._get_state(), altura, True, {}
elif dist > self.initial_distance: # mano se aleja
return self._get_state(), -10.0, True, {}
if time.time() - self.initial_epoch_time > 5: # too much time
return self._get_state(), -10.0, True, {}
# Reward
pollo_height = np.exp(-altura*10) # para 1m pollo_height = 0.001, para 0m pollo_height = 1
reward = - pollo_height - dist
return self._get_state(), reward, False, {}
def close(self):
self.pr.stop()
self.pr.shutdown()
def render(self):
print("RENDER")
np_pollo_en_camara = self.getPolloEnCamara()
# capture depth image
depth = self.camera.capture_rgb()
circ = plt.Circle((int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])),10)
plt.clf()
fig = plt.gcf()
ax = fig.gca()
ax.add_artist(circ)
ax.imshow(depth, cmap = 'hot')
plt.pause(0.000001)
# Aux
# transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates
def getPolloEnCamara(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot(np.append([np_pollo_target],1.0))
np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot(np_pollo_target_cam)
np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2]
np_pollo_en_camara = np.delete(np_pollo_en_camara,2)
return np_pollo_en_camara
def getPolloEnCamaraEx(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot(np.append([np_pollo_target],1.0))
return np_pollo_en_camara
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
# return (self.agent.get_joint_positions() +
# self.agent.get_joint_velocities() +
# self.pollo_target.get_position())
p = self.getPolloEnCamaraEx()
j = self.agent.get_joint_positions()
r = np.array([j[0],j[1],j[2],j[3],j[4],j[5],p[0],p[1],p[2]])
return r
def vrepToNP(self, c):
return np.array([[c[0],c[4],c[8],c[3]],
[c[1],c[5],c[9],c[7]],
[c[2],c[6],c[10],c[11]],
[0, 0, 0, 1]])
class SphericalVisionSensor(Object):
"""An object able capture 360 degree omni-directional images.
"""
def __init__(self, name):
super().__init__(name)
# final omni-directional sensors
self._sensor_depth = VisionSensor('%s_sensorDepth' % (self.get_name()))
self._sensor_rgb = VisionSensor('%s_sensorRGB' % (self.get_name()))
# directed sub-sensors
names = ['front', 'top', 'back', 'bottom', 'left', 'right']
self._six_sensors = [VisionSensor('%s_%s' % (self.get_name(), n)) for n in names]
self._front = self._six_sensors[0]
# directed sub-sensor handles list
self._six_sensor_handles = [vs.get_handle() for vs in self._six_sensors]
# set all to explicit handling
self._set_all_to_explicit_handling()
# assert sensor properties are matching
self._assert_matching_resolutions()
self._assert_matching_render_modes()
self._assert_matching_entities_to_render()
self._assert_matching_window_sizes()
self._assert_matching_near_clipping_planes()
self._assert_matching_far_clipping_planes()
# omni resolution
self._omni_resolution = self._sensor_rgb.get_resolution()
# near and far clipping plane
self._near_clipping_plane = self.get_near_clipping_plane()
self._far_clipping_plane = self.get_far_clipping_plane()
self._clipping_plane_diff = self._far_clipping_plane - self._near_clipping_plane
# projective cam region image
self._planar_to_radial_depth_scalars = self._get_planar_to_radial_depth_scalars()
# flip config
self._flip_x = True
# Private #
# --------#
def _set_all_to_explicit_handling(self):
self._sensor_depth.set_explicit_handling(1)
self._sensor_rgb.set_explicit_handling(1)
for sensor in self._six_sensors:
sensor.set_explicit_handling(1)
def _assert_matching_resolutions(self):
assert self._sensor_depth.get_resolution() == self._sensor_rgb.get_resolution()
front_sensor_res = self._front.get_resolution()
for sensor in self._six_sensors[1:]:
assert sensor.get_resolution() == front_sensor_res
def _assert_matching_render_modes(self):
assert self._sensor_depth.get_render_mode() == self._sensor_rgb.get_render_mode()
front_sensor_render_mode = self._front.get_render_mode()
for sensor in self._six_sensors[1:]:
assert sensor.get_render_mode() == front_sensor_render_mode
def _assert_matching_entities_to_render(self):
assert self._sensor_depth.get_entity_to_render() == self._sensor_rgb.get_entity_to_render()
front_sensor_entity_to_render = self._front.get_entity_to_render()
for sensor in self._six_sensors[1:]:
assert sensor.get_entity_to_render() == front_sensor_entity_to_render
def _assert_matching_window_sizes(self):
assert self._sensor_depth.get_windowed_size() == self._sensor_rgb.get_windowed_size()
def _assert_matching_near_clipping_planes(self):
front_sensor_near = self._front.get_near_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_near_clipping_plane() == front_sensor_near
def _assert_matching_far_clipping_planes(self):
front_sensor_far = self._front.get_far_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_far_clipping_plane() == front_sensor_far
def _get_requested_type(self) -> ObjectType:
return ObjectType(sim.simGetObjectType(self.get_handle()))
@staticmethod
def _create_uniform_pixel_coords_image(image_dims):
pixel_x_coords = np.reshape(np.tile(np.arange(image_dims[1]), [image_dims[0]]),
(image_dims[0], image_dims[1], 1)).astype(np.float32)
pixel_y_coords_ = np.reshape(np.tile(np.arange(image_dims[0]), [image_dims[1]]),
(image_dims[1], image_dims[0], 1)).astype(np.float32)
pixel_y_coords = np.transpose(pixel_y_coords_, (1, 0, 2))
return np.concatenate((pixel_x_coords, pixel_y_coords), -1)
def _get_planar_to_radial_depth_scalars(self):
# reference: https://en.wikipedia.org/wiki/Spherical_coordinate_system#Cartesian_coordinates
# polar coord image
coord_img = self._create_uniform_pixel_coords_image([self._omni_resolution[1], self._omni_resolution[0]])
pixels_per_rad = self._omni_resolution[1] / np.pi
polar_angles = coord_img / pixels_per_rad
phi = polar_angles[..., 0:1]
theta = polar_angles[..., 1:2]
# image borders wrt the 6 projective cameras
phis_rel = ((phi + np.pi/4) % (np.pi/2)) / (np.pi/2)
lower_borders = np.arccos(1/((phis_rel*2-1)**2 + 2)**0.5)
upper_borders = np.pi - lower_borders
# omni image regions from the 6 projective cameras
xy_region = np.logical_and(theta <= upper_borders, theta >= lower_borders)
pos_x = np.logical_and(xy_region, np.logical_or(phi <= 45 * np.pi/180, phi >= (45 + 270) * np.pi/180))
pos_y = np.logical_and(xy_region,
np.logical_and(phi >= 45 * np.pi/180, phi <= (45 + 90) * np.pi/180))
neg_x = np.logical_and(xy_region,
np.logical_and(phi >= (45 + 90) * np.pi/180, phi <= (45 + 180) * np.pi/180))
neg_y = np.logical_and(xy_region,
np.logical_and(phi >= (45 + 180) * np.pi/180, phi <= (45 + 270) * np.pi/180))
pos_z = theta <= lower_borders
neg_z = theta >= upper_borders
# trig terms for conversion
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
sin_phi = np.sin(phi)
cos_phi = np.cos(phi)
# reciprocal terms
recip_sin_theta_cos_phi = 1/(sin_theta * cos_phi + MIN_DIVISOR)
recip_sin_theta_sin_phi = 1/(sin_theta * sin_phi + MIN_DIVISOR)
recip_cos_theta = 1/(cos_theta + MIN_DIVISOR)
# planar to radial depth scalars
return np.where(pos_x, recip_sin_theta_cos_phi,
np.where(neg_x, -recip_sin_theta_cos_phi,
np.where(pos_y, recip_sin_theta_sin_phi,
np.where(neg_y, -recip_sin_theta_sin_phi,
np.where(pos_z, recip_cos_theta,
np.where(neg_z, -recip_cos_theta,
np.zeros_like(recip_cos_theta)))))))[..., 0]
# Public #
# -------#
def handle_explicitly(self) -> None:
"""Handle spherical vision sensor explicitly.
This enables capturing image (e.g., capture_rgb())
without PyRep.step().
"""
utils.script_call('handleSpherical@PyRep', PYREP_SCRIPT_TYPE,
[self._sensor_depth._handle, self._sensor_rgb._handle] + self._six_sensor_handles, [], [], [])
def capture_rgb(self) -> np.ndarray:
"""Retrieves the rgb-image of a spherical vision sensor.
:return: A numpy array of size (width, height, 3)
"""
rgb = self._sensor_rgb.capture_rgb()
if self._flip_x:
return np.flip(rgb, 1)
return rgb
def capture_depth(self, in_meters=False) -> np.ndarray:
"""Retrieves the depth-image of a spherical vision sensor.
:param in_meters: Whether the depth should be returned in meters.
:return: A numpy array of size (width, height)
"""
planar_depth = self._sensor_depth.capture_rgb()[:, :, 0]*self._clipping_plane_diff + self._near_clipping_plane
radial_depth = planar_depth * self._planar_to_radial_depth_scalars
if in_meters:
if self._flip_x:
return np.flip(radial_depth, 1)
return radial_depth
scaled_depth = (radial_depth - self._near_clipping_plane)/self._clipping_plane_diff
if self._flip_x:
return np.flip(scaled_depth, 1)
return scaled_depth
def get_resolution(self) -> List[int]:
""" Return the spherical vision sensor's resolution.
:return: Resolution [x, y]
"""
return self._sensor_depth.get_resolution()
def set_resolution(self, resolution: List[int]) -> None:
""" Set the spherical vision sensor's resolution.
:param resolution: New resolution [x, y]
"""
if resolution[0] != 2*resolution[1]:
raise Exception('Spherical vision sensors must have an X resolution 2x the Y resolution')
if resolution[1] % 2 != 0:
raise Exception('Spherical vision sensors must have an even Y resolution')
box_sensor_resolution = [int(resolution[1]/2), int(resolution[1]/2)]
for sensor in self._six_sensors:
sensor.set_resolution(box_sensor_resolution)
self._sensor_depth.set_resolution(resolution)
self._sensor_rgb.set_resolution(resolution)
self._omni_resolution = resolution
self._planar_to_radial_depth_scalars = self._get_planar_to_radial_depth_scalars()
def get_render_mode(self) -> RenderMode:
""" Retrieves the spherical vision sensor's rendering mode
:return: RenderMode for the current rendering mode.
"""
return self._sensor_depth.get_render_mode()
def set_render_mode(self, render_mode: RenderMode) -> None:
""" Set the spherical vision sensor's rendering mode
:param render_mode: The new sensor rendering mode, one of:
RenderMode.OPENGL
RenderMode.OPENGL_AUXILIARY
RenderMode.OPENGL_COLOR_CODED
RenderMode.POV_RAY
RenderMode.EXTERNAL
RenderMode.EXTERNAL_WINDOWED
RenderMode.OPENGL3
RenderMode.OPENGL3_WINDOWED
"""
self._sensor_depth.set_render_mode(render_mode)
self._sensor_rgb.set_render_mode(render_mode)
for sensor in self._six_sensors:
sensor.set_render_mode(render_mode)
def get_windowed_size(self) -> Sequence[int]:
"""Get the size of windowed rendering for the omni-directional image.
:return: The (x, y) resolution of the window. 0 for full-screen.
"""
return self._sensor_depth.get_windowed_size()
def set_windowed_size(self, resolution: Sequence[int] = (0, 0)) -> None:
"""Set the size of windowed rendering for the omni-directional image.
:param resolution: The (x, y) resolution of the window.
0 for full-screen.
"""
self._sensor_depth.set_windowed_size(resolution)
self._sensor_rgb.set_windowed_size(resolution)
def get_near_clipping_plane(self) -> float:
""" Get the spherical depth sensor's near clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_near_clipping_plane()
def set_near_clipping_plane(self, near_clipping: float) -> None:
""" Set the spherical depth sensor's near clipping plane.
:param near_clipping: New near clipping plane (in metres)
"""
self._near_clipping_plane = near_clipping
self._clipping_plane_diff = self._far_clipping_plane - near_clipping
for sensor in self._six_sensors:
sensor.set_near_clipping_plane(near_clipping)
def get_far_clipping_plane(self) -> float:
""" Get the Sensor's far clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_far_clipping_plane()
def set_far_clipping_plane(self, far_clipping: float) -> None:
""" Set the spherical depth sensor's far clipping plane.
:param far_clipping: New far clipping plane (in metres)
"""
self._far_clipping_plane = far_clipping
self._clipping_plane_diff = far_clipping - self._near_clipping_plane
for sensor in self._six_sensors:
sensor.set_far_clipping_plane(far_clipping)
def set_entity_to_render(self, entity_to_render: int) -> None:
""" Set the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 to render all objects in scene.
:param entity_to_render: Handle of the entity to render
"""
self._sensor_depth.set_entity_to_render(entity_to_render)
self._sensor_rgb.set_entity_to_render(entity_to_render)
def get_entity_to_render(self) -> None:
""" Get the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 if all objects in scene are rendered.
:return: Handle of the entity to render
"""
return self._sensor_depth.get_entity_to_render()
class environment(object):
def __init__(self,
continuous_control=True,
obs_lowdim=True,
rpa=3,
frames=4):
self.pr = PyRep()
SCENE_FILE = join(dirname(abspath(__file__)), 'reacher_v2.ttt')
self.pr.launch(SCENE_FILE, headless=True)
self.pr.start()
self.continuous_control = continuous_control
self.target = Shape('target')
self.tip = Dummy('Reacher_tip')
self.camera = VisionSensor('Vision_sensor')
self.agent = Reacher()
self.done = False
self.rpa = rpa
self.obs_lowdim = obs_lowdim
self.frames = frames
self.prev_obs = []
self.steps_ep = 0
self.increment = 4 * pi / 180 # to radians
self.action_all = [[self.increment, self.increment],
[-self.increment, -self.increment],
[0, self.increment], [0, -self.increment],
[self.increment, 0], [-self.increment, 0],
[-self.increment, self.increment],
[self.increment, -self.increment]]
def threshold_check(self):
for _ in range(5):
self.reset_target_position(random_=True)
while True:
self.pr.step()
tip_pos = self.tip_position()
dist_tip_target = sqrt((tip_pos[0] - self.target_pos[0])**2 + \
(tip_pos[1] - self.target_pos[1])**2)
if dist_tip_target < 0.07:
reward = 1
self.done = True
break
else:
reward = -dist_tip_target / 5
print('Reward:%s' % reward)
def render(self):
return self.camera.capture_rgb() * 256 # (dim,dim,3)
def get_observation(self):
if self.obs_lowdim:
joints_pos = self.agent.get_joint_positions()
cos_joints = []
sin_joints = []
for theta in joints_pos:
cos_joints.append(cos(theta))
sin_joints.append(sin(theta))
joints_vel = self.agent.get_joint_velocities()
target_pos = self.target_position()
tip_pos = self.tip_position()
tip_target_vec = np.array(tip_pos) - np.array(target_pos)
return torch.tensor(
np.concatenate((cos_joints, sin_joints, joints_pos, joints_vel,
tip_target_vec[0:2]),
axis=0)).float()
else:
new_obs = resize(self.render()).view(-1, 64, 64)
if self.steps_ep == 0:
obs = new_obs.expand(self.frames, 64, 64)
else:
obs = self.prev_obs
for i in range(self.frames - 1):
obs[i, :, :] = obs[i + 1, :, :]
obs[self.frames - 1, :, :] = new_obs
self.prev_obs = obs
return obs.view(-1, 64, 64)
def step(self, action):
self.steps_ep += 1
#TODO: change directly from pos to vel without changing in scene
for action_rep in range(self.rpa):
if self.continuous_control == True:
self.agent.set_joint_target_velocities(action) # radians/s
else:
position_all = self.action_all[action]
joints_pos = self.agent.get_joint_positions()
self.agent.set_joint_target_positions([
joints_pos[0] + position_all[0],
joints_pos[1] + position_all[1]
]) # radians
self.pr.step()
tip_pos = self.tip_position()
tip_target_dist = sqrt((tip_pos[0] - self.target_pos[0])**2 + \
(tip_pos[1] - self.target_pos[1])**2)
if tip_target_dist < 0.07:
reward = 1
self.done = True
else:
reward = -tip_target_dist / 3
state = self.get_observation()
return state, reward, self.done
def tip_position(self):
return self.tip.get_position()
def target_position(self):
return self.target.get_position()
def reset_target_position(self, random_=False, x=0.4, y=0.4):
if random_ == True:
xy_min = 0.12
xy_max = 0.5325
x = random.random() * (xy_max - xy_min) + xy_min
y_max = sqrt(xy_max**2 - x**2)
y_min = 0
y = random.random() * (y_max - y_min) + y_min
quadrant = random.randint(1, 4)
if quadrant == 1:
x = -x
y = -y
elif quadrant == 2:
x = -x
y = y
elif quadrant == 3:
x = x
y = -y
elif quadrant == 4:
x = x
y = y
self.target.set_position([x, y, 0.0275])
self.target_pos = self.target_position()
self.done = False
def reset_robot_position(self, random_=False, joints_pos=[0, 0]):
if random_ == True:
joints_pos = [random.random() * 2 * pi for _ in range(2)]
self.agent.set_joint_positions(joints_pos) # radians
if self.continuous_control:
for _ in range(2):
self.agent.set_joint_target_velocities([0, 0])
self.pr.step()
def sample_action(self):
return [
1.3 * (3 * random.random() - 1.5),
1.3 * (3 * random.random() - 1.5)
]
def display(self):
img = self.camera.capture_rgb()
plt.imshow(img, interpolation='nearest')
plt.axis('off')
plt.show()
plt.pause(0.01)
def random_agent(self, episodes=10):
steps_all = []
for _ in range(episodes):
steps = 0
while True:
action = random.randrange(len(self.action_all))
reward = self.step(action)
steps += 1
if steps == 40:
break
if reward == 1:
steps_all.append(steps)
break
return sum(steps_all) / episodes
def terminate(self):
self.pr.stop() # Stop the simulation
self.pr.shutdown()
def reset(self):
self.steps_ep = 0
self.reset_target_position(random_=True)
self.reset_robot_position(random_=True)
state = self.get_observation()
return state
def action_space(self):
return 2
def action_type(self):
return 'continuous'
def observation_space(self):
return self.get_observation().shape
def step_limit(self):
return 60
interpolate=True,
decal_mode=False,
repeat_along_u=True,
repeat_along_v=True,
uv_scaling=[1, 1])
j.set_color([1, 1, 1])
texture_id, texture_object = _get_texture(pr)
color = [random() for _ in range(3)]
for i in obj2:
i.set_texture(texture=texture_id,
mapping_mode=TextureMappingMode.PLANE,
interpolate=True,
decal_mode=False)
i.set_color([1, 1, 1])
for _ in range(30):
pr.step()
else:
[j.remove_texture() for j in walls]
for i in obj2:
i.remove_texture()
i.set_color([176 / 255, 58 / 255, 46 / 255])
floor.remove_texture()
for _ in range(30):
pr.step()
rgb_img_rand = camera_arm.capture_rgb()
img = Image.fromarray(np.uint8(rgb_img_rand * 255), 'RGB')
img.save('test_rcan%s.png' % (k))
class EnvPollos(Env):
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
self.vrep = vrep
logging.basicConfig(level=logging.DEBUG)
# they have to be simmetric. We interpret actions as angular increments
#self.action_space = Box(low=np.array([-0.1, -1.7, -2.5, -1.5, 0.0, 0.0]),
# high=np.array([0.1, 1.7, 2.5, 1.5, 0.0, 0.0]))
inc = 0.1
self.action_space = Box(low=np.array([-inc, -inc, -inc, -inc, 0,
-inc]),
high=np.array([inc, inc, inc, inc, 0, inc]))
self.observation_space = Box(low=np.array([-0.5, 0.0, -0.2]),
high=np.array([0.5, 1.0, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
self.joints = [
Joint('UR10_joint1'),
Joint('UR10_joint2'),
Joint('UR10_joint3'),
Joint('UR10_joint4'),
Joint('UR10_joint5'),
Joint('UR10_joint6')
]
#self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
# for j in self.joints]
self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3]
self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5]
#self.agent_hand = Shape('hand')
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip(
).get_quaternion()
self.initial_agent_tip_euler = self.agent.get_tip().get_orientation()
self.target = Dummy('UR10_target')
self.initial_target_orientation = self.target.get_orientation()
self.initial_joint_positions = self.agent.get_joint_positions()
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
#self.table_target = Dummy('table_target')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
#self.table_target = Dummy('table_target')
#self.succion = Shape('suctionPad')
self.waypoints = [Dummy('dummy_gancho%d' % i) for i in range(4)]
# objects
#self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')]
#self.agent_tip = self.agent.get_tip()
self.initial_distance = np.linalg.norm(
np.array(self.initial_pollo_position) -
np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(
self.camera.get_handle(), -1)
self.np_camera_matrix_extrinsics = np.delete(
np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width / 2.0) / math.tan(angle / 2.0)
focaly_px = (height / 2.0) / math.tan(angle / 2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def reset(self):
pos = list(
np.random.uniform([-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) +
self.initial_pollo_position)
self.pollo.set_position(pos)
self.pollo.set_quaternion(self.initial_pollo_orientation)
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_epoch_time = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False, True)):
break
print("ENV RESET DONE")
return self._get_state()
def step(self, action):
if action is None:
self.pr.step()
return self._get_state(), 0.0, False, {}
# check for nan
if np.any(np.isnan(action)):
print(action)
return self._get_state(), -1.0, False, {}
# action[1] = np.interp(action[1], [-1,7,1.7], [-0.2,1.7])
# action[2] = np.interp(action[2], [-2.5,2.5], [0,2.5])
# action[3] = np.interp(action[3], [-1.5,1.5], [-1.5,0])
# add actions as increments to current joints value
new_joints = np.array(
self.agent.get_joint_positions()) + np.array(action)
# check limits
if np.any(np.greater(new_joints, self.high_joints_limits)) or np.any(
np.less(new_joints, self.low_joints_limits)):
logging.debug("New joints value out of limits r=-10")
return self._get_state(), -10.0, True, {}
# move the robot and wait to stop
self.agent.set_joint_target_positions(new_joints)
reloj = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False,
True)) or (time.time() - reloj) > 3:
break
# compute relevant magnitudes
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
dist = np.linalg.norm(np_pollo_target - np_robot_tip_position)
altura = np.clip((np_pollo_target[2] - self.initial_pollo_position[2]),
0, 0.5)
for obj in self.scene_objects: # colisión con la mesa
if self.agent_hand.check_collision(obj):
logging.debug("Collision with objects r=-10")
return self._get_state(), -10.0, True, {}
if altura > 0.3: # pollo en mano
logging.debug("Height reached !!! r=0")
return self._get_state(), 30.0, True, {}
elif dist > self.initial_distance: # mano se aleja
logging.debug("Hand moving away from chicken r=-10")
return self._get_state(), -10.0, True, {}
if time.time() - self.initial_epoch_time > 5: # too much time
logging.debug("Time out. End of epoch r=-10")
return self._get_state(), -10.0, True, {}
# Reward
#pollo_height = np.exp(-altura*20) # para 0.3m pollo_height = 0.002, para 0m pollo_height = 1
reward = altura - dist
logging.debug("New joints value out of limits r=-10")
return self._get_state(), reward, False, {}
def close(self):
self.pr.stop()
self.pr.shutdown()
def render(self):
print("RENDER")
np_pollo_en_camara = self.getPolloEnCamara()
# capture depth image
depth = self.camera.capture_rgb()
circ = plt.Circle(
(int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])), 10)
plt.clf()
fig = plt.gcf()
ax = fig.gca()
ax.add_artist(circ)
ax.imshow(depth, cmap='hot')
plt.pause(0.000001)
# Aux
# transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates
def getPolloEnCamara(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot(
np_pollo_target_cam)
np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2]
np_pollo_en_camara = np.delete(np_pollo_en_camara, 2)
return np_pollo_en_camara
def getPolloEnCamaraEx(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
return np_pollo_en_camara
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
# return (self.agent.get_joint_positions() +
# self.agent.get_joint_velocities() +
# self.pollo_target.get_position())
p = self.getPolloEnCamaraEx()
j = self.agent.get_joint_positions()
#r = np.array([p[0],p[1],p[2],j[0],j[1],j[2],j[3],j[4],j[5]])
r = np.array([p[0], p[1], p[2]])
return r
def vrepToNP(self, c):
return np.array([[c[0], c[4], c[8], c[3]], [c[1], c[5], c[9], c[7]],
[c[2], c[6], c[10], c[11]], [0, 0, 0, 1]])
class PyrepDevice(object):
"""
PyrepDevice handles communication with CoppeliaSim Vision Sensors via
Pyrep. Retrieved images are converted to cv2 format and passed to registered
callback functions.
"""
@staticmethod
def autodetect_nicoeyes():
"""
Returns a tuple containing the detected names of left and right eye
sensor
:return: Devicenames as (left, right) tuple (None if not found)
:rtype: tuple
"""
eye_sensors = ["left_eye", "right_eye"]
for i in range(len(eye_sensors)):
if not VisionSensor.exists(eye_sensors[i]):
logger.warning(
"Could not find vision sensor named '%s'", eye_sensors[i]
)
eye_sensors[i] = None
return tuple(eye_sensors)
def __init__(
self,
sensor_name,
width=640,
height=480,
near_clipping_plane=1e-2,
far_clipping_plane=10.0,
view_angle=60.0,
):
"""
Connects to Vision Sensor with given name and updates parameters
accordingly.
:param sensor_name: name (or handle) of the vision sensor in the scene
:type sensor_name: str
:param width: horizontal camera resolution
:type width: int
:param heigth: vertical camera resolution
:type heigth: int
:param near_clipping_plane: the minimum distance from which the sensor will be able to detect.
:type near_clipping_plane: float
:param far_clipping_plane: the maximum distance from which the sensor will be able to detect.
:type far_clipping_plane: float
:param view_angle: field of view of the camera in degree.
:type view_angle: float
"""
self._sensor = VisionSensor(sensor_name)
self.resolution = (width, height)
self.near_clipping_plane = near_clipping_plane
self.far_clipping_plane = far_clipping_plane
self.view_angle = view_angle
self._callback = []
self._removed_ids = []
ref = weakref.ref(self, StepListener.unregister)
StepListener.register(ref)
@property
def sensor_name(self):
"""
:return: name of the sensor in the scene
:rtype: str
"""
return self._sensor.get_name()
@property
def resolution(self):
"""
:return: camera resolution (width, height)
:rtype: tuple(int, int)
"""
return tuple(self._sensor.get_resolution())
@resolution.setter
def resolution(self, res):
"""
:param res: camera resolution (width, height)
:type res: tuple(int, int)
"""
self._sensor.set_resolution(res)
@property
def near_clipping_plane(self):
"""
:return: the minimum distance from which the sensor will be able to detect.
:rtype: float
"""
return self._sensor.get_near_clipping_plane()
@near_clipping_plane.setter
def near_clipping_plane(self, val):
"""
:param val: the minimum distance from which the sensor will be able to detect.
:type val: float
"""
self._sensor.set_near_clipping_plane(val)
@property
def far_clipping_plane(self):
"""
:return: the maximum distance from which the sensor will be able to detect.
:rtype: float
"""
return self._sensor.get_far_clipping_plane()
@far_clipping_plane.setter
def far_clipping_plane(self, val):
"""
:param val: the maximum distance from which the sensor will be able to detect.
:type val: float
"""
self._sensor.set_far_clipping_plane(val)
@property
def view_angle(self):
"""
:return: field of view of the camera in degree.
:rtype: float
"""
return self._sensor.get_perspective_angle()
@view_angle.setter
def view_angle(self, val):
"""
:param val: field of view of the camera in degree.
:type val: float
"""
self._sensor.set_perspective_angle(val)
def get_image(self):
"""
Captures an image for the current simulation step from the
vision sensor and converts it to cv2 format
:return: the image
:rtype: cv2 image
"""
frame = self._sensor.capture_rgb()
frame = np.uint8(frame[:, :, [2, 1, 0]] * 255)
return frame
def step(self):
"""
Executes all registered callbacks with the sensor image for the current
simulation step.
"""
frame = self.get_image()
id_start = 0
# iterate through callbacks while skipping removed ids (if any)
for id_end in self._removed_ids:
for id in range(id_start, id_end):
self._callback[id](frame)
id_start = id_end
# iterate through (remaining) callbacks
for id in range(id_start, len(self._callback)):
self._callback[id](frame)
def add_callback(self, function):
"""
Adds a callback for the event loop
Whenever step is called, all registered callbacks are executed.
The callback must take exactly 1 argument through which it receives the
frame.
:param function: Function to add as callback
:type function: function
:return: id of the added callback
:rtype: int
"""
if not (inspect.isfunction(function) or inspect.ismethod(function)):
logger.error("Trying to add non-function callback")
raise ValueError("Trying to add non-function callback")
if len(self._removed_ids) == 0:
self._callback += [function]
id = len(self._callback) - 1
else:
id = min(self._removed_ids)
self._callback[id]
self._removed_ids.remove(id)
logger.info("Added callback with id %i", id)
return id
def remove_callback(self, id):
"""
Removes callback with given id
:param id: id of the callback
:type id: int
"""
if id in self._removed_ids or id not in range(0, len(self._callback)):
logger.warning("Callback with id %i does not exist", id)
return
if id == len(self._callback) - 1:
i = id
for removed in reversed(self._removed_ids):
if i - 1 != removed:
break
self._removed_ids[:-1]
i -= 1
self._callback = self._callback[:i]
else:
self._callback[id] = None
self._removed_ids += [id]
self._removed_ids.sort()
logger.info("Removed callback with id %i", id)
def clean_callbacks(self):
"""
Removes all saved callbacks
"""
self._callback.clear()
self._removed_ids.clear()
logger.info("Cleared callbacks")
class Env:
def __init__(self, cfg):
self.enabled_joints = cfg.agent.enabled_joints
self._launch(cfg.env.env_path, cfg.env.headless)
self._setup_robot()
self._setup_vision("Vision_sensor")
self._setup_actions(cfg.agent.n_discrete_actions, cfg.agent.vel_min,
cfg.agent.vel_max)
self._setup_debug_cameras("debug_vis1", "debug_vis2")
self._setup_target()
self._setup_distractor()
self._setup_table()
def _setup_distractor(self):
self.distractor = Shape('distractor')
self.hide_distractor()
def _setup_table(self):
self.table = Shape("diningTable")
self.table_near = False
self.table_initial_pos = self.table.get_position()
def _setup_target(self):
self.target = Shape('target')
def _setup_robot(self):
self.robot = Panda()
self.joint_init = self.robot.get_joint_positions()
self.robot.set_control_loop_enabled(False)
self.robot.set_motor_locked_at_zero_velocity(True)
self.tip = self.robot.get_tip()
def _setup_vision(self, vis_name):
self.vision = VisionSensor(vis_name)
def _launch(self, path, headless):
self.pr = PyRep()
self.pr.launch(path, headless=headless)
self.pr.start()
def _setup_actions(self, n_discrete_actions, vel_min, vel_max):
self.poss_actions = np.linspace(vel_min, vel_max, n_discrete_actions)
def _convert_action(self, action):
return self.poss_actions[action]
def _setup_debug_cameras(self, name0, name1):
self.vis_debug0 = VisionSensor(name0)
self.vis_debug1 = VisionSensor(name1)
def step(self, action):
converted_action = self._convert_action(action)
action = np.zeros(7)
action[self.enabled_joints] = converted_action
self.robot.set_joint_target_velocities(action)
self.pr.step()
reward = self._calculate_reward()
rgb = self.vision.capture_rgb()
reward = 0
done = False
# todo: change to include more meaningful info
return rgb, reward, done, {}
def show_distractor(self):
self.distractor.set_position([0.15, 0., 1.])
def hide_distractor(self):
self.distractor.set_position([-1.5, 0., 1.])
def toggle_table(self):
if self.table_near:
self._move_table_far()
else:
self._move_table_near()
def _move_table_near(self):
print("moving table near")
pos = self.table_initial_pos
pos[0] -= 1
self.table.set_position(pos)
def _move_table_far(self):
self.table.set_position(self.table_initial_pos)
def _calculate_reward(self):
ax, ay, az = self.tip.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
reward = -np.sqrt((ax - tx)**2 + (ay - ty)**2 + (az - tz)**2)
return reward
def reset(self):
self.robot.set_joint_positions(self.joint_init)
rgb = self.vision.capture_rgb()
return rgb
def get_debug_images(self):
debug0 = self.vis_debug0.capture_rgb()
debug1 = self.vis_debug1.capture_rgb()
return debug0, debug1
def get_joint_positions(self):
pos = np.array(self.robot.get_joint_positions())
pos = pos[self.enabled_joints]
return pos
class PioneerEnv(object):
def __init__(self,
escene_name='proximity_sensor.ttt',
start=[100, 100],
goal=[180, 500],
rand_area=[100, 450],
path_resolution=5.0,
margin=0.,
margin_to_goal=0.5,
action_max=[.5, 1.],
action_min=[0., 0.],
_load_path=True,
path_name="PathNodes",
type_of_planning="PID",
type_replay_buffer="random",
max_laser_range=1.0,
headless=False):
SCENE_FILE = join(dirname(abspath(__file__)), escene_name)
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = Pioneer("Pioneer_p3dx",
type_of_planning=type_of_planning,
type_replay_buffer=type_replay_buffer)
self.agent.set_control_loop_enabled(False)
self.initial_joint_positions = self.agent.get_joint_positions()
self.initial_position = self.agent.get_position()
print(f"Agent initial position: {self.initial_position}")
self.vision_map = VisionSensor("VisionMap")
self.vision_map.handle_explicitly()
self.vision_map_handle = self.vision_map.get_handle()
self.floor = Shape("Floor_respondable")
self.perspective_angle = self.vision_map.get_perspective_angle # degrees
self.resolution = self.vision_map.get_resolution # list [resx, resy]
self.margin = margin
self.margin_to_goal = margin_to_goal
self.rand_area = rand_area
self.start = start
self.goal = goal
self.type_of_planning = type_of_planning
self.max_laser_range = max_laser_range
self.max_angle_orientation = np.pi
scene_image = self.vision_map.capture_rgb() * 255
scene_image = np.flipud(scene_image)
self.rew_weights = [1, 10000, 5000]
self.start_position = Dummy("Start").get_position()
self.goal_position = Dummy("Goal").get_position() # [x, y, z]
self.local_goal_aux = Dummy("Local_goal_aux")
self.local_goal_aux_pos_list = [[-3.125, 2.175, 0], [2.9, 3.575, 0],
self.goal_position, self.goal_position]
self.ind_local_goal_aux = 0
self.max_distance = get_distance([-7.5, -4.1, 0.], self.goal_position)
self.distance_to_goal_m1 = get_distance(self.start_position,
self.goal_position)
self.c_lin_vel = self.c_ang_vel = self.b_lin_vel = self.b_ang_vel = 0.
self.action_max = action_max
self.action_min = action_min
if type_of_planning == 'PID':
self.planning_info_logger = get_logger("./loggers",
"Planning_Info.log")
self.image_path = None
if exists("./paths/" + path_name + ".npy") and _load_path:
print("Load Path..")
self.image_path = np.load("./paths/" + path_name + ".npy",
allow_pickle=True)
else:
print("Planning...")
self.image_path = self.Planning(
scene_image,
self.start,
self.goal,
self.rand_area,
path_resolution=path_resolution,
logger=self.planning_info_logger)
assert self.image_path is not None, "path should not be a Nonetype"
self.real_path = self.path_image2real(self.image_path,
self.start_position)
# project in coppelia sim
sim_drawing_points = 0
point_size = 10 #[pix]
duplicate_tolerance = 0
parent_obj_handle = self.floor.get_handle()
max_iter_count = 999999
ambient_diffuse = (255, 0, 0)
blue_ambient_diffuse = (0, 0, 255)
point_container = sim.simAddDrawingObject(
sim_drawing_points,
point_size,
duplicate_tolerance,
parent_obj_handle,
max_iter_count,
ambient_diffuse=ambient_diffuse)
local_point_container = sim.simAddDrawingObject(
sim_drawing_points,
point_size,
duplicate_tolerance,
parent_obj_handle,
max_iter_count,
ambient_diffuse=blue_ambient_diffuse)
# debug
for point in self.real_path:
point_data = (point[0], point[1], 0)
sim.simAddDrawingObjectItem(point_container, point_data)
# You need to get the real coord in the real world
assert local_point_container is not None, "point container shouldn't be empty"
self.agent.load_point_container(local_point_container)
self.agent.load_path(self.real_path)
def reset(self):
self.pr.stop()
if self.type_of_planning == 'PID':
self.agent.local_goal_reset()
self.pr.start()
self.pr.step()
sensor_state, distance_to_goal, orientation_to_goal = self._get_state()
self.distance_to_goal_m1 = distance_to_goal
self.orientation_to_goal_m1 = orientation_to_goal
sensor_state_n, distance_to_goal, orientation_to_goal = self.normalize_states(
sensor_state, distance_to_goal, orientation_to_goal)
# pos_x, pos_y = self.agent.get_position()[:-1]
return np.hstack((sensor_state_n, distance_to_goal,
orientation_to_goal)), sensor_state
def step(self, action, pf_f=0):
self.agent.set_joint_target_velocities(action)
self.pr.step() # Step the physics simulation
scene_image = self.vision_map.capture_rgb() * 255 # numpy -> [w, h, 3]
sensor_state, distance_to_goal, orientation_to_goal = self._get_state()
self.b_lin_vel, self.b_ang_vel = self.c_lin_vel, self.c_ang_vel
self.c_lin_vel, self.c_ang_vel = self.agent.get_velocity(
) # 3d coordinates
self.c_lin_vel = np.linalg.norm(self.c_lin_vel)
self.c_ang_vel = self.c_ang_vel[-1] # w/r z
reward, done = self._get_reward(sensor_state, distance_to_goal,
orientation_to_goal, pf_f)
sensor_state_n, distance_to_goal, orientation_to_goal = self.normalize_states(
sensor_state, distance_to_goal, orientation_to_goal)
# pos_x, pos_y = self.agent.get_position()[:-1]
state = np.hstack(
(sensor_state_n, distance_to_goal, orientation_to_goal))
return state, reward, scene_image, done, sensor_state
def _get_state(self):
sensor_state = np.array([
proxy_sensor.read()
for proxy_sensor in self.agent.proximity_sensors
]) # list of distances. -1 if not detect anything
goal_transformed = world_to_robot_frame(
self.agent.get_position(), self.goal_position,
self.agent.get_orientation()[-1])
# distance_to_goal = get_distance(goal_transformed[:-1], np.array([0,0])) # robot frame
distance_to_goal = get_distance(self.agent.get_position()[:-1],
self.goal_position[:-1])
orientation_to_goal = np.arctan2(goal_transformed[1],
goal_transformed[0])
return sensor_state, distance_to_goal, orientation_to_goal
def _get_reward(self,
sensor_state,
distance_to_goal,
orientation_to_goal,
pf_f=0):
done = False
r_local_goal = self.update_local_goal_aux()
r_target = -(distance_to_goal - self.distance_to_goal_m1)
r_vel = self.c_lin_vel / self.action_max[0]
# r_orientation = - (orientation_to_goal - self.orientation_to_goal_m1)
reward = self.rew_weights[0] * (r_local_goal + r_vel) # - pf_f/20)
# distance_to_goal = get_distance(agent_position[:-1], goal[:-1])
self.distance_to_goal_m1 = distance_to_goal
self.orientation_to_goal_m1 = orientation_to_goal
# collision check
if self.collision_check(sensor_state, self.margin):
reward = -1 * self.rew_weights[1]
done = True
# goal achievement
if distance_to_goal < self.margin_to_goal:
reward = self.rew_weights[2]
done = True
return reward, done
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
def model_update(self, method="DDPG", pretraining_loop=False):
if method == "DDPG": self.agent.trainer.update()
elif method == "IL":
loss = self.agent.trainer.IL_update()
return loss
elif method == "CoL":
loss = self.agent.trainer.CoL_update(pretraining_loop)
else:
raise NotImplementedError()
def normalize_states(self, sensor_state, distance_to_goal,
orientation_to_goal):
sensor_state = self.normalize_laser(sensor_state, self.norm_func)
distance_to_goal = self.norm_func(distance_to_goal, self.max_distance)
orientation_to_goal = self.norm_func(orientation_to_goal,
self.max_angle_orientation)
return sensor_state, distance_to_goal, orientation_to_goal
def normalize_laser(self, obs, norm_func):
return np.array([
norm_func(o, self.max_laser_range) if o >= 0 else -1 for o in obs
])
def norm_func(self, x, max_value):
return 2 * (1 - min(x, max_value) / max_value) - 1
def set_margin(self, margin):
self.margin = margin
# def set_start_goal_position(self, start_pos, goal_pos):
def update_local_goal_aux(self):
pos = self.agent.get_position()[:-1]
distance = get_distance(pos, self.local_goal_aux.get_position()[:-1])
if distance < 0.5:
self.local_goal_aux.set_position(
self.local_goal_aux_pos_list[self.ind_local_goal_aux])
self.ind_local_goal_aux += 1
return -1 * distance**2
@staticmethod
def collision_check(observations, margin):
if observations.sum() == -1 * observations.shape[0]:
return False
elif observations[observations >= 0].min() < margin:
return True
else:
return False
@staticmethod
def Planning(Map,
start,
goal,
rand_area,
path_resolution=5.0,
logger=None):
"""
:parameter Map(ndarray): Image that planning over with
"""
Map = Image.fromarray(Map.astype(np.uint8)).convert('L')
path, n_paths = main(Map,
start,
goal,
rand_area,
path_resolution=path_resolution,
logger=logger,
show_animation=False)
if path is not None:
np.save("./paths/PathNodes_" + str(n_paths) + ".npy", path)
return path
else:
logger.info("Not found Path")
return None
@staticmethod
def path_image2real(image_path, start):
"""
image_path: np.array[pix] points of the image path
start_array: np.array
"""
scale = 13.0 / 512 # [m/pix]
x_init = start[0]
y_init = start[1]
deltas = [(image_path[i + 1][0] - image_path[i][0],
image_path[i + 1][1] - image_path[i][1])
for i in range(image_path.shape[0] - 1)]
path = np.zeros((image_path.shape[0], 3))
path[0, :] = np.array([x_init, y_init, 0])
for i in range(1, image_path.shape[0]):
path[i, :] = np.array([
path[i - 1, 0] + deltas[i - 1][0] * scale,
path[i - 1, 1] - deltas[i - 1][1] * scale, 0
])
rot_mat = np.diagflat([-1, -1, 1])
tras_mat = np.zeros_like(path)
tras_mat[:, 0] = np.ones_like(path.shape[0]) * 0.3
tras_mat[:, 1] = np.ones_like(path.shape[0]) * 4.65
path = path @ rot_mat + tras_mat
return np.flip(path, axis=0)
class Camera():
def __init__(self,
name,
resolution,
perspective_angle,
min_depth,
max_depth,
near_clipping=0.2,
far_clipping=3):
self.base = Dummy(name)
self.cam_rgb = VisionSensor(name + "_rgb")
self.cam_depth = VisionSensor(name + "_depth")
self.cam_mask = VisionSensor(name + "_mask")
self.cam_all = [self.cam_rgb, self.cam_depth, self.cam_mask]
self.min_depth = min_depth
self.max_depth = max_depth
self.set_resolution(resolution)
self.set_perspective_angle(perspective_angle)
self.set_near_clipping_plane(near_clipping)
self.set_far_clipping_plane(far_clipping)
def get_resolution(self):
return self._resolution
def set_resolution(self, resolution):
for cam in self.cam_all:
cam.set_resolution(resolution)
self._resolution = resolution
def get_perspective_angle(self):
return self._perspective_angle
def set_perspective_angle(self, angle: float):
for cam in self.cam_all:
cam.set_perspective_angle(angle)
self._perspective_angle = angle
def set_near_clipping_plane(self, near_clipping):
self.cam_rgb.set_near_clipping_plane(near_clipping)
self.cam_mask.set_near_clipping_plane(near_clipping)
self.cam_depth.set_near_clipping_plane(self.min_depth)
def set_far_clipping_plane(self, far_clipping):
self.cam_rgb.set_far_clipping_plane(far_clipping)
self.cam_mask.set_far_clipping_plane(far_clipping)
self.cam_depth.set_far_clipping_plane(self.max_depth)
def capture_rgb(self):
return np.uint8(255 * self.cam_rgb.capture_rgb())[:, :, ::-1]
def capture_depth(self):
return np.uint8(255 * self.cam_depth.capture_depth())
def capture_mask(self):
mask = np.uint8(255 * self.cam_mask.capture_rgb())
mask = mask[:, :, 1]
mask[mask != 0] = 1
return mask
def set_pose(self, pose, relative_to=None):
pos = pose[:3]
qx, qy, qz, qw = pose[3:]
quat_ori = Quaternion([qw, qx, qy, qz])
quat_rotate_z = Quaternion(axis=[0, 0, 1], angle=np.radians(180))
quat = quat_ori * quat_rotate_z
qw, qx, qy, qz = quat.elements
quat = [qx, qy, qz, qw]
# quat = pose[3:]
self.base.set_pose([*pos] + [*quat], relative_to=relative_to)
def get_image(self):
rgb = self.capture_rgb()
depth = self.capture_depth()
mask = self.capture_mask()
return rgb, depth, mask
class SphericalVisionSensor(Object):
"""An object able capture 360 degree omni-directional images.
"""
def __init__(self, name):
super().__init__(name)
# final omni-directional sensors
self._sensor_depth = VisionSensor('%s_sensorDepth' % (self.get_name()))
self._sensor_rgb = VisionSensor('%s_sensorRGB' % (self.get_name()))
# directed sub-sensors
names = ['front', 'top', 'back', 'bottom', 'left', 'right']
self._six_sensors = [
VisionSensor('%s_%s' % (self.get_name(), n)) for n in names
]
self._front = self._six_sensors[0]
# directed sub-sensor handles list
self._six_sensor_handles = [
vs.get_handle() for vs in self._six_sensors
]
# set all to explicit handling
self._set_all_to_explicit_handling()
# assert sensor properties are matching
self._assert_matching_resolutions()
self._assert_matching_render_modes()
self._assert_matching_entities_to_render()
self._assert_matching_window_sizes()
self._assert_matching_near_clipping_planes()
self._assert_matching_far_clipping_planes()
# near and far clipping plane
self._near_clipping_plane = self.get_near_clipping_plane()
self._far_clipping_plane = self.get_far_clipping_plane()
self._clipping_plane_diff = self._far_clipping_plane - self._near_clipping_plane
# Private #
# --------#
def _set_all_to_explicit_handling(self):
self._sensor_depth.set_explicit_handling(1)
self._sensor_rgb.set_explicit_handling(1)
for sensor in self._six_sensors:
sensor.set_explicit_handling(1)
def _assert_matching_resolutions(self):
assert self._sensor_depth.get_resolution(
) == self._sensor_rgb.get_resolution()
front_sensor_res = self._front.get_resolution()
for sensor in self._six_sensors[1:]:
assert sensor.get_resolution() == front_sensor_res
def _assert_matching_render_modes(self):
assert self._sensor_depth.get_render_mode(
) == self._sensor_rgb.get_render_mode()
front_sensor_render_mode = self._front.get_render_mode()
for sensor in self._six_sensors[1:]:
assert sensor.get_render_mode() == front_sensor_render_mode
def _assert_matching_entities_to_render(self):
assert self._sensor_depth.get_entity_to_render(
) == self._sensor_rgb.get_entity_to_render()
front_sensor_entity_to_render = self._front.get_entity_to_render()
for sensor in self._six_sensors[1:]:
assert sensor.get_entity_to_render(
) == front_sensor_entity_to_render
def _assert_matching_window_sizes(self):
assert self._sensor_depth.get_windowed_size(
) == self._sensor_rgb.get_windowed_size()
def _assert_matching_near_clipping_planes(self):
front_sensor_near = self._front.get_near_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_near_clipping_plane() == front_sensor_near
def _assert_matching_far_clipping_planes(self):
front_sensor_far = self._front.get_far_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_far_clipping_plane() == front_sensor_far
def _get_requested_type(self) -> ObjectType:
return ObjectType(sim.simGetObjectType(self.get_handle()))
# Public #
# -------#
def handle_explicitly(self) -> None:
"""Handle spherical vision sensor explicitly.
This enables capturing image (e.g., capture_rgb())
without PyRep.step().
"""
utils.script_call(
'handleSpherical@PyRep', PYREP_SCRIPT_TYPE,
[self._sensor_depth._handle, self._sensor_rgb._handle] +
self._six_sensor_handles, [], [], [])
def capture_rgb(self) -> np.ndarray:
"""Retrieves the rgb-image of a spherical vision sensor.
:return: A numpy array of size (width, height, 3)
"""
return self._sensor_rgb.capture_rgb()
def capture_depth(self, in_meters=False) -> np.ndarray:
"""Retrieves the depth-image of a spherical vision sensor.
:param in_meters: Whether the depth should be returned in meters.
:return: A numpy array of size (width, height)
"""
if in_meters:
return self._sensor_depth.capture_rgb(
)[:, :, 0] * self._clipping_plane_diff + self._near_clipping_plane
return self._sensor_depth.capture_rgb()[:, :, 0]
def get_resolution(self) -> List[int]:
""" Return the spherical vision sensor's resolution.
:return: Resolution [x, y]
"""
return self._sensor_depth.get_resolution()
def set_resolution(self, resolution: List[int]) -> None:
""" Set the spherical vision sensor's resolution.
:param resolution: New resolution [x, y]
"""
if resolution[0] != 2 * resolution[1]:
raise Exception(
'Spherical vision sensors must have an X resolution 2x the Y resolution'
)
if resolution[1] % 2 != 0:
raise Exception(
'Spherical vision sensors must have an even Y resolution')
box_sensor_resolution = [
int(resolution[1] / 2),
int(resolution[1] / 2)
]
for sensor in self._six_sensors:
sensor.set_resolution(box_sensor_resolution)
self._sensor_depth.set_resolution(resolution)
self._sensor_rgb.set_resolution(resolution)
def get_render_mode(self) -> RenderMode:
""" Retrieves the spherical vision sensor's rendering mode
:return: RenderMode for the current rendering mode.
"""
return self._sensor_depth.get_render_mode()
def set_render_mode(self, render_mode: RenderMode) -> None:
""" Set the spherical vision sensor's rendering mode
:param render_mode: The new sensor rendering mode, one of:
RenderMode.OPENGL
RenderMode.OPENGL_AUXILIARY
RenderMode.OPENGL_COLOR_CODED
RenderMode.POV_RAY
RenderMode.EXTERNAL
RenderMode.EXTERNAL_WINDOWED
RenderMode.OPENGL3
RenderMode.OPENGL3_WINDOWED
"""
self._sensor_depth.set_render_mode(render_mode)
self._sensor_rgb.set_render_mode(render_mode)
for sensor in self._six_sensors:
sensor.set_render_mode(render_mode)
def get_windowed_size(self) -> Sequence[int]:
"""Get the size of windowed rendering for the omni-directional image.
:return: The (x, y) resolution of the window. 0 for full-screen.
"""
return self._sensor_depth.get_windowed_size()
def set_windowed_size(self, resolution: Sequence[int] = (0, 0)) -> None:
"""Set the size of windowed rendering for the omni-directional image.
:param resolution: The (x, y) resolution of the window.
0 for full-screen.
"""
self._sensor_depth.set_windowed_size(resolution)
self._sensor_rgb.set_windowed_size(resolution)
def get_near_clipping_plane(self) -> float:
""" Get the spherical depth sensor's near clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_near_clipping_plane()
def set_near_clipping_plane(self, near_clipping: float) -> None:
""" Set the spherical depth sensor's near clipping plane.
:param near_clipping: New near clipping plane (in metres)
"""
self._near_clipping_plane = near_clipping
self._clipping_plane_diff = self._far_clipping_plane - near_clipping
for sensor in self._six_sensors:
sensor.set_near_clipping_plane(near_clipping)
def get_far_clipping_plane(self) -> float:
""" Get the Sensor's far clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_far_clipping_plane()
def set_far_clipping_plane(self, far_clipping: float) -> None:
""" Set the spherical depth sensor's far clipping plane.
:param far_clipping: New far clipping plane (in metres)
"""
self._far_clipping_plane = far_clipping
self._clipping_plane_diff = far_clipping - self._near_clipping_plane
for sensor in self._six_sensors:
sensor.set_far_clipping_plane(far_clipping)
def set_entity_to_render(self, entity_to_render: int) -> None:
""" Set the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 to render all objects in scene.
:param entity_to_render: Handle of the entity to render
"""
self._sensor_depth.set_entity_to_render(entity_to_render)
self._sensor_rgb.set_entity_to_render(entity_to_render)
def get_entity_to_render(self) -> None:
""" Get the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 if all objects in scene are rendered.
:return: Handle of the entity to render
"""
return self._sensor_depth.get_entity_to_render()
class TestVisionSensors(TestCore):
def setUp(self):
super().setUp()
[self.pyrep.step() for _ in range(10)]
self.cam = VisionSensor('cam0')
def test_handle_explicitly(self):
cam = VisionSensor.create((640, 480))
# blank image
rgb = cam.capture_rgb()
self.assertEqual(rgb.sum(), 0)
# non-blank image
cam.set_explicit_handling(value=1)
cam.handle_explicitly()
rgb = cam.capture_rgb()
self.assertNotEqual(rgb.sum(), 0)
cam.remove()
def test_capture_rgb(self):
img = self.cam.capture_rgb()
self.assertEqual(img.shape, (16, 16, 3))
# Check that it's not a blank image
self.assertFalse(img.min() == img.max() == 0.0)
def test_capture_depth(self):
img = self.cam.capture_depth()
self.assertEqual(img.shape, (16, 16))
# Check that it's not a blank depth map
self.assertFalse(img.min() == img.max() == 1.0)
def test_create(self):
cam = VisionSensor.create([640, 480],
perspective_mode=True,
view_angle=35.0,
near_clipping_plane=0.25,
far_clipping_plane=5.0,
render_mode=RenderMode.OPENGL3)
self.assertEqual(cam.capture_rgb().shape, (480, 640, 3))
self.assertEqual(cam.get_perspective_mode(),
PerspectiveMode.PERSPECTIVE)
self.assertAlmostEqual(cam.get_perspective_angle(), 35.0, 3)
self.assertEqual(cam.get_near_clipping_plane(), 0.25)
self.assertEqual(cam.get_far_clipping_plane(), 5.0)
self.assertEqual(cam.get_render_mode(), RenderMode.OPENGL3)
def test_get_set_resolution(self):
self.cam.set_resolution([320, 240])
self.assertEqual(self.cam.get_resolution(), [320, 240])
self.assertEqual(self.cam.capture_rgb().shape, (240, 320, 3))
def test_get_set_perspective_mode(self):
for perspective_mode in PerspectiveMode:
self.cam.set_perspective_mode(perspective_mode)
self.assertEqual(self.cam.get_perspective_mode(), perspective_mode)
def test_get_set_render_mode(self):
for render_mode in [RenderMode.OPENGL, RenderMode.OPENGL3]:
self.cam.set_render_mode(render_mode)
self.assertEqual(self.cam.get_render_mode(), render_mode)
def test_get_set_perspective_angle(self):
self.cam.set_perspective_angle(45.0)
self.assertAlmostEqual(self.cam.get_perspective_angle(), 45.0, 3)
def test_get_set_orthographic_size(self):
self.cam.set_orthographic_size(1.0)
self.assertEqual(self.cam.get_orthographic_size(), 1.0)
def test_get_set_near_clipping_plane(self):
self.cam.set_near_clipping_plane(0.5)
self.assertEqual(self.cam.get_near_clipping_plane(), 0.5)
def test_get_set_far_clipping_plane(self):
self.cam.set_far_clipping_plane(0.5)
self.assertEqual(self.cam.get_far_clipping_plane(), 0.5)
def test_get_set_windowed_size(self):
self.cam.set_windowed_size((640, 480))
self.assertEqual(self.cam.get_windowed_size(), (640, 480))
def test_get_set_entity_to_render(self):
self.cam.set_entity_to_render(-1)
self.assertEqual(self.cam.get_entity_to_render(), -1)
class EnvPollos(Env):
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
logging.basicConfig(level=logging.DEBUG)
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 0.8
self.agent.set_control_loop_enabled(True)
#self.agent.set_motor_locked_at_zero_velocity(True)
self.MAX_INC = 0.2
self.joints = [
Joint('UR10_joint1'),
Joint('UR10_joint2'),
Joint('UR10_joint3'),
Joint('UR10_joint4'),
Joint('UR10_joint5'),
Joint('UR10_joint6')
]
#self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
# for j in self.joints]
self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3]
self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5]
self.initial_joint_positions = self.agent.get_joint_positions()
self.agent_hand = Shape('hand')
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip(
).get_quaternion()
self.target = Dummy('UR10_target')
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
self.table_target = Dummy('table_target')
self.table_target = Dummy('table_target')
# objects to check collisions
self.scene_objects = [
Shape('table0'),
Shape('Plane'),
Shape('Plane0'),
Shape('ConcretBlock')
]
self.initial_distance = np.linalg.norm(
np.array(self.initial_pollo_position) -
np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(
self.camera.get_handle(), -1)
self.np_camera_matrix_extrinsics = np.delete(
np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width / 2.0) / math.tan(angle / 2.0)
focaly_px = (height / 2.0) / math.tan(angle / 2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def reset(self):
pos = list(
np.random.uniform([-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) +
self.initial_pollo_position)
self.pollo.set_position(pos)
self.pollo.set_quaternion(self.initial_pollo_orientation)
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_epoch_time = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False, True)):
break
return self._get_state()
def step(self, action):
if action is None:
print(self.total_reward)
return self._get_state(), -10.0, True, {}
# check for nan
if np.any(np.isnan(action)):
print("NAN values ", action)
self.NANS_COMING = True
return self._get_state(), -10.0, True, {}
# check for strange values
# if np.any(np.greater(action, self.MAX_INC)) or np.any(np.less(action, -self.MAX_INC)):
# print("Strange values ", action)
# self.NANS_COMING = True
# return self._get_state(), -10.0, True, {}
# check for NAN in VREP get_position()
if np.any(np.isnan(self.agent.get_tip().get_position())):
print("NAN values in get_position()", action,
self.agent.get_tip().get_position())
return self._get_state(), -10.0, True, {}
self.pr.step()
return self._get_state(), 0, True, {}
def close(self):
self.pr.stop()
self.pr.shutdown()
def render(self):
print("RENDER")
np_pollo_en_camara = self.getPolloEnCamara()
# capture depth image
depth = self.camera.capture_rgb()
circ = plt.Circle(
(int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])), 10)
plt.clf()
fig = plt.gcf()
ax = fig.gca()
ax.add_artist(circ)
ax.imshow(depth, cmap='hot')
plt.pause(0.000001)
# Aux
# transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates
def getPolloEnCamara(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot(
np_pollo_target_cam)
np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2]
np_pollo_en_camara = np.delete(np_pollo_en_camara, 2)
return np_pollo_en_camara
def getPolloEnCamaraEx(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
return np_pollo_en_camara
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
# return (self.agent.get_joint_positions() +
# self.agent.get_joint_velocities() +
# self.pollo_target.get_position())
p = self.getPolloEnCamaraEx()
j = self.agent.get_joint_positions()
#r = np.array([p[0],p[1],p[2],j[0],j[1],j[2],j[3],j[4],j[5]])
r = np.array([p[0], p[1], p[2]])
return r
def vrepToNP(self, c):
return np.array([[c[0], c[4], c[8], c[3]], [c[1], c[5], c[9], c[7]],
[c[2], c[6], c[10], c[11]], [0, 0, 0, 1]])
class ReacherEnv(object):
def __init__(self,
headless,
control_mode='end_position',
visual_control=False):
'''
:visual_control: bool, controlled by visual state or not (vector state).
'''
self.reward_offset = 10.0
self.reward_range = self.reward_offset # reward range for register gym env when using vectorized env wrapper
self.fall_down_offset = 0.1 # for judging the target object fall off the table
self.metadata = [] # gym env format
self.visual_control = visual_control
self.control_mode = control_mode
self.pr = PyRep()
if control_mode == 'end_position': # need to use different scene, the one with all joints in inverse kinematics mode
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/sawyer_reacher_rl_new_ik.ttt')
elif control_mode == 'joint_velocity': # the scene with all joints in force/torche mode for forward kinematics
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/sawyer_reacher_rl_new.ttt')
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = Sawyer()
self.gripper = BaxterGripper()
self.gripper_left_pad = Shape(
'BaxterGripper_leftPad') # the pad on the gripper finger
self.proximity_sensor = ProximitySensor(
'BaxterGripper_attachProxSensor'
) # need the name of the sensor here
self.vision_sensor = VisionSensor(
'Vision_sensor') # need the name of the sensor here
if control_mode == 'end_position':
self.agent.set_control_loop_enabled(True) # if false, won't work
self.action_space = np.zeros(
4) # 3 DOF end position control + 1 rotation of gripper
elif control_mode == 'joint_velocity':
self.agent.set_control_loop_enabled(False)
self.action_space = np.zeros(
8) # 7 DOF velocity control + 1 rotation of gripper
else:
raise NotImplementedError
if self.visual_control == False:
self.observation_space = np.zeros(
17
) # position and velocity of 7 joints + position of the target
else:
self.observation_space = np.zeros(100) # dim of img!
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape('target') # object
self.tip_target = Dummy(
'Sawyer_target') # the target point of the tip to move towards
# self.table = Shape('diningTable')
self.agent_ee_tip = self.agent.get_tip()
self.tip_pos = self.agent_ee_tip.get_position()
self.tip_quat = self.agent_ee_tip.get_quaternion(
) # tip rotation as quaternion, if not control the rotation
# set a proper initial gesture/tip position
if control_mode == 'end_position':
# agent_position=self.agent.get_position()
# initial_pos_offset = [0.4, 0.3, 0.2] # initial relative position of gripper to the whole arm
# initial_pos = [(a + o) for (a, o) in zip(agent_position, initial_pos_offset)]
initial_pos = [0.3, 0.1, 0.9]
self.tip_target.set_position(initial_pos)
# one big step for rotation setting is enough, with reset_dynamics=True, set the rotation instantaneously
self.tip_target.set_orientation(
[0, 3.1415, 1.5707],
reset_dynamics=True) # make gripper face downwards
self.pr.step()
elif control_mode == 'joint_velocity':
self.initial_joint_positions = [
0.001815199851989746, -1.4224984645843506, 0.704303503036499,
2.54307222366333, 2.972468852996826, -0.4989511966705322,
4.105560302734375
]
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_joint_positions = self.agent.get_joint_positions()
self.initial_tip_positions = self.agent_ee_tip.get_position()
self.initial_target_positions = self.target.get_position()
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
return np.array(self.agent.get_joint_positions() +
self.agent.get_joint_velocities() +
self.target.get_position())
def _is_holding(self):
'''
Return is holding the target or not, return bool.
'''
# Nothe that the collision check is not always accurate all the time,
# for continuous conllision, maybe only the first 4-5 frames of collision can be detected
pad_collide_object = self.gripper_left_pad.check_collision(self.target)
if pad_collide_object and self.proximity_sensor.is_detected(
self.target) == True:
return True
else:
return False
def _get_visual_state(self):
# Return a numpy array of size (width, height, 3)
return self.vision_sensor.capture_rgb(
) # A numpy array of size (width, height, 3)
def _is_holding(self):
# Return is holding the target or not, return bool
# Nothe that the collision check is not always accurate all the time,
# for continuous conllision, maybe only the first 4-5 frames of collision can be detected
pad_collide_object = self.gripper_left_pad.check_collision(self.target)
if pad_collide_object and self.proximity_sensor.is_detected(
self.target) == True:
return True
else:
return False
# def _move(self, action):
# '''
# Move the tip according to the action with inverse kinematics for 'end_position' control;
# with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode.
# '''
# robot_moving_unit=0.01 # the amount of single step move of robot, not accurate
# moving_loop_itr=int(np.sum(np.abs(action[:3]))/robot_moving_unit)+1 # adaptive number of moving steps, with minimal of 1 step.
# # print(moving_loop_itr)
# small_step = list(1./moving_loop_itr*np.array(action)) # break the action into small steps, as the robot cannot move to the target position within one frame
# pos=self.agent_ee_tip.get_position()
# '''
# there is a mismatch between the object set_orientation() and get_orientation():
# the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation().
# '''
# ori_z=-self.agent_ee_tip.get_orientation()[2] # the minus is because the mismatch between the set and get
# assert len(small_step) == len(pos)+1 # 3 values for position, 1 value for rotation
# for _ in range(moving_loop_itr):
# for idx in range(len(pos)):
# pos[idx] += small_step[idx]
# self.tip_target.set_position(pos)
# self.pr.step()
# ''' deprecated! no need to use small steps for the rotation with reset_dynamics=True'''
# # ori_z+=small_step[3] # change the orientation along z-axis with a small step
# # self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards
# # self.pr.step()
# ''' one big step for z-rotation is enough, with reset_dynamics=True, set the rotation instantaneously '''
# ori_z+=action[3]
# self.tip_target.set_orientation([0,3.1415,ori_z], reset_dynamics=True) # make gripper face downwards
# self.pr.step()
def _move(self, action):
'''
Move the tip according to the action with inverse kinematics for 'end_position' control;
with control of tip target in inverse kinematics mode instead of using .solve_ik() in forward kinematics mode.
Mode 2: a close-loop control, using ik.
'''
pos = self.gripper.get_position()
bounding_offset = 0.1
step_factor = 0.2 # small step factor mulitplied on the gradient step calculated by inverse kinematics
max_itr = 20 # maximum moving iterations
max_error = 0.1 # upper bound of distance error for movement at each call
rotation_norm = 5. # factor for normalization of rotation values
# check if state+action will be within of the bounding box, if so, move normally; else no action.
# x_min < x < x_max and y_min < y < y_max and z > z_min
if pos[0]+action[0]>POS_MIN[0]-bounding_offset and pos[0]+action[0]<POS_MAX[0]+bounding_offset \
and pos[1]+action[1] > POS_MIN[1]-bounding_offset and pos[1]+action[1] < POS_MAX[1]+bounding_offset \
and pos[2]+action[2] > POS_MIN[2]-bounding_offset:
'''
there is a mismatch between the object set_orientation() and get_orientation():
the (x,y,z) in set_orientation() will be (y,x,-z) in get_orientation().
'''
ori_z = -self.agent_ee_tip.get_orientation()[
2] # the minus is because the mismatch between the set and get
target_pos = np.array(self.agent_ee_tip.get_position()) + np.array(
action[:3])
diff = 1
itr = 0
# print('before: ', ori_z)
while np.sum(np.abs(diff)) > max_error and itr < max_itr:
itr += 1
# set pos in small step
cur_pos = self.agent_ee_tip.get_position()
diff = target_pos - cur_pos
pos = cur_pos + step_factor * diff
self.tip_target.set_position(pos.tolist())
self.pr.step()
''' one big step for z-rotation is enough '''
ori_z += rotation_norm * action[3]
self.tip_target.set_orientation([0, np.pi, ori_z
]) # make gripper face downwards
self.pr.step()
else:
# print("Potential Movement Out of the Bounding Box!")
pass # no action if potentially out of the bounding box
def reset(self):
# Get a random position within a cuboid and set the target position
max_itr = 10
pos = list(np.random.uniform(POS_MIN, POS_MAX))
self.target.set_position(pos)
self.target.set_orientation([0, 0, 0])
self.pr.step()
# changing the color or texture for domain randomization
self.target.set_color(
np.random.uniform(low=0, high=1, size=3).tolist()
) # set [r,g,b] 3 channel values of object color
# set end position to be initialized
if self.control_mode == 'end_position': # JointMode.IK
self.agent.set_control_loop_enabled(True)
self.tip_target.set_position(
self.initial_tip_positions
) # cannot set joint positions directly due to in ik mode nor force/torch mode
self.pr.step()
# prevent stuck case
itr = 0
while np.sum(
np.abs(
np.array(self.agent_ee_tip.get_position() -
np.array(self.initial_tip_positions)))
) > 0.1 and itr < max_itr:
itr += 1
self.step(np.random.uniform(
-0.2, 0.2,
4)) # take random actions for preventing the stuck cases
self.pr.step()
elif self.control_mode == 'joint_velocity': # JointMode.FORCE
self.agent.set_joint_positions(
self.initial_joint_positions
) # sometimes the gripper is stuck, cannot get back to initial
self.pr.step()
# self.table.set_collidable(True)
self.gripper_left_pad.set_collidable(
True
) # set the pad on the gripper to be collidable, so as to check collision
self.target.set_collidable(True)
if np.sum(self.gripper.get_open_amount()) < 1.5:
self.gripper.actuate(1, velocity=0.5) # open the gripper
self.pr.step()
if self.visual_control:
return self._get_visual_state()
else:
return self._get_state()
def step(self, action):
'''
Move the robot arm according to the action.
If control_mode=='joint_velocity', action is 7 dim of joint velocity values + 1 dim of gripper rotation.
if control_mode=='end_position', action is 3 dim of tip (end of robot arm) position values + 1 dim of gripper rotation.
'''
if self.control_mode == 'end_position':
if action is None or action.shape[0] != 4:
action = list(np.random.uniform(-0.1, 0.1, 4)) # random
self._move(action)
elif self.control_mode == 'joint_velocity':
if action is None or action.shape[0] != 7:
print('No actions or wrong action dimensions!')
action = list(np.random.uniform(-0.1, 0.1, 7)) # random
self.agent.set_joint_target_velocities(
action) # Execute action on arm
self.pr.step()
else:
raise NotImplementedError
ax, ay, az = self.gripper.get_position()
tx, ty, tz = self.target.get_position()
# Reward is negative distance to target
distance = (ax - tx)**2 + (ay - ty)**2 + (az - tz)**2
done = False
# print(self.proximity_sensor.is_detected(self.target))
current_vision = self.vision_sensor.capture_rgb(
) # capture a screenshot of the view with vision sensor
plt.imshow(current_vision)
plt.savefig('./img/vision.png')
reward = 0
# close the gripper if close enough to the object and the object is detected with the proximity sensor
if distance < 0.1 and self.proximity_sensor.is_detected(
self.target) == True:
# make sure the gripper is open before grasping
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
self.gripper.actuate(
0, velocity=0.5
) # if done, close the hand, 0 for close and 1 for open.
self.pr.step() # Step the physics simulation
if self._is_holding():
# reward for hold here!
reward += 10
done = True
else:
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
elif np.sum(
self.gripper.get_open_amount()
) < 1.5: # if gripper is closed due to collision or esle, open it; .get_open_amount() return list of gripper joint values
self.gripper.actuate(1, velocity=0.5)
self.pr.step()
else:
pass
reward -= np.sqrt(distance)
if tz < self.initial_target_positions[
2] - self.fall_down_offset: # the object fall off the table
done = True
reward = -self.reward_offset
if self.visual_control:
return self._get_visual_state(), reward, done, {}
else:
return self._get_state(), reward, done, {}
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
def simulate(scene_dir, cls_indices):
# read 3d point cloud vertices npy (for visualization)
vertex_npy = np.load("mesh_data/custom_vertex.npy")
# loop over all scene files in scenes directory
for scene_path in os.listdir(scene_dir):
# check whether it's a scene file or not
if not scene_path[-3:] == 'ttt':
continue
# create an output directory for each scene
scene_out_dir = os.path.join('./sim_data/', scene_path[:-4])
if not os.path.isdir(scene_out_dir):
os.mkdir(scene_out_dir)
# launch scene file
SCENE_FILE = os.path.join(os.path.dirname(os.path.abspath(__file__)), os.path.join(scene_dir, scene_path))
pr = PyRep()
pr.launch(SCENE_FILE, headless=True)
pr.start()
pr.step()
# define vision sensor
camera = VisionSensor('cam')
# set camera absolute pose to world coordinates
camera.set_pose([0,0,0,0,0,0,1])
pr.step()
# define scene shapes
shapes = []
shapes.append(Shape('Shape1'))
shapes.append(Shape('Shape2'))
shapes.append(Shape('Shape3'))
shapes.append(Shape('Shape4'))
pr.step()
print("Getting vision sensor intrinsics ...")
# get vision sensor parameters
cam_res = camera.get_resolution()
cam_per_angle = camera.get_perspective_angle()
ratio = cam_res[0]/cam_res[1]
cam_angle_x = 0.0
cam_angle_y = 0.0
if (ratio > 1):
cam_angle_x = cam_per_angle
cam_angle_y = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio))
else:
cam_angle_x = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio))
cam_angle_y = cam_per_angle
# get vision sensor intrinsic matrix
cam_focal_x = float(cam_res[0] / 2.0) / tan(radians(cam_angle_x / 2.0))
cam_focal_y = float(cam_res[1] / 2.0) / tan(radians(cam_angle_y / 2.0))
intrinsics = np.array([[cam_focal_x, 0.00000000e+00, float(cam_res[0]/2.0)],
[0.00000000e+00, cam_focal_y, float(cam_res[1]/2.0)],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
# loop to get 5000 samples per scene
for i in range(5000):
print("Randomizing objects' poses ...")
# set random pose to objects in the scene
obj_colors = []
for shape in shapes:
shape.set_pose([
random.uniform(-0.25,0.25), random.uniform(-0.25,0.25), random.uniform(0.25,2),
random.uniform(-1,1), random.uniform(-1,1), random.uniform(-1,1),
random.uniform(-1,1)
])
obj_colors.append([random.uniform(0,1), random.uniform(0,1), random.uniform(0,1)])
pr.step()
print("Reading vision sensor RGB signal ...")
# read vision sensor RGB image
img = camera.capture_rgb()
img = np.uint8(img * 255.0)
print("Reading vision sensor depth signal ...")
# read vision sensor depth image
depth = camera.capture_depth()
depth = np.uint8(depth * 255.0)
print("Generating front mask for RGB signal ...")
# generate RGB front mask
front_mask = np.sum(img, axis=2)
front_mask[front_mask != 0] = 255
front_mask = Image.fromarray(np.uint8(front_mask))
alpha_img = Image.fromarray(img)
alpha_img.putalpha(front_mask)
print("Saving sensor output ...")
# save sensor output
alpha_img.save(scene_out_dir+f'/{str(i).zfill(6)}-color.png')
imsave(scene_out_dir+f'/{str(i).zfill(6)}-depth.png', depth)
print("Getting objects' poses ...")
# get poses for all objects in scene
poses = []
for shape in shapes:
poses.append(get_object_pose(shape, camera))
pose_mat = np.stack(poses, axis=2)
# save pose visualization on RGB image
img_viz = visualize_predictions(poses, cls_indices[scene_path], img, vertex_npy, intrinsics)
imsave(scene_out_dir+f'/{str(i).zfill(6)}-viz.png', img_viz)
print("Saving meta-data ...")
# save meta-data .mat
meta_dict = {
'cls_indexes' : np.array(cls_indices[scene_path]),
'intrinsic_matrix' : intrinsics,
'poses' : pose_mat
}
savemat(scene_out_dir+f'/{str(i).zfill(6)}-meta.mat', meta_dict)
print("Performing semantic segmentation of RGB signal ...")
# perform semantic segmentation of RGB image
seg_img = camera.capture_rgb()
seg_img = perform_segmentation(seg_img, cls_indices[scene_path], poses, vertex_npy, intrinsics)
imsave(scene_out_dir+f'/{str(i).zfill(6)}-label.png', seg_img)
# stop simulation
pr.stop()
pr.shutdown()
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
class robotEnv(object):
def __init__(self, scene_name, reward_dense, boundary):
self.pr = PyRep()
SCENE_FILE = join(dirname(abspath(__file__)), scene_name)
self.pr.launch(SCENE_FILE, headless=True)
self.pr.start()
self.pr.set_simulation_timestep(0.05)
if scene_name != 'robot_scene.ttt': #youbot_navig2
home_dir = os.path.expanduser('~')
os.chdir(join(home_dir, 'robotics_drl'))
self.pr.import_model('robot.ttm')
# Vision sensor handles
self.camera_top = VisionSensor('Vision_sensor')
# self.camera_top.set_render_mode(RenderMode.OPENGL3)
# self.camera_top.set_resolution([256,256])
self.camera_arm = VisionSensor('Vision_sensor1')
self.camera_arm.set_render_mode(RenderMode.OPENGL3)
self.camera_arm.set_resolution([256, 256])
self.reward_dense = reward_dense
self.reward_termination = 1 if self.reward_dense else 0
self.boundary = boundary
# Robot links
robot_links = []
links_size = [3, 5, 5, 1]
for i in range(len(links_size)):
robot_links.append(
[Shape('link%s_%s' % (i, j)) for j in range(links_size[i])])
links_color = [[0, 0, 1], [0, 1, 0], [1, 0, 0]]
color_i = 0
for j in robot_links:
if color_i > 2:
color_i = 0
for i in j:
i.remove_texture()
i.set_color(links_color[color_i])
color_i += 1
def render(self, view='top'):
if view == 'top':
img = self.camera_top.capture_rgb() * 256 # (dim,dim,3)
elif view == 'arm':
img = self.camera_arm.capture_rgb()
return img
def terminate(self):
self.pr.stop() # Stop the simulation
self.pr.shutdown()
def sample_action(self):
return [(2 * random.random() - 1) for _ in range(self.action_space)]
def rand_bound(self):
x = random.uniform(-self.boundary, self.boundary)
y = random.uniform(-self.boundary, self.boundary)
orientation = random.random() * 2 * pi
return x, y, orientation
def action_type(self):
return 'continuous'
def observation_space(self):
_, obs = self.get_observation()
return obs.shape
def step_limit(self):
return 250
