diff --git a/Sampling-based Planning/gif/Dynamic_RRT_2D.gif b/Sampling-based Planning/gif/Dynamic_RRT_2D.gif new file mode 100644 index 0000000..1faa9e1 Binary files /dev/null and b/Sampling-based Planning/gif/Dynamic_RRT_2D.gif differ diff --git a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc index da349a5..1e50eef 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_2D/dynamic_rrt.py b/Sampling-based Planning/rrt_2D/dynamic_rrt.py index 0df804c..8151ac3 100644 --- a/Sampling-based Planning/rrt_2D/dynamic_rrt.py +++ b/Sampling-based Planning/rrt_2D/dynamic_rrt.py @@ -6,6 +6,7 @@ DYNAMIC_RRT_2D import os import sys import math +import copy import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches @@ -23,12 +24,14 @@ class Node: self.x = n[0] self.y = n[1] self.parent = None + self.flag = "VALID" class Edge: def __init__(self, n_p, n_c): self.parent = n_p self.child = n_c + self.flag = "VALID" class DynamicRrt: @@ -40,7 +43,9 @@ class DynamicRrt: self.waypoint_sample_rate = waypoint_sample_rate self.iter_max = iter_max self.vertex = [self.s_start] - self.edges = set() + self.vertex_old = [] + self.vertex_new = [] + self.edges = [] self.env = env.Env() self.plotting = plotting.Plotting(s_start, s_goal) @@ -52,6 +57,7 @@ class DynamicRrt: self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary + self.obs_add = [0, 0, 0] self.path = [] self.waypoint = [] @@ -64,13 +70,14 @@ class DynamicRrt: if node_new and not self.utils.is_collision(node_near, node_new): self.vertex.append(node_new) + self.edges.append(Edge(node_near, node_new)) dist, _ = self.get_distance_and_angle(node_new, self.s_goal) if dist <= self.step_len: self.new_state(node_new, self.s_goal) path = self.extract_path(node_new) - self.plot_grid("Extended_RRT") + self.plot_grid("Dynamic_RRT") self.plot_visited() self.plot_path(path) self.path = path @@ -89,21 +96,68 @@ class DynamicRrt: else: x, y = int(x), int(y) print("Add circle obstacle at: x =", x, ",", "y =", y) + self.obs_add = [x, y, 2] self.obs_circle.append([x, y, 2]) self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle) - path, waypoint = self.replanning() + self.InvalidateNodes() + + if self.is_path_invalid(): + print("Path is Replanning ...") + path, waypoint = self.replanning() + + print("len_vertex: ", len(self.vertex)) + print("len_vertex_old: ", len(self.vertex_old)) + print("len_vertex_new: ", len(self.vertex_new)) + + plt.cla() + self.plot_grid("Dynamic_RRT") + self.plot_vertex_old() + self.plot_path(self.path, color='blue') + self.plot_vertex_new() + self.vertex_new = [] + self.plot_path(path) + self.path = path + self.waypoint = waypoint + else: + print("Trimming Invalid Nodes ...") + self.TrimRRT() + + plt.cla() + self.plot_grid("Dynamic_RRT") + self.plot_visited(animation=False) + self.plot_path(self.path) - plt.cla() - self.plot_grid("Extended_RRT") - self.plot_path(self.path, color='blue') - self.plot_visited() - self.plot_path(path) - self.path = path - self.waypoint = waypoint self.fig.canvas.draw_idle() + def InvalidateNodes(self): + for edge in self.edges: + if self.is_collision_obs_add(edge.parent, edge.child): + edge.child.flag = "INVALID" + edge.flag = "INVALID" + + def is_path_invalid(self): + for node in self.waypoint: + if node.flag == "INVALID": + return True + + def is_collision_obs_add(self, start, end): + delta = self.utils.delta + obs_add = self.obs_add + + if math.hypot(start.x - obs_add[0], start.y - obs_add[1]) <= obs_add[2] + delta: + return True + + if math.hypot(end.x - obs_add[0], end.y - obs_add[1]) <= obs_add[2] + delta: + return True + + o, d = self.utils.get_ray(start, end) + if self.utils.is_intersect_circle(o, d, [obs_add[0], obs_add[1]], obs_add[2]): + return True + + return False + def replanning(self): - self.vertex = [self.s_start] + self.TrimRRT() for i in range(self.iter_max): node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate) @@ -112,17 +166,32 @@ class DynamicRrt: if node_new and not self.utils.is_collision(node_near, node_new): self.vertex.append(node_new) + self.vertex_new.append(node_new) + self.edges.append(Edge(node_near, node_new)) dist, _ = self.get_distance_and_angle(node_new, self.s_goal) if dist <= self.step_len: self.new_state(node_new, self.s_goal) path = self.extract_path(node_new) waypoint = self.extract_waypoint(node_new) + print("path: ", len(path)) + print("waypoint: ", len(waypoint)) return path, waypoint return None + def TrimRRT(self): + for i in range(1, len(self.vertex)): + node = self.vertex[i] + node_p = node.parent + if node_p.flag == "INVALID": + node.flag = "INVALID" + + self.vertex = [node for node in self.vertex if node.flag == "VALID"] + self.vertex_old = copy.deepcopy(self.vertex) + self.edges = [Edge(node.parent, node) for node in self.vertex[1:len(self.vertex)]] + def generate_random_node(self, goal_sample_rate): delta = self.utils.delta @@ -139,12 +208,11 @@ class DynamicRrt: if p < goal_sample_rate: return self.s_goal elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate: - return self.waypoint[np.random.randint(0, len(self.path) - 1)] + return self.waypoint[np.random.randint(0, len(self.waypoint) - 1)] else: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) - @staticmethod def nearest_neighbor(node_list, n): return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) @@ -224,8 +292,7 @@ class DynamicRrt: plt.title(name) plt.axis("equal") - def plot_visited(self): - animation = True + def plot_visited(self, animation=True): if animation: count = 0 for node in self.vertex: @@ -242,6 +309,24 @@ class DynamicRrt: if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + def plot_vertex_old(self): + for node in self.vertex_old: + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + + def plot_vertex_new(self): + count = 0 + + for node in self.vertex_new: + count += 1 + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], color='darkorange') + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: + [exit(0) if event.key == 'escape' else None]) + if count % 10 == 0: + plt.pause(0.001) + @staticmethod def plot_path(path, color='red'): plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color)