├── BIT_star.py ├── README.md └── RRT_and_Pruning ├── .gitignore ├── RRTs.py ├── bugtrap.yaml ├── drawer.py ├── environment.py ├── main.py └── pruning.png /BIT_star.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import yaml 4 | import heapq 5 | import time 6 | import matplotlib.pyplot as plt 7 | from matplotlib.patches import Ellipse 8 | from shapely.geometry import Point, LineString, Polygon 9 | from descartes import PolygonPatch 10 | from shapely import affinity 11 | import itertools 12 | 13 | INF = float("inf") 14 | 15 | class Environment: 16 | def __init__(self, yaml_file=None, bounds=None): 17 | self.yaml_file = yaml_file 18 | self.obstacles = [] 19 | # self.obstacles_map = {} 20 | self.bounds = bounds 21 | if yaml_file: 22 | self.load_from_yaml_file(yaml_file) 23 | 24 | def bounds(self): 25 | return self.bounds 26 | 27 | def add_obstacles(self, obstacles): 28 | self.obstacles = self.obstacles + obstacles 29 | 30 | def load_from_yaml_file(self, yaml_file): 31 | with open(yaml_file) as f: 32 | data = yaml.safe_load(f) 33 | return self.parse_yaml_data(data) 34 | 35 | def parse_yaml_data(self, data): 36 | if 'environment' in data: 37 | env = data['environment'] 38 | self.parse_yaml_obstacles(env['obstacles']) 39 | 40 | def parse_yaml_obstacles(self, obstacles): 41 | # only parse rectangle and polygon obstacles 42 | for name, description in obstacles.items(): 43 | # Double underscore not allowed in region names. 44 | if name.find("__") != -1: 45 | raise Exception("Names cannot contain double underscores.") 46 | if description['shape'] == 'rectangle': 47 | parsed = self.parse_rectangle(name, description) 48 | elif description['shape'] == 'polygon': 49 | parsed = self.parse_polygon(name, description) 50 | else: 51 | raise Exception("not a rectangle or polygon") 52 | if not parsed.is_valid: 53 | raise Exception("%s is not valid!"%name) 54 | self.obstacles.append(parsed) 55 | # self.obstacles_map[name] = parsed 56 | 57 | def parse_rectangle(self, name, description): 58 | center = description['center'] 59 | center = Point((center[0], center[1])) 60 | length = description['length'] 61 | width = description['width'] 62 | # convert rotation to radians 63 | rotation = description['rotation']# * math.pi/180 64 | # figure out the four corners. 65 | corners = [(center.x - length/2., center.y - width/2.), 66 | (center.x + length/2., center.y - width/2.), 67 | (center.x + length/2., center.y + width/2.), 68 | (center.x - length/2., center.y + width/2.)] 69 | # print corners 70 | polygon = Polygon(corners) 71 | out = affinity.rotate(polygon, rotation, origin=center) 72 | out.name = name 73 | out.cc_length = length 74 | out.cc_width = width 75 | out.cc_rotation = rotation 76 | return out 77 | 78 | def parse_polygon(self, name, description): 79 | _points = description['corners'] 80 | for points in itertools.permutations(_points): 81 | polygon = Polygon(points) 82 | polygon.name = name 83 | if polygon.is_valid: 84 | return polygon 85 | 86 | 87 | class Drawer: 88 | def __init__(self, env): 89 | self.env = env 90 | 91 | def plot_environment(self): 92 | minx, miny, maxx, maxy = self.env.bounds 93 | 94 | max_width, max_height = 5, 5 95 | figsize = (max_width, max_height) 96 | f = plt.figure(figsize=figsize) 97 | ax = f.add_subplot(111) 98 | for i, obs in enumerate(self.env.obstacles): 99 | patch = PolygonPatch(obs, fc='blue', ec='blue', alpha=0.5, zorder=20) 100 | ax.add_patch(patch) 101 | 102 | plt.xlim([minx, maxx]) 103 | plt.ylim([miny, maxy]) 104 | ax.set_aspect('equal', adjustable='box') 105 | # Cancel coordinate axis display 106 | ax.set_xticks([]) 107 | ax.set_yticks([]) 108 | return ax 109 | 110 | def plot_many_points(self, env_plot, point_array, pointSize=1, colorselect='gray'): 111 | x = [point[0] for point in point_array] 112 | y = [point[1] for point in point_array] 113 | env_plot.plot(x, y,'o',markersize=pointSize,color=colorselect) 114 | 115 | def plot_many_edges(self, env_plot, edge_array, color='gray', lineWidthSelect=0.8): 116 | for edge in edge_array: 117 | x = [point[0] for point in edge] 118 | y = [point[1] for point in edge] 119 | env_plot.plot(x, y, color=color, linewidth=lineWidthSelect, solid_capstyle='butt', zorder=1) 120 | 121 | def plot_path(self, env_plot, path, color="red"): 122 | if path: 123 | x = [point[0] for point in path] 124 | y = [point[1] for point in path] 125 | env_plot.plot(x, y,color=color,linewidth=2) 126 | 127 | def plot_ellipse(self, env_plot, c_best, start, goal, color="blue"): 128 | if c_best!=INF: 129 | center_xy = ((start[0]+goal[0])/2, (start[0]+goal[0])/2) 130 | two_c = math.sqrt((start[0]-goal[0])**2 + (start[1]-goal[1])**2) 131 | two_b=math.sqrt(c_best**2 - two_c**2) 132 | angle=math.atan2(start[1]-goal[1],start[0]-goal[0])/math.pi*180 133 | 134 | ellipse = Ellipse(xy=center_xy, width=c_best, height=two_b, angle=angle) 135 | 136 | env_plot.add_artist(ellipse) 137 | ellipse.set_fill(0) 138 | ellipse.set_edgecolor(color) 139 | ellipse.set_linestyle('--') 140 | ellipse.set_linewidth(1.5) 141 | 142 | def plot_iteration(self, cost_set, label_set): 143 | for i in range(len(cost_set)): 144 | X = [item[0] for item in cost_set[i]] 145 | Y = [item[-1] for item in cost_set[i]] 146 | plt.plot(X,Y, label=label_set[i]) 147 | plt.legend(loc='upper right') 148 | plt.xlabel("Number of iterations") 149 | plt.ylabel("Path Cost") 150 | plt.show() 151 | 152 | 153 | class BITStar: 154 | def __init__(self, environment, start, goal, bounds, maxIter=50000, plot_flag=True): 155 | self.env = environment 156 | self.obstacles = environment.obstacles 157 | 158 | self.draw = Drawer(self.env) 159 | 160 | self.start = start 161 | self.goal = goal 162 | 163 | self.bounds = bounds 164 | self.minx, self.miny, self.maxx, self.maxy = bounds 165 | self.dimension = 2 166 | 167 | # This is the tree 168 | self.vertices = [] 169 | self.edges = dict() # key = point，value = parent 170 | self.g_scores = dict() 171 | 172 | self.samples = [] 173 | self.vertex_queue = [] 174 | self.edge_queue = [] 175 | self.old_vertices = set() 176 | 177 | self.maxIter = maxIter 178 | self.r = INF 179 | self.batch_size = 200 180 | self.eta = 1.1 # tunable parameter 181 | self.obj_radius = 1 182 | self.resolution = 3 183 | 184 | # the parameters for informed sampling 185 | self.c_min = self.distance(self.start, self.goal) 186 | self.center_point = None 187 | self.C = None 188 | 189 | # whether plot the middle planning process 190 | self.plot_planning_process = plot_flag 191 | 192 | def setup_planning(self): 193 | # add goal to the samples 194 | self.samples.append(self.goal) 195 | self.g_scores[self.goal] = INF 196 | 197 | # add start to the tree 198 | self.vertices.append(self.start) 199 | self.g_scores[self.start] = 0 200 | 201 | # Computing the sampling space 202 | self.informed_sample_init() 203 | radius_constant = self.radius_init() 204 | 205 | return radius_constant 206 | 207 | def radius_init(self): 208 | # Hypersphere radius calculation 209 | unit_ball_volume = math.pi 210 | n = self.dimension 211 | gamma = (1.0 + 1.0/n) * (self.maxx - self.minx) * (self.maxy - self.miny)/unit_ball_volume 212 | radius_constant = 2 * self.eta * (gamma**(1.0/n)) 213 | return radius_constant 214 | 215 | def informed_sample_init(self): 216 | self.center_point = np.matrix([[(self.start[0] + self.goal[0]) / 2.0],[(self.start[1] + self.goal[1]) / 2.0], [0]]) 217 | a_1 = np.matrix([[(self.goal[0] - self.start[0]) / self.c_min],[(self.goal[1] - self.start[1]) / self.c_min], [0]]) 218 | id1_t = np.matrix([1.0,0,0]) 219 | M = np.dot(a_1, id1_t) 220 | U,S,Vh = np.linalg.svd(M, 1, 1) 221 | self.C = np.dot(np.dot(U, np.diag([1.0,1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) 222 | 223 | def sample_unit_ball(self): 224 | a = np.random.random() 225 | b = np.random.random() 226 | if b < a: 227 | a,b=b,a 228 | sample = (b*math.cos(2*math.pi*a/b), b*math.sin(2*math.pi*a/b)) 229 | return np.array([[sample[0]], [sample[1]], [0]]) 230 | 231 | def informed_sample(self, c_best, sample_num): 232 | if c_best < float('inf'): 233 | c_b = math.sqrt(c_best**2 - self.c_min**2)/2.0 234 | r= [c_best /2.0, c_b, c_b] 235 | L = np.diag(r) 236 | sample_array = [] 237 | cur_num = 0 238 | while cur_num < sample_num: 239 | if c_best < float('inf'): 240 | x_ball = self.sample_unit_ball() 241 | random_point = np.dot(np.dot(self.C,L), x_ball) + self.center_point 242 | random_point = (random_point[(0,0)], random_point[(1,0)]) 243 | if not self.is_point_free(random_point): 244 | continue 245 | else: 246 | random_point = self.get_collision_free_random_point() 247 | cur_num += 1 248 | sample_array.append(random_point) 249 | return sample_array 250 | 251 | def get_collision_free_random_point(self): 252 | # Run until a valid point is found 253 | while True: 254 | point = self.get_random_point() 255 | if self.is_point_free(point): 256 | return point 257 | 258 | def get_random_point(self): 259 | x = self.minx + np.random.random() * (self.maxx - self.minx) 260 | y = self.miny + np.random.random() * (self.maxy - self.miny) 261 | return (x, y) 262 | 263 | def is_point_free(self, point): 264 | buffered_point = Point(point).buffer(self.obj_radius+1, self.resolution) 265 | for obstacle in self.obstacles: 266 | if obstacle.intersects(buffered_point): 267 | return False 268 | return True 269 | 270 | def is_edge_free(self, edge): 271 | line = LineString(edge) 272 | expanded_line = line.buffer(self.obj_radius, self.resolution) 273 | for obstacle in self.obstacles: 274 | if expanded_line.intersects(obstacle): 275 | return False 276 | return True 277 | 278 | def get_g_score(self, point): 279 | # gT(x) 280 | if point == self.start: 281 | return 0 282 | if point not in self.edges: 283 | return INF 284 | else: 285 | return self.g_scores.get(point) 286 | 287 | def get_f_score(self, point): 288 | # f^(x) 289 | return self.heuristic_cost(self.start,point) + self.heuristic_cost(point, self.goal) 290 | 291 | def actual_edge_cost(self, point1, point2): 292 | # c(x1,x2) 293 | if not self.is_edge_free([point1,point2]): 294 | return INF 295 | return self.distance(point1, point2) 296 | 297 | def heuristic_cost(self, point1, point2): 298 | # Euler distance as the heuristic distance 299 | return self.distance(point1, point2) 300 | 301 | def distance(self,point1, point2): 302 | return math.sqrt((point1[0]-point2[0])**2 + (point1[1]-point2[1])**2) 303 | 304 | def get_edge_value(self,edge): 305 | # sort value for edge 306 | return self.get_g_score(edge[0])+self.heuristic_cost(edge[0],edge[1])+self.heuristic_cost(edge[1],self.goal) 307 | 308 | def get_point_value(self,point): 309 | # sort value for point 310 | return self.get_g_score(point)+self.heuristic_cost(point,self.goal) 311 | 312 | def bestVertexQueueValue(self): 313 | if not self.vertex_queue: 314 | return INF 315 | else: 316 | return self.vertex_queue[0][0] 317 | 318 | def bestEdgeQueueValue(self): 319 | if not self.edge_queue: 320 | return INF 321 | else: 322 | return self.edge_queue[0][0] 323 | 324 | def prune_edge(self, c_best): 325 | edge_array = list(self.edges.items()) 326 | for point, parent in edge_array: 327 | if self.get_f_score(point) > c_best or self.get_f_score(parent) > c_best: 328 | self.edges.pop(point) 329 | 330 | def prune(self, c_best): 331 | self.samples = [point for point in self.samples if self.get_f_score(point) < c_best ] 332 | self.prune_edge(c_best) 333 | vertices_temp = [] 334 | for point in self.vertices: 335 | if self.get_f_score(point) <= c_best: 336 | if self.get_g_score(point)==INF: 337 | self.samples.append(point) 338 | else: 339 | vertices_temp.append(point) 340 | self.vertices = vertices_temp 341 | 342 | def expand_vertex(self, point): 343 | # get the nearest value in vertex for every one in samples where difference is less than the radius 344 | neigbors_sample = [] 345 | for sample in self.samples: 346 | if self.distance(point,sample) <= self.r: 347 | neigbors_sample.append(sample) 348 | 349 | # add an edge to the edge queue is the path might improve the solution 350 | for neighbor in neigbors_sample: 351 | estimated_f_score = self.heuristic_cost(self.start, point) + \ 352 | self.heuristic_cost(point, neighbor) + self.heuristic_cost(neighbor, self.goal) 353 | if estimated_f_score < self.g_scores[self.goal]: 354 | heapq.heappush(self.edge_queue,(self.get_edge_value((point,neighbor)),(point,neighbor))) 355 | 356 | # add the vertex to the edge queue 357 | if point not in self.old_vertices: 358 | neigbors_vertex = [] 359 | for ver in self.vertices: 360 | if self.distance(point,ver) <= self.r: 361 | neigbors_vertex.append(ver) 362 | for neighbor in neigbors_vertex: 363 | if neighbor not in self.edges or point != self.edges.get(neighbor): 364 | estimated_f_score = self.heuristic_cost(self.start, point) + \ 365 | self.heuristic_cost(point, neighbor) + self.heuristic_cost(neighbor, self.goal) 366 | if estimated_f_score < self.g_scores[self.goal]: 367 | estimated_g_score = self.get_g_score(point) + self.heuristic_cost(point, neighbor) 368 | if estimated_g_score < self.get_g_score(neighbor): 369 | heapq.heappush(self.edge_queue,(self.get_edge_value((point,neighbor)),(point,neighbor))) 370 | 371 | def get_best_path(self): 372 | path = [] 373 | if self.g_scores[self.goal] != INF: 374 | path.append(self.goal) 375 | point = self.goal 376 | while point != self.start: 377 | point = self.edges[point] 378 | path.append(point) 379 | path.reverse() 380 | return path 381 | 382 | def path_length_calculate(self, path): 383 | path_length=0 384 | for i in range(len(path)-1): 385 | path_length += self.distance(path[i], path[i+1]) 386 | return path_length 387 | 388 | def plot_function(self, path): 389 | env_plot = self.draw.plot_environment() 390 | self.draw.plot_many_points(env_plot, self.samples) 391 | self.draw.plot_many_points(env_plot, [self.start], pointSize=6, colorselect="green") 392 | self.draw.plot_many_points(env_plot, [self.goal], pointSize=6, colorselect="red") 393 | self.draw.plot_many_edges(env_plot, list(self.edges.items())) 394 | self.draw.plot_ellipse(env_plot, self.g_scores[self.goal], self.start, self.goal, color="black") 395 | self.draw.plot_path(env_plot, path) # self.get_best_path() 396 | # plt.show() 397 | 398 | def plan(self, pathLengthLimit): 399 | radius_constant = self.setup_planning() 400 | path = [] 401 | for i in range(self.maxIter): 402 | if not self.vertex_queue and not self.edge_queue: 403 | c_best = self.g_scores[self.goal] 404 | path = self.get_best_path() 405 | self.prune(c_best) 406 | self.samples.extend(self.informed_sample(c_best, self.batch_size)) 407 | 408 | self.old_vertices = set(self.vertices) 409 | self.vertex_queue = [(self.get_point_value(point),point) for point in self.vertices] 410 | heapq.heapify(self.vertex_queue) # change to op priority queue 411 | q = len(self.vertices)+len(self.samples) 412 | self.r = radius_constant * ((math.log(q) / q) ** (1.0/self.dimension)) 413 | 414 | while self.bestVertexQueueValue() <= self.bestEdgeQueueValue(): 415 | _, point = heapq.heappop(self.vertex_queue) 416 | self.expand_vertex(point) 417 | 418 | best_edge_value, bestEdge = heapq.heappop(self.edge_queue) 419 | 420 | # Check if this can improve the current solution 421 | if best_edge_value < self.g_scores[self.goal]: 422 | actual_cost_of_edge = self.actual_edge_cost(bestEdge[0], bestEdge[1]) 423 | actual_f_edge = self.heuristic_cost(self.start, bestEdge[0]) + actual_cost_of_edge + self.heuristic_cost(bestEdge[1], self.goal) 424 | if actual_f_edge < self.g_scores[self.goal]: 425 | actual_g_score_of_point = self.get_g_score(bestEdge[0]) + actual_cost_of_edge 426 | if actual_g_score_of_point < self.get_g_score(bestEdge[1]): 427 | self.g_scores[bestEdge[1]] = actual_g_score_of_point 428 | self.edges[bestEdge[1]] = bestEdge[0] 429 | if bestEdge[1] not in self.vertices: 430 | self.samples.remove(bestEdge[1]) 431 | self.vertices.append(bestEdge[1]) 432 | heapq.heappush(self.vertex_queue,(self.get_point_value(bestEdge[1]),bestEdge[1])) 433 | 434 | self.edge_queue = [ item for item in self.edge_queue if item[1][1]!=bestEdge[1] or \ 435 | self.get_g_score(item[1][0]) + self.heuristic_cost(item[1][0],item[1][1])): A list of tuples/coordinates representing the nodes in a path from start to the goal region 68 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 69 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 70 | """ 71 | self.env = environment 72 | 73 | self.initialise(environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations) 74 | 75 | # Define start and goal in terms of coordinates. The goal is the centroid of the goal polygon. 76 | x0, y0 = start_pose 77 | x1, y1 = goal_region.centroid.coords[0] 78 | start = (x0, y0) 79 | goal = (x1, y1) 80 | elapsed_time=0 81 | path=[] 82 | 83 | # Handle edge case where where the start is already at the goal 84 | if start == goal: 85 | path = [start, goal] 86 | self.V.union([start, goal]) 87 | self.E.union([(start, goal)]) 88 | # There might also be a straight path to goal, consider this case before invoking algorithm 89 | elif self.isEdgeCollisionFree(start, goal): 90 | path = [start, goal] 91 | self.V.union([start, goal]) 92 | self.E.union([(start, goal)]) 93 | # Run the appropriate RRT algorithm according to RRT_Flavour 94 | else: 95 | if RRT_Flavour == "RRT": 96 | start_time = time.time() 97 | path = self.RRTSearch() 98 | elapsed_time = time.time() - start_time 99 | 100 | if path and drawResults: 101 | draw_results("RRT", path, self.V, self.E, environment, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time) 102 | 103 | return path 104 | 105 | def RRTSearch(self): 106 | """Returns path using RRT algorithm. 107 | 108 | Builds a tree exploring from the start node until it reaches the goal region. It works by sampling random points in the map and connecting them with 109 | the tree we build off on each iteration of the algorithm. 110 | 111 | Returns: 112 | path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region 113 | self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree 114 | self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree 115 | """ 116 | 117 | # Initialize path and tree to be empty. 118 | path = [] 119 | path_length = float('inf') 120 | tree_size = 0 121 | path_size = 0 122 | self.V.add(self.start_pose) 123 | goal_centroid = self.get_centroid(self.goal_region) 124 | 125 | # Iteratively sample N random points in environment to build tree 126 | for i in range(self.N): 127 | if(random.random()>=1.95): # Change to a value under 1 to bias search towards goal, right now this line doesn't run 128 | random_point = goal_centroid 129 | else: 130 | random_point = self.get_collision_free_random_point() 131 | 132 | # The new point to be added to the tree is not the sampled point, but a colinear point with it and the nearest point in the tree. 133 | # This keeps the branches short 134 | nearest_point = self.find_nearest_point(random_point) 135 | new_point = self.steer(nearest_point, random_point) 136 | 137 | # If there is no obstacle between nearest point and sampled point, add the new point to the tree. 138 | if self.isEdgeCollisionFree(nearest_point, new_point): 139 | self.V.add(new_point) 140 | self.E.add((nearest_point, new_point)) 141 | self.setParent(nearest_point, new_point) 142 | # If new point of the tree is at the goal region, we can find a path in the tree from start node to goal. 143 | if self.isAtGoalRegion(new_point): 144 | if not self.runForFullIterations: # If not running for full iterations, terminate as soon as a path is found. 145 | path, tree_size, path_size, path_length = self.find_path(self.start_pose, new_point) 146 | break 147 | else: # If running for full iterations, we return the shortest path found. 148 | tmp_path, tmp_tree_size, tmp_path_size, tmp_path_length = self.find_path(self.start_pose, new_point) 149 | if tmp_path_length < path_length: 150 | path_length = tmp_path_length 151 | path = tmp_path 152 | tree_size = tmp_tree_size 153 | path_size = tmp_path_size 154 | uniPruningPath=self.uniPruning(path) 155 | # If no path is found, then path would be an empty list. 156 | return [path,uniPruningPath] 157 | 158 | """ 159 | ****************************************************************************************************************************************** 160 | ***************************************************** Helper Functions ******************************************************************* 161 | ****************************************************************************************************************************************** 162 | """ 163 | 164 | def sample(self, c_max, c_min, x_center, C): 165 | if c_max < float('inf'): 166 | r= [c_max /2.0, math.sqrt(c_max**2 - c_min**2)/2.0, math.sqrt(c_max**2 - c_min**2)/2.0] 167 | L = np.diag(r) 168 | x_ball = self.sample_unit_ball() 169 | random_point = np.dot(np.dot(C,L), x_ball) + x_center 170 | random_point = (random_point[(0,0)], random_point[(1,0)]) 171 | else: 172 | random_point = self.get_collision_free_random_point() 173 | return random_point 174 | 175 | def sample_unit_ball(self): 176 | a = random.random() 177 | b = random.random() 178 | 179 | if b < a: 180 | tmp = b 181 | b = a 182 | a = tmp 183 | sample = (b*math.cos(2*math.pi*a/b), b*math.sin(2*math.pi*a/b)) 184 | return np.array([[sample[0]], [sample[1]], [0]]) 185 | 186 | def find_min_point(self, nearest_set, nearest_point, new_point): 187 | min_point = nearest_point 188 | min_cost = self.cost(nearest_point) + self.linecost(nearest_point, new_point) 189 | for vertex in nearest_set: 190 | if self.isEdgeCollisionFree(vertex, new_point): 191 | temp_cost = self.cost(vertex) + self.linecost(vertex, new_point) 192 | if temp_cost < min_cost: 193 | min_point = vertex 194 | min_cost = temp_cost 195 | return min_point 196 | 197 | def cost(self, vertex): 198 | path, tree_size, path_size, path_length = self.find_path(self.start_pose, vertex) 199 | return path_length 200 | 201 | 202 | def linecost(self, point1, point2): 203 | return self.euclidian_dist(point1, point2) 204 | 205 | def getParent(self, vertex): 206 | return self.child_to_parent_dict[vertex] 207 | 208 | def setParent(self, parent, child): 209 | self.child_to_parent_dict[child] = parent 210 | 211 | def get_random_point(self): 212 | x = self.minx + random.random() * (self.maxx - self.minx) 213 | y = self.miny + random.random() * (self.maxy - self.miny) 214 | return (x, y) 215 | 216 | def get_collision_free_random_point(self): 217 | # Run until a valid point is found 218 | while True: 219 | point = self.get_random_point() 220 | # Pick a point, if no obstacle overlaps with a circle centered at point with some obj_radius then return said point. 221 | buffered_point = Point(point).buffer(self.obj_radius, self.resolution) 222 | if self.isPointCollisionFree(buffered_point): 223 | return point 224 | 225 | def isPointCollisionFree(self, point): 226 | for obstacle in self.obstacles: 227 | if obstacle.contains(point): 228 | return False 229 | return True 230 | 231 | def find_nearest_point(self, random_point): 232 | closest_point = None 233 | min_dist = float('inf') 234 | for vertex in self.V: 235 | euc_dist = self.euclidian_dist(random_point, vertex) 236 | if euc_dist < min_dist: 237 | min_dist = euc_dist 238 | closest_point = vertex 239 | return closest_point 240 | 241 | def isOutOfBounds(self, point): 242 | if((point[0] - self.obj_radius) < self.minx): 243 | return True 244 | if((point[1] - self.obj_radius) < self.miny): 245 | return True 246 | if((point[0] + self.obj_radius) > self.maxx): 247 | return True 248 | if((point[1] + self.obj_radius) > self.maxy): 249 | return True 250 | return False 251 | 252 | 253 | def isEdgeCollisionFree(self, point1, point2): 254 | if self.isOutOfBounds(point2): 255 | return False 256 | line = LineString([point1, point2]) 257 | expanded_line = line.buffer(self.obj_radius, self.resolution) 258 | for obstacle in self.obstacles: 259 | if expanded_line.intersects(obstacle): 260 | return False 261 | return True 262 | 263 | def steer(self, from_point, to_point): 264 | fromPoint_buffered = Point(from_point).buffer(self.obj_radius, self.resolution) 265 | toPoint_buffered = Point(to_point).buffer(self.obj_radius, self.resolution) 266 | if fromPoint_buffered.distance(toPoint_buffered) < self.steer_distance: 267 | return to_point 268 | else: 269 | from_x, from_y = from_point 270 | to_x, to_y = to_point 271 | theta = math.atan2(to_y - from_y, to_x- from_x) 272 | new_point = (from_x + self.steer_distance * math.cos(theta), from_y + self.steer_distance * math.sin(theta)) 273 | return new_point 274 | 275 | def isAtGoalRegion(self, point): 276 | buffered_point = Point(point).buffer(self.obj_radius, self.resolution) 277 | intersection = buffered_point.intersection(self.goal_region) 278 | inGoal = intersection.area / buffered_point.area 279 | return inGoal >= 0.5 280 | 281 | def euclidian_dist(self, point1, point2): 282 | return math.sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2) 283 | 284 | def find_path(self, start_point, end_point): 285 | # Returns a path by backtracking through the tree formed by one of the RRT algorithms starting at the end_point until reaching start_node. 286 | path = [end_point] 287 | tree_size, path_size, path_length = len(self.V), 1, 0 288 | current_node = end_point 289 | previous_node = None 290 | target_node = start_point 291 | while current_node != target_node: 292 | parent = self.getParent(current_node) 293 | path.append(parent) 294 | previous_node = current_node 295 | current_node = parent 296 | path_length += self.euclidian_dist(current_node, previous_node) 297 | path_size += 1 298 | path.reverse() 299 | return path, tree_size, path_size, path_length 300 | 301 | def get_centroid(self, region): 302 | centroid = region.centroid.wkt 303 | filtered_vals = centroid[centroid.find("(")+1:centroid.find(")")] 304 | filtered_x = filtered_vals[0:filtered_vals.find(" ")] 305 | filtered_y = filtered_vals[filtered_vals.find(" ") + 1: -1] 306 | (x,y) = (float(filtered_x), float(filtered_y)) 307 | return (x,y) 308 | 309 | def uniPruning(self, path): #Pruning function 310 | unidirectionalPath=[path[0]] 311 | pointTem=path[0] 312 | for i in range(3,len(path)): 313 | if not self.isEdgeCollisionFree(pointTem,path[i]): 314 | pointTem=path[i-1] 315 | unidirectionalPath.append(pointTem) 316 | unidirectionalPath.append(path[-1]) 317 | return unidirectionalPath -------------------------------------------------------------------------------- /RRT_and_Pruning/bugtrap.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | obstacles: 3 | obstacle0000: 4 | corners: 5 | - [0.0, 0.0] 6 | - [0.0, 1.0] 7 | - [4.0, 1.0] 8 | - [4.0, 0.0] 9 | - [0.0, 0.0] 10 | shape: polygon 11 | obstacle0001: 12 | corners: 13 | - [0.0, 4.0] 14 | - [0.0, 5.0] 15 | - [4.0, 5.0] 16 | - [4.0, 4.0] 17 | - [0.0, 4.0] 18 | shape: polygon 19 | obstacle0002: 20 | corners: 21 | - [3.0, 1.0] 22 | - [3.0, 4.0] 23 | - [4.0, 4.0] 24 | - [4.0, 1.0] 25 | - [3.0, 1.0] 26 | shape: polygon 27 | obstacle0003: 28 | corners: 29 | - [7.0, 3.0] 30 | - [7.0, 8.0] 31 | - [8.0, 8.0] 32 | - [8.0, 3.0] 33 | - [7.0, 3.0] 34 | shape: polygon 35 | obstacle0004: 36 | corners: 37 | - [7.0, -3.0] 38 | - [7.0, 2.0] 39 | - [8.0, 2.0] 40 | - [8.0, -3.0] 41 | - [7.0, -3.0] 42 | shape: polygon 43 | -------------------------------------------------------------------------------- /RRT_and_Pruning/drawer.py: -------------------------------------------------------------------------------- 1 | from shapely.geometry import Point, Polygon, LineString, box 2 | from environment import Environment, plot_environment, plot_line, plot_poly 3 | from math import sqrt 4 | 5 | def draw_results(algo_name, path, V, E, env, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time): 6 | """ 7 | Plots the path from start node to goal region as well as the graph (or tree) searched with the Sampling Based Algorithms. 8 | 9 | Args: 10 | algo_name (str): The name of the algorithm used (used as title of the plot) 11 | path (list<(float,float), (float,float)>): The sequence of coordinates traveled to reach goal from start node 12 | V (set<(float, float)>): All nodes in the explored graph/tree 13 | E (set<(float,float), (float, float)>): The set of all edges considered in the graph/tree 14 | env (yaml environment): 2D yaml environment for the path planning to take place 15 | bounds (int, int int int): min x, min y, max x, max y of the coordinates in the environment. 16 | object_radius (float): radius of our object. 17 | resolution (int): Number of segments used to approximate a quarter circle around a point. 18 | start_pose(float,float): Coordinates of initial point of the path. 19 | goal_region (Polygon): A polygon object representing the end goal. 20 | elapsed_time (float): Time it took for the algorithm to run 21 | 22 | Return: 23 | None 24 | 25 | Action: 26 | Plots a path using the environment module. 27 | """ 28 | originalPath,pruningPath=path 29 | graph_size = len(V) 30 | path_size = len(originalPath) 31 | # Calculate path length 32 | path_length1 = 0.0 33 | path_length2 = 0.0 34 | for i in range(len(originalPath)-1): 35 | path_length1 += euclidian_dist(originalPath[i], originalPath[i+1]) 36 | for i in range(len(pruningPath)-1): 37 | path_length2 += euclidian_dist(pruningPath[i], pruningPath[i+1]) 38 | 39 | # Create title with descriptive information based on environment, path length, and elapsed_time 40 | title = algo_name + "\n" + str(graph_size) + " Nodes. " + str(len(env.obstacles)) + " Obstacles. Path Size: " + str(path_size) + "\n Path Length: " + str([path_length1,path_length2]) + "\n Runtime(s)= " + str(elapsed_time) 41 | 42 | # Plot environment 43 | env_plot = plot_environment(env, bounds) 44 | # Add title 45 | env_plot.set_title(title) 46 | # Plot goal 47 | plot_poly(env_plot, goal_region, 'green') 48 | # Plot start 49 | buffered_start_vertex = Point(start_pose).buffer(object_radius, resolution) 50 | plot_poly(env_plot, buffered_start_vertex, 'red') 51 | 52 | # Plot Edges explored by ploting lines between each edge 53 | for edge in E: 54 | line = LineString([edge[0], edge[1]]) 55 | plot_line(env_plot, line) 56 | 57 | # Plot path 58 | plot_path(env_plot, originalPath, object_radius,'black') 59 | plot_path(env_plot, pruningPath, object_radius,'red') 60 | 61 | 62 | def euclidian_dist(point1, point2): 63 | return sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2) 64 | 65 | def plot_path(env_plot, path, object_radius,colorset): 66 | # Plots path by taking an enviroment plot and ploting in red the edges that form part of the path 67 | line = LineString(path) 68 | x, y = line.xy 69 | env_plot.plot(x, y, color=colorset, linewidth=3, solid_capstyle='round', zorder=1) 70 | -------------------------------------------------------------------------------- /RRT_and_Pruning/environment.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import math 3 | 4 | import shapely.geometry as geom 5 | from shapely import affinity 6 | import itertools 7 | from matplotlib import pyplot as plt 8 | from descartes import PolygonPatch 9 | import random 10 | 11 | 12 | def plot_environment(env, bounds=None, figsize=None): 13 | if bounds is None and env.bounds: 14 | minx, miny, maxx, maxy = env.bounds 15 | elif bounds: 16 | minx, miny, maxx, maxy = bounds 17 | else: 18 | minx, miny, maxx, maxy = (-10,-5,10,5) 19 | 20 | max_width, max_height = 12, 5.5 21 | if figsize is None: 22 | width, height = max_width, (maxy-miny)*max_width/(maxx-minx) 23 | if height > 5: 24 | width, height = (maxx-minx)*max_height/(maxy-miny), max_height 25 | figsize = (width, height) 26 | #print(figsize) 27 | f = plt.figure(figsize=figsize) 28 | # f.hold('on') 29 | ax = f.add_subplot(111) 30 | for i, obs in enumerate(env.obstacles): 31 | patch = PolygonPatch(obs, fc='blue', ec='blue', alpha=0.5, zorder=20) 32 | ax.add_patch(patch) 33 | 34 | plt.xlim([minx, maxx]) 35 | plt.ylim([miny, maxy]) 36 | ax.set_aspect('equal', adjustable='box') 37 | return ax 38 | 39 | def plot_line(ax, line): 40 | x, y = line.xy 41 | ax.plot(x, y, color='gray', linewidth=1, solid_capstyle='butt', zorder=1) 42 | 43 | 44 | def plot_poly(ax, poly, color, alpha=1.0, zorder=1): 45 | patch = PolygonPatch(poly, fc=color, ec="black", alpha=alpha, zorder=zorder) 46 | ax.add_patch(patch) 47 | 48 | class Environment: 49 | def __init__(self, yaml_file=None, bounds=None): 50 | self.yaml_file = yaml_file 51 | self.environment_loaded = False 52 | self.obstacles = [] 53 | self.obstacles_map = {} 54 | self.bounds = bounds 55 | if not yaml_file is None: 56 | if self.load_from_yaml_file(yaml_file): 57 | if bounds is None: 58 | self.calculate_scene_dimensions() 59 | self.environment_loaded = True 60 | 61 | #@property 62 | def bounds(self): 63 | return self.bounds 64 | 65 | def add_obstacles(self, obstacles): 66 | self.obstacles = self.obstacles + obstacles 67 | self.calculate_scene_dimensions() 68 | 69 | def calculate_scene_dimensions(self): 70 | """Compute scene bounds from obstacles.""" 71 | points = [] 72 | for elem in self.obstacles: 73 | points = points + list(elem.boundary.coords) 74 | 75 | mp = geom.MultiPoint(points) 76 | self.bounds = mp.bounds 77 | 78 | def load_from_yaml_file(self, yaml_file): 79 | f = open(yaml_file) 80 | self.data = yaml.safe_load(f) 81 | f.close() 82 | return self.parse_yaml_data(self.data) 83 | 84 | def parse_yaml_data(self, data): 85 | if 'environment' in data: 86 | env = data['environment'] 87 | self.parse_yaml_obstacles(env['obstacles']) 88 | # self.parse_yaml_features(env['features']) 89 | return True 90 | else: 91 | return False 92 | 93 | def parse_yaml_obstacles(self, obstacles): 94 | self.obstacles = [] 95 | self.obstacles_map = {} 96 | for name, description in obstacles.items(): 97 | # Double underscore not allowed in region names. 98 | if name.find("__") != -1: 99 | raise Exception("Names cannot contain double underscores.") 100 | if description['shape'] == 'rectangle': 101 | parsed = self.parse_rectangle(name, description) 102 | elif description['shape'] == 'polygon': 103 | parsed = self.parse_polygon(name, description) 104 | else: 105 | raise Exception("not a rectangle") 106 | if not parsed.is_valid: 107 | raise Exception("%s is not valid!"%name) 108 | self.obstacles.append(parsed) 109 | self.obstacles_map[name] = parsed 110 | 111 | self.expanded_obstacles = [obs.buffer(0.75/2, resolution=2) for obs in self.obstacles] 112 | 113 | 114 | def parse_rectangle(self, name, description): 115 | center = description['center'] 116 | center = geom.Point((center[0], center[1])) 117 | length = description['length'] 118 | width = description['width'] 119 | # convert rotation to radians 120 | rotation = description['rotation']# * math.pi/180 121 | # figure out the four corners. 122 | corners = [(center.x - length/2., center.y - width/2.), 123 | (center.x + length/2., center.y - width/2.), 124 | (center.x + length/2., center.y + width/2.), 125 | (center.x - length/2., center.y + width/2.)] 126 | # print corners 127 | polygon = geom.Polygon(corners) 128 | out = affinity.rotate(polygon, rotation, origin=center) 129 | out.name = name 130 | out.cc_length = length 131 | out.cc_width = width 132 | out.cc_rotation = rotation 133 | return out 134 | 135 | def parse_polygon(self, name, description): 136 | _points = description['corners'] 137 | for points in itertools.permutations(_points): 138 | polygon = geom.Polygon(points) 139 | polygon.name = name 140 | if polygon.is_valid: 141 | return polygon 142 | 143 | def save_to_yaml(self, yaml_file,N,start_pose,goal_region): 144 | yaml_dict = {} 145 | obstacles = {} 146 | rand_obstacles=self.rand_obstacles_creat(N,start_pose,goal_region) 147 | for i, ob in enumerate(rand_obstacles): 148 | ob_dict = {} 149 | ob_dict['shape'] = 'polygon' 150 | ob_dict['corners'] = [list(t) for t in list(ob.boundary.coords)] 151 | ob_name = "obstacle%.4d"%i 152 | obstacles[ob_name] = ob_dict 153 | yaml_dict['environment'] = {'obstacles' : obstacles} 154 | 155 | f = open(yaml_file, 'w') 156 | f.write(yaml.dump(yaml_dict, default_flow_style=None)) 157 | f.close() 158 | 159 | -------------------------------------------------------------------------------- /RRT_and_Pruning/main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Parts of the code in this file are from https://github.com/yrouben/Sampling-Based-Path-Planning-Library. Thanks for the coder yrouben. 3 | """ 4 | 5 | from __future__ import division 6 | from shapely.geometry import Polygon 7 | from environment import Environment 8 | from RRTs import RRTPlanner 9 | from matplotlib import pyplot as plt 10 | 11 | 12 | environment = Environment('bugtrap.yaml') 13 | bounds = (-2, -3, 12, 8) 14 | start_pose = (2, 2.5) 15 | goal_region = Polygon([(10,5), (10,6), (11,6), (11,5)]) 16 | object_radius = 0.3 17 | steer_distance = 0.3 18 | num_iterations = 10000 19 | resolution = 3 20 | drawResults = True 21 | runForFullIterations = False 22 | 23 | 24 | sbpp = RRTPlanner() 25 | path= sbpp.RRT(environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, drawResults, runForFullIterations) 26 | plt.show() 27 | 28 | -------------------------------------------------------------------------------- /RRT_and_Pruning/pruning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hichenway/sampling-based-path-planning/d882aeb85db5d997bf3efc15097a1aec85f0ccbe/RRT_and_Pruning/pruning.png --------------------------------------------------------------------------------