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
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 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)
