def get_cost(traj_node, traj_mps, peri_boundary, total_traj_time, habitats, sharkGrid, weights): """ Return the mean cost of the produced path Parameter: trajectory: a list of Node objects """ total_cost = 0 num_steps = 0 max_time = traj_node[-1].time_stamp time = 0 cal_cost = Cost() while time < max_time: currPath_mps = get_curr_mps_path(time, traj_mps) currPath_node = get_curr_node_path(time, traj_node) cost = cal_cost.habitat_shark_cost_func(currPath_mps, currPath_node[-1].pathLen, peri_boundary, total_traj_time, habitats, sharkGrid, weights) # print ("habitat_shark_cost = ", cost[0], "currPath length = ", len(currPath_mps), "currPath_node[-1].pathLen = ", currPath_node[-1].pathLen) total_cost += cost[0] time += 2 # dt = 2s num_steps += 1 mean_cost = total_cost / num_steps print("mean cost = ", total_cost, " / ", num_steps, " = ", mean_cost) return mean_cost
class RRT: """ Class for RRT planning """ def __init__(self, boundary, obstacles, shark_dict, sharkGrid, exp_rate=1, dist_to_end=2, diff_max=0.5, freq=30): '''setting parameters: initial_location: initial Motion_plan_state of AUV, [x, y, z, theta, v, w, time_stamp] goal_location: Motion_plan_state of the shark, [x, y, z] obstacle_list: Motion_plan_state of obstacles [[x1, y1, z1, size1], [x2, y2, z2, size2] ...] boundary: max & min Motion_plan_state of the configuration space [[x_min, y_min, z_min],[x_max, y_max, z_max]]''' #initialize obstacle, boundary, habitats for path planning self.boundary = boundary #initialize corners for boundaries coords = [] for corner in self.boundary: coords.append((corner.x, corner.y)) self.boundary_poly = Polygon(coords) self.obstacle_list = obstacles self.mps_list = [] # a list of motion_plan_state self.time_bin = {} #if minimum path length is not achieved within maximum iteration, return the latest path self.last_path = [] #setting parameters for path planning self.exp_rate = exp_rate self.dist_to_end = dist_to_end self.diff_max = diff_max self.freq = freq # initialize shark occupancy grid self.cell_list = splitCell(self.boundary_poly, 10) self.sharkGrid = sharkGrid self.sharkDict = shark_dict # initialize cost function self.cal_cost = Cost() # keep track of the longest single path in the tree to normalize every path length self.peri_boundary = self.cal_boundary_peri() def replanning(self, start, habitats, plan_time_budget, traj_time_length, replan_time_interval): ''' RRT path planning to continually generate optimal path given the current trajectory AUV is following by choosing certain point along the curretn trajectory as starting node for new RRT planner After replan time interval, RRT planner will be called to generate a new optimal trajectory given the start node. For each iteration of planning, RRT planner is given a plan time constructio budget to plan and generate optimal trajectory with trajectory time length as maxmimum paramters: start: initial node to start with, need to be updated plan_time_budget: Plan construction time budget, i.e. the amount of time dedicated to constructed trajectories for RRT planner traj_time_length: Trajectory time length, i.e. the difference between the the last and first time stamp of the trajectory constructed i.e. how long the AUV will drive around for replan_time_interval: Replan time interval i.e. the time between each query to the planner to construct a new traj ''' traj = [start] time_dict = {} final_traj_time = list(self.sharkGrid.keys())[-1][1] plan_time = (plan_time_budget + replan_time_interval) count = 1 oriHabitats = habitats.copy() while (traj[-1].traj_time_stamp + plan_time) < final_traj_time: if traj_time_length + traj[-1].traj_time_stamp > final_traj_time: traj_time_length = final_traj_time - traj[-1].traj_time_stamp temp = self.exploring(traj[-1], habitats, 0.5, 5, 2, plan_time, traj_time_stamp=True, max_plan_time=plan_time_budget, max_traj_time=(traj_time_length + traj[-1].traj_time_stamp), plan_time=True, weights=[1, -3, -1, -5]) temp_path = temp["path"][1][list(temp["path"][1].keys())[0]] temp_path.reverse() traj.extend(temp_path) time_dict[count] = [temp_path, habitats.copy()] habitats = self.removeHabitat(habitats, temp_path) count += 1 time.sleep(replan_time_interval) length = self.cal_length(traj[1:]) cost = self.cal_cost.habitat_shark_cost_func(traj[1:], length, self.peri_boundary, traj[-1].traj_time_stamp, oriHabitats, self.sharkGrid, weight=[1, -3, -1, -5]) return [traj[1:], time_dict, cost] def exploring(self, initial, habitats, plot_interval, bin_interval, v, shark_interval, traj_time_stamp=False, max_plan_time=5, max_traj_time=200.0, plan_time=True, weights=[1, -1, -1, -1]): """ rrt path planning without setting a specific goal, rather try to explore the configuration space as much as possible calculate cost while expand and keep track of the current optimal cost path max_iter: maximum iteration for the tree to expand plan_time: expand by randomly picking a time stamp and find the motion_plan_state along the path with smallest time difference """ # keep track of the motion_plan_state whose path is optimal opt_cost = [float("inf")] opt_path = None opt_cost_list = [] self.mps_list = [initial] self.t_start = time.time() n_expand = math.ceil(max_plan_time / plot_interval) if traj_time_stamp: time_expand = math.ceil(max_traj_time / bin_interval) for i in range(1, time_expand + 1): self.time_bin[bin_interval * i] = [] self.time_bin[bin_interval].append(initial) for i in range(1, n_expand + 1): t_end = self.t_start + i * plot_interval while time.time() < t_end: # find the closest motion_plan_state by generating a random time stamp and # find the motion_plan_state whose time stamp is closest to it if plan_time: if traj_time_stamp: ran_bin = int(random.uniform(1, time_expand + 1)) while self.time_bin[bin_interval * ran_bin] == []: ran_bin = int(random.uniform(1, time_expand + 1)) ran_index = int( random.uniform( 0, len(self.time_bin[bin_interval * ran_bin]))) closest_mps = self.time_bin[bin_interval * ran_bin][ran_index] else: ran_time = random.uniform(0, max_plan_time * self.freq) closest_mps = self.get_closest_mps_time( ran_time, self.mps_list) if closest_mps.traj_time_stamp > max_traj_time: continue # find the closest motion_plan_state by generating a random motion_plan_state # and find the motion_plan_state with smallest distance else: ran_mps = self.get_random_mps() closest_mps = self.get_closest_mps(ran_mps, self.mps_list) if closest_mps.traj_time_stamp > max_traj_time: continue new_mps = self.steer(closest_mps, self.dist_to_end, self.diff_max, self.freq, 0.5, v, traj_time_stamp) if self.check_collision(new_mps, self.obstacle_list): new_mps.parent = closest_mps self.mps_list.append(new_mps) #add to time stamp bin if traj_time_stamp: curr_bin = (new_mps.traj_time_stamp // bin_interval + 1) * bin_interval if curr_bin > max_traj_time: self.time_bin[curr_bin] = [] self.time_bin[curr_bin].append(new_mps) # if new_mps.traj_time_stamp > longest_traj_time: # longest_traj_time = new_mps.traj_time_stamp #else: # if new_mps.traj_time_stamp > max_traj_time: # continue #Question: how to normalize the path length? if new_mps.traj_time_stamp >= max_traj_time - 30: path = self.generate_final_course(new_mps) new_mps.length = self.cal_length(path) #find the corresponding shark occupancy grid sharkOccupancyDict = {} start = initial.traj_time_stamp end = new_mps.traj_time_stamp for time_bin in self.sharkGrid: if (start >= time_bin[0] and start <= time_bin[1] ) or (time_bin[0] >= start and time_bin[1] <= end) or (end >= time_bin[0] and end <= time_bin[1]): sharkOccupancyDict[time_bin] = self.sharkGrid[ time_bin] new_cost = self.cal_cost.habitat_shark_cost_func( path, new_mps.length, self.peri_boundary, new_mps.traj_time_stamp, habitats, sharkOccupancyDict, weights) if new_cost[0] < opt_cost[0]: opt_cost = new_cost opt_path = [new_mps.length, path] # opt_cost_list.append(opt_cost[0]) path = self.splitPath(opt_path[1], shark_interval, [initial.traj_time_stamp, max_traj_time]) return { "path length": opt_path[0], "path": [opt_path[1], path], "cost": opt_cost } def planning(self, bin_interval=5, v=1, traj_time_stamp=False, max_plan_time=5, max_traj_time=200.0, plan_time=True): """ rrt path planning animation: flag for animation on or off """ self.mps_list = [self.start] self.t_start = time.time() t_end = time.time() + max_plan_time if traj_time_stamp: time_expand = math.ceil(max_traj_time / bin_interval) for i in range(1, time_expand + 1): self.time_bin[bin_interval * i] = [] self.time_bin[bin_interval].append(self.start) while time.time() < t_end: #find the closest motion_plan_state by generating a random time stamp and #find the motion_plan_state whose time stamp is closest to it if plan_time: if traj_time_stamp: ran_bin = int(random.uniform(1, time_expand + 1)) while self.time_bin[bin_interval * ran_bin] == []: ran_bin = int(random.uniform(1, time_expand + 1)) ran_index = int( random.uniform( 0, len(self.time_bin[bin_interval * ran_bin]))) closest_mps = self.time_bin[bin_interval * ran_bin][ran_index] else: ran_time = random.uniform(0, max_plan_time * self.freq) closest_mps = self.get_closest_mps_time( ran_time, self.mps_list) if closest_mps.traj_time_stamp > max_traj_time: continue #find the closest motion_plan_state by generating a random motion_plan_state #and find the motion_plan_state with smallest distance else: ran_mps = self.get_random_mps() closest_mps = self.get_closest_mps(ran_mps, self.mps_list) if closest_mps.traj_time_stamp > max_traj_time: continue new_mps = self.steer(closest_mps, self.dist_to_end, self.diff_max, self.freq, v, traj_time_stamp) if self.check_collision(new_mps, self.obstacle_list): print(new_mps) new_mps.parent = closest_mps self.mps_list.append(new_mps) #add to time stamp bin if traj_time_stamp: curr_bin = (new_mps.traj_time_stamp // bin_interval + 1) * bin_interval if curr_bin > max_traj_time: #continue self.time_bin[curr_bin] = [] self.time_bin[curr_bin].append(new_mps) final_mps = self.connect_to_goal_curve_alt(self.mps_list[-1], self.exp_rate) if self.check_collision(final_mps, self.obstacle_list): final_mps.parent = self.mps_list[-1] path = self.generate_final_course(final_mps) return path return None # cannot find path def steer(self, mps, dist_to_end, diff_max, freq, min_dist, velocity=1, traj_time_stamp=False): #dubins library '''new_mps = Motion_plan_state(from_mps.x, from_mps.y, theta = from_mps.theta) new_mps.path = [] q0 = (from_mps.x, from_mps.y, from_mps.theta) q1 = (to_mps.x, to_mps.y, to_mps.theta) turning_radius = 1.0 path = dubins.shortest_path(q0, q1, turning_radius) configurations, _ = path.sample_many(exp_rate) for configuration in configurations: new_mps.path.append(Motion_plan_state(x = configuration[0], y = configuration[1], theta = configuration[2])) new_mps.path.append(to_mps) dubins_path = new_mps.path new_mps = dubins_path[-2] new_mps.path = dubins_path''' new_mps = Motion_plan_state(mps.x, mps.y, theta=mps.theta, plan_time_stamp=time.time() - self.t_start, traj_time_stamp=mps.traj_time_stamp) new_mps.path = [mps] '''if dist_to_end > d: dist_to_end = dist_to_end / 10 n_expand = math.floor(dist_to_end / exp_rate)''' n_expand = random.uniform(0, freq) n_expand = math.floor(n_expand / 1) for _ in range(n_expand): #setting random parameters dist = random.uniform(0, dist_to_end) #setting random range diff = random.uniform(-diff_max, diff_max) #setting random range if abs(dist) > abs(diff): s1 = dist + diff s2 = dist - diff radius = (s1 + s2) / (-s1 + s2) phi = (s1 + s2) / (2 * radius) ori_theta = new_mps.theta new_mps.theta += phi delta_x = radius * (math.sin(new_mps.theta) - math.sin(ori_theta)) delta_y = radius * (-math.cos(new_mps.theta) + math.cos(ori_theta)) new_mps.x += delta_x new_mps.y += delta_y velocity_temp = random.uniform(0, 2 * velocity) new_mps.plan_time_stamp = time.time() - self.t_start movement = math.sqrt(delta_x**2 + delta_y**2) new_mps.traj_time_stamp += movement / velocity_temp if movement >= min_dist: new_mps.path.append( Motion_plan_state( new_mps.x, new_mps.y, v=velocity_temp, theta=new_mps.theta, traj_time_stamp=new_mps.traj_time_stamp, plan_time_stamp=new_mps.plan_time_stamp)) #d, theta = self.get_distance_angle(new_mps, to_mps) '''d, _ = self.get_distance_angle(new_mps, to_mps) if d <= dist_to_end: new_mps.path.append(to_mps)''' #new_node.parent = from_node new_mps.path[0] = mps return new_mps def connect_to_goal(self, mps, exp_rate, dist_to_end=float("inf")): new_mps = Motion_plan_state(mps.x, mps.y) d, theta = self.get_distance_angle(new_mps, self.goal) new_mps.path = [new_mps] if dist_to_end > d: dist_to_end = d n_expand = math.floor(dist_to_end / exp_rate) for _ in range(n_expand): new_mps.x += exp_rate * math.cos(theta) new_mps.y += exp_rate * math.sin(theta) new_mps.path.append(Motion_plan_state(new_mps.x, new_mps.y)) d, _ = self.get_distance_angle(new_mps, self.goal) if d <= dist_to_end: new_mps.path.append(self.goal) new_mps.path[0] = mps return new_mps def generate_final_course(self, mps): path = [mps] mps = mps while mps.parent is not None: reversed_path = reversed(mps.path) for point in reversed_path: path.append(point) mps = mps.parent #path.append(mps) return path def get_random_mps(self, size_max=15): x_max = max([mps.x for mps in self.boundary]) x_min = min([mps.x for mps in self.boundary]) y_max = max([mps.y for mps in self.boundary]) y_min = min([mps.y for mps in self.boundary]) ran_x = random.uniform(x_min, x_max) ran_y = random.uniform(y_min, y_max) ran_theta = random.uniform(-math.pi, math.pi) ran_size = random.uniform(0, size_max) mps = Motion_plan_state(ran_x, ran_y, theta=ran_theta, size=ran_size) #ran_z = random.uniform(self.min_area.z, self.max_area.z) return mps def draw_graph_replan(self, traj_path, rnd=None): fig = plt.figure(1, figsize=(10, 30)) x, y = self.boundary_poly.exterior.xy row = math.ceil(len(list(traj_path[1].keys())) / 3) for index, arr in traj_path[1].items(): ax = fig.add_subplot(row, 3, index) ax.plot(x, y, color="black") # for mps in self.mps_list: # if mps.parent: # plt.plot([point.x for point in mps.path], [point.y for point in mps.path], '-g') # plot obstacels as circles for obs in self.obstacle_list: ax.add_patch( plt.Circle((obs.x, obs.y), obs.size, color='#000000', fill=False)) for habitat in arr[1]: ax.add_patch( plt.Circle((habitat.x, habitat.y), habitat.size, color='b', fill=False)) # patch = [] # occ = [] # for cell in self.cell_list: # polygon = patches.Polygon(list(cell.exterior.coords), True) # patch.append(polygon) # occ.append(self.sharkGrid[time_tuple][cell.bounds]) # p = collections.PatchCollection(patch) # p.set_cmap("Greys") # p.set_array(np.array(occ)) # ax.add_collection(p) # fig.colorbar(p, ax=ax) ax.set_xlim([ self.boundary_poly.bounds[0] - 10, self.boundary_poly.bounds[2] + 10 ]) ax.set_ylim([ self.boundary_poly.bounds[1] - 10, self.boundary_poly.bounds[3] + 10 ]) # ax.title.set_text(str(list(self.sharkGrid.keys())[i])) ax.plot([mps.x for mps in traj_path[0]], [mps.y for mps in traj_path[0]], "r") ax.plot([mps.x for mps in arr[0]], [mps.y for mps in arr[0]], 'b') plt.show() def draw_graph_explore(self, habitats, traj_path, rnd=None): fig = plt.figure(1, figsize=(10, 8)) x, y = self.boundary_poly.exterior.xy for i in range(len(list(self.sharkGrid.keys()))): ax = fig.add_subplot(5, 2, i + 1) ax.plot(x, y, color="black") # for mps in self.mps_list: # if mps.parent: # plt.plot([point.x for point in mps.path], [point.y for point in mps.path], '-g') # plot obstacels as circles for obs in self.obstacle_list: ax.add_patch( plt.Circle((obs.x, obs.y), obs.size, color='#000000', fill=False)) for habitat in habitats: ax.add_patch( plt.Circle((habitat.x, habitat.y), habitat.size, color='b', fill=False)) patch = [] occ = [] key = list(self.sharkGrid.keys())[i] for cell in self.cell_list: polygon = patches.Polygon(list(cell.exterior.coords), True) patch.append(polygon) occ.append(self.sharkGrid[key][cell.bounds]) p = collections.PatchCollection(patch) p.set_cmap("Greys") p.set_array(np.array(occ)) ax.add_collection(p) fig.colorbar(p, ax=ax) ax.set_xlim([ self.boundary_poly.bounds[0] - 10, self.boundary_poly.bounds[2] + 10 ]) ax.set_ylim([ self.boundary_poly.bounds[1] - 10, self.boundary_poly.bounds[3] + 10 ]) ax.title.set_text(str(list(self.sharkGrid.keys())[i])) ax.plot([mps.x for mps in traj_path[0]], [mps.y for mps in traj_path[0]], "r") ax.plot([mps.x for mps in traj_path[1][key]], [mps.y for mps in traj_path[1][key]], 'b') plt.show() @staticmethod def plot_circle(x, y, size, color="-b"): # pragma: no cover deg = list(range(0, 360, 5)) deg.append(0) xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] plt.plot(xl, yl, color) def connect_to_goal_curve(self, mps1): a = (self.goal.y - mps1.y) / (np.cosh(self.goal.x) - np.cosh(mps1.x)) b = mps1.y - a * np.cosh(mps1.x) x = np.linspace(mps1.x, self.goal.x, 15) x = x.tolist() y = a * np.cosh(x) + b y = y.tolist() new_mps = Motion_plan_state(x[-2], y[-2]) new_mps.path.append(mps1) for i in range(1, len(x)): new_mps.path.append(Motion_plan_state(x[i], y[i])) new_mps.length += math.sqrt( (new_mps.path[i].x - new_mps.path[i - 1].x)**2 + (new_mps.path[i].y - new_mps.path[i - 1].y)**2) '''plt.plot(mps1.x, mps1.y, color) plt.plot(self.goal.x, self.goal.y, color) plt.plot([mps.x for mps in new_mps.path], [mps.y for mps in new_mps.path], color) plt.show()''' return new_mps def connect_to_goal_curve_alt(self, mps, exp_rate): new_mps = Motion_plan_state(mps.x, mps.y, theta=mps.theta, traj_time_stamp=mps.traj_time_stamp) theta_0 = new_mps.theta _, theta = self.get_distance_angle(mps, self.goal) diff = theta - theta_0 diff = self.angle_wrap(diff) if abs(diff) > math.pi / 2: return #polar coordinate r_G = math.hypot(self.goal.x - new_mps.x, self.goal.y - new_mps.y) phi_G = math.atan2(self.goal.y - new_mps.y, self.goal.x - new_mps.x) #arc phi = 2 * self.angle_wrap(phi_G - new_mps.theta) radius = r_G / (2 * math.sin(phi_G - new_mps.theta)) length = radius * phi if phi > math.pi: phi -= 2 * math.pi length = -radius * phi elif phi < -math.pi: phi += 2 * math.pi length = -radius * phi new_mps.length += length ang_vel = phi / (length / exp_rate) #center of rotation x_C = new_mps.x - radius * math.sin(new_mps.theta) y_C = new_mps.y + radius * math.cos(new_mps.theta) n_expand = math.floor(length / exp_rate) for i in range(n_expand + 1): new_mps.x = x_C + radius * math.sin(ang_vel * i + theta_0) new_mps.y = y_C - radius * math.cos(ang_vel * i + theta_0) new_mps.theta = ang_vel * i + theta_0 new_mps.path.append( Motion_plan_state(new_mps.x, new_mps.y, theta=new_mps.theta, plan_time_stamp=time.time() - self.t_start)) return new_mps def angle_wrap(self, ang): if -math.pi <= ang <= math.pi: return ang elif ang > math.pi: ang += (-2 * math.pi) return self.angle_wrap(ang) elif ang < -math.pi: ang += (2 * math.pi) return self.angle_wrap(ang) def get_closest_mps(self, ran_mps, mps_list): min_dist, _ = self.get_distance_angle(mps_list[0], ran_mps) closest_mps = mps_list[0] for mps in mps_list: dist, _ = self.get_distance_angle(mps, ran_mps) if dist < min_dist: min_dist = dist closest_mps = mps return closest_mps def get_closest_mps_time(self, ran_time, mps_list): while len(mps_list) > 3: left = mps_list[len(mps_list) // 2 - 1] left_diff = abs(left.plan_time_stamp - ran_time) right = mps_list[len(mps_list) // 2 + 1] right_diff = abs(right.plan_time_stamp - ran_time) index = len(mps_list) // 2 if left_diff >= right_diff: mps_list = mps_list[index:] else: mps_list = mps_list[:index] return mps_list[0] def check_collision(self, mps, obstacleList): if mps is None: return False dList = [] for obstacle in obstacleList: for point in mps.path: d, _ = self.get_distance_angle(obstacle, point) dList.append(d) if min(dList) <= obstacle.size: return False # collision for point in mps.path: point = Point(point.x, point.y) if not point.within(self.boundary_poly): return False return True # safe def check_collision_obstacle(self, mps, obstacleList): for obstacle in obstacleList: d, _ = self.get_distance_angle(obstacle, mps) if d <= obstacle.size: return False return True def get_distance_angle(self, start_mps, end_mps): dx = end_mps.x - start_mps.x dy = end_mps.y - start_mps.y #dz = end_mps.z-start_mps.z dist = math.sqrt(dx**2 + dy**2) theta = math.atan2(dy, dx) return dist, theta def cal_length(self, path): length = 0 for i in range(1, len(path)): length += math.sqrt((path[i].x - path[i - 1].x)**2 + (path[i].y - path[i - 1].y)**2) return length def cal_boundary_peri(self): peri = 0 for i in range(len(self.boundary) - 1): dist, _ = self.get_distance_angle(self.boundary[i], self.boundary[i + 1]) peri += dist return peri def plot_performance(self, time_list, perf_list): _, ax = plt.subplots() ax.plot(time_list, perf_list) # Add some text for labels, title and custom x-axis tick labels, etc. ax.set_ylabel('optimal sum cost') ax.set_title('RRT performance') ax.legend() plt.show() def splitPath(self, path, shark_interval, traj_time): n_expand = math.floor(traj_time[1] / shark_interval) res = {} start = traj_time[0] for i in range(n_expand): res[(start + i * shark_interval, start + (i + 1) * shark_interval)] = [] for point in path: for time, arr in res.items(): if point.traj_time_stamp >= time[ 0] and point.traj_time_stamp <= time[1]: arr.append(point) break return res def removeHabitat(self, habitats, path): for point in path: for habitat in habitats: if math.sqrt((point.x - habitat.x)**2 + (point.y - habitat.y)**2) <= habitat.size: habitats.remove(habitat) break return habitats