From 2b316502e336de102888cb62cd0ae39769b191b3 Mon Sep 17 00:00:00 2001 From: Rafael Rojas Date: Thu, 1 Oct 2020 13:15:52 +0200 Subject: [PATCH] Bug RRT* fix, issues #382 and #383 (#401) --- PathPlanning/RRT/rrt.py | 55 +++++++------ PathPlanning/RRTStar/rrt_star.py | 132 +++++++++++++++++++++++-------- 2 files changed, 131 insertions(+), 56 deletions(-) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 4acea0c1..834fb24e 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -32,8 +32,15 @@ class RRT: self.path_y = [] self.parent = None - def __init__(self, start, goal, obstacle_list, rand_area, - expand_dis=3.0, path_resolution=0.5, goal_sample_rate=5, max_iter=500): + def __init__(self, + start, + goal, + obstacle_list, + rand_area, + expand_dis=3.0, + path_resolution=0.5, + goal_sample_rate=5, + max_iter=500): """ Setting Parameter @@ -75,8 +82,10 @@ class RRT: if animation and i % 5 == 0: self.draw_graph(rnd_node) - if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis: - final_node = self.steer(self.node_list[-1], self.end, self.expand_dis) + if self.calc_dist_to_goal(self.node_list[-1].x, + self.node_list[-1].y) <= self.expand_dis: + final_node = self.steer(self.node_list[-1], self.end, + self.expand_dis) if self.check_collision(final_node, self.obstacle_list): return self.generate_final_course(len(self.node_list) - 1) @@ -108,6 +117,8 @@ class RRT: if d <= self.path_resolution: new_node.path_x.append(to_node.x) new_node.path_y.append(to_node.y) + new_node.x = to_node.x + new_node.y = to_node.y new_node.parent = from_node @@ -130,8 +141,9 @@ class RRT: def get_random_node(self): if random.randint(0, 100) > self.goal_sample_rate: - rnd = self.Node(random.uniform(self.min_rand, self.max_rand), - random.uniform(self.min_rand, self.max_rand)) + rnd = self.Node( + random.uniform(self.min_rand, self.max_rand), + random.uniform(self.min_rand, self.max_rand)) else: # goal point sampling rnd = self.Node(self.end.x, self.end.y) return rnd @@ -139,8 +151,9 @@ class RRT: def draw_graph(self, rnd=None): plt.clf() # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: @@ -167,8 +180,8 @@ class RRT: @staticmethod def get_nearest_node_index(node_list, rnd_node): - dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) - ** 2 for node in node_list] + dlist = [(node.x - rnd_node.x)**2 + (node.y - rnd_node.y)**2 + for node in node_list] minind = dlist.index(min(dlist)) return minind @@ -184,7 +197,7 @@ class RRT: dy_list = [oy - y for y in node.path_y] d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)] - if min(d_list) <= size ** 2: + if min(d_list) <= size**2: return False # collision return True # safe @@ -202,20 +215,14 @@ def main(gx=6.0, gy=10.0): print("start " + __file__) # ====Search Path with RRT==== - obstacleList = [ - (5, 5, 1), - (3, 6, 2), - (3, 8, 2), - (3, 10, 2), - (7, 5, 2), - (9, 5, 2), - (8, 10, 1) - ] # [x, y, radius] + obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2), + (9, 5, 2), (8, 10, 1)] # [x, y, radius] # Set Initial parameters - rrt = RRT(start=[0, 0], - goal=[gx, gy], - rand_area=[-2, 15], - obstacle_list=obstacleList) + rrt = RRT( + start=[0, 0], + goal=[gx, gy], + rand_area=[-2, 15], + obstacle_list=obstacleList) path = rrt.planning(animation=show_animation) if path is None: diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py index 10ad93a6..bef00860 100644 --- a/PathPlanning/RRTStar/rrt_star.py +++ b/PathPlanning/RRTStar/rrt_star.py @@ -12,8 +12,7 @@ import sys import matplotlib.pyplot as plt -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../RRT/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRT/") try: from rrt import RRT @@ -33,15 +32,17 @@ class RRTStar(RRT): super().__init__(x, y) self.cost = 0.0 - def __init__(self, start, goal, obstacle_list, rand_area, + def __init__(self, + start, + goal, + obstacle_list, + rand_area, expand_dis=30.0, path_resolution=1.0, goal_sample_rate=20, max_iter=300, - connect_circle_dist=50.0 - ): - super().__init__(start, goal, obstacle_list, - rand_area, expand_dis, path_resolution, goal_sample_rate, max_iter) + connect_circle_dist=50.0, + search_until_max_iter=False): """ Setting Parameter @@ -51,15 +52,17 @@ class RRTStar(RRT): randArea:Random Sampling Area [min,max] """ + super().__init__(start, goal, obstacle_list, rand_area, expand_dis, + path_resolution, goal_sample_rate, max_iter) self.connect_circle_dist = connect_circle_dist self.goal_node = self.Node(goal[0], goal[1]) + self.search_until_max_iter = search_until_max_iter - def planning(self, animation=True, search_until_max_iter=True): + def planning(self, animation=True): """ rrt star path planning - animation: flag for animation on or off - search_until_max_iter: search until max iteration for path improving or not + animation: flag for animation on or off . """ self.node_list = [self.start] @@ -67,19 +70,28 @@ class RRTStar(RRT): print("Iter:", i, ", number of nodes:", len(self.node_list)) rnd = self.get_random_node() nearest_ind = self.get_nearest_node_index(self.node_list, rnd) - new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis) + new_node = self.steer(self.node_list[nearest_ind], rnd, + self.expand_dis) + near_node = self.node_list[nearest_ind] + new_node.cost = near_node.cost + \ + math.hypot(new_node.x-near_node.x, + new_node.y-near_node.y) if self.check_collision(new_node, self.obstacle_list): near_inds = self.find_near_nodes(new_node) - new_node = self.choose_parent(new_node, near_inds) - if new_node: + node_with_updated_parent = self.choose_parent( + new_node, near_inds) + if node_with_updated_parent: + self.rewire(node_with_updated_parent, near_inds) + self.node_list.append(node_with_updated_parent) + else: self.node_list.append(new_node) - self.rewire(new_node, near_inds) - if animation and i % 5 == 0: + if animation: self.draw_graph(rnd) - if (not search_until_max_iter) and new_node: # check reaching the goal + if ((not self.search_until_max_iter) + and new_node): # if reaches goal last_index = self.search_best_goal_node() if last_index is not None: return self.generate_final_course(last_index) @@ -93,6 +105,21 @@ class RRTStar(RRT): return None def choose_parent(self, new_node, near_inds): + """ + Computes the cheapest point to new_node contained in the list + near_inds and set such a node as the parent of new_node. + Arguments: + -------- + new_node, Node + randomly generated node with a path from its neared point + There are not coalitions between this node and th tree. + near_inds: list + Indices of indices of the nodes what are near to new_node + + Returns. + ------ + Node, a copy of new_node + """ if not near_inds: return None @@ -113,14 +140,18 @@ class RRTStar(RRT): min_ind = near_inds[costs.index(min_cost)] new_node = self.steer(self.node_list[min_ind], new_node) - new_node.parent = self.node_list[min_ind] new_node.cost = min_cost return new_node def search_best_goal_node(self): - dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list] - goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_dis] + dist_to_goal_list = [ + self.calc_dist_to_goal(n.x, n.y) for n in self.node_list + ] + goal_inds = [ + dist_to_goal_list.index(i) for i in dist_to_goal_list + if i <= self.expand_dis + ] safe_goal_inds = [] for goal_ind in goal_inds: @@ -139,17 +170,48 @@ class RRTStar(RRT): return None def find_near_nodes(self, new_node): + """ + 1) defines a ball centered on new_node + 2) Returns all nodes of the three that are inside this ball + Arguments: + --------- + new_node: Node + new randomly generated node, without collisions between + its nearest node + Returns: + ------- + list + List with the indices of the nodes inside the ball of + radius r + """ nnode = len(self.node_list) + 1 r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode)) - # if expand_dist exists, search vertices in a range no more than expand_dist - if hasattr(self, 'expand_dis'): + # if expand_dist exists, search vertices in a range no more than + # expand_dist + if hasattr(self, 'expand_dis'): r = min(r, self.expand_dis) - dist_list = [(node.x - new_node.x) ** 2 + - (node.y - new_node.y) ** 2 for node in self.node_list] - near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2] + dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2 + for node in self.node_list] + near_inds = [dist_list.index(i) for i in dist_list if i <= r**2] return near_inds def rewire(self, new_node, near_inds): + """ + For each node in near_inds, this will check if it is cheaper to + arrive to them from new_node. + In such a case, this will re-assign the parent of the nodes in + near_inds to new_node. + Parameters: + ---------- + new_node, Node + Node randomly added which can be joined to the tree + + near_inds, list of uints + A list of indices of the self.new_node which contains + nodes within a circle of a given radius. + Remark: parent is designated in choose_parent. + + """ for i in near_inds: near_node = self.node_list[i] edge_node = self.steer(new_node, near_node) @@ -161,7 +223,12 @@ class RRTStar(RRT): improved_cost = near_node.cost > edge_node.cost if no_collision and improved_cost: - self.node_list[i] = edge_node + near_node.x = edge_node.x + near_node.y = edge_node.y + near_node.cost = edge_node.cost + near_node.path_x = edge_node.path_x + near_node.path_y = edge_node.path_y + near_node.parent = edge_node.parent self.propagate_cost_to_leaves(new_node) def calc_new_cost(self, from_node, to_node): @@ -192,10 +259,12 @@ def main(): ] # [x,y,size(radius)] # Set Initial parameters - rrt_star = RRTStar(start=[0, 0], - goal=[6, 10], - rand_area=[-2, 15], - obstacle_list=obstacle_list) + rrt_star = RRTStar( + start=[0, 0], + goal=[6, 10], + rand_area=[-2, 15], + obstacle_list=obstacle_list, + expand_dis=1) path = rrt_star.planning(animation=show_animation) if path is None: @@ -206,10 +275,9 @@ def main(): # Draw final path if show_animation: rrt_star.draw_graph() - plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') + plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--') plt.grid(True) - plt.pause(0.01) # Need for Mac - plt.show() + plt.show() if __name__ == '__main__':