diff --git a/README.md b/README.md index 2e82e86..09aa4e4 100644 --- a/README.md +++ b/README.md @@ -116,15 +116,19 @@ Directory Structure ## Papers -* [Potential Field, ](https://journals.sagepub.com/doi/abs/10.1177/027836498600500106) [[PPT]: ](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf) Real-Time Obstacle Avoidance for Manipulators and Mobile Robots -* [Hybrid A*: ](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) Practical Search Techniques in Path Planning for Autonomous Driving -* [Anytime Repairing A*: ](https://papers.nips.cc/paper/2382-ara-anytime-a-with-provable-bounds-on-sub-optimality.pdf) ARA*: Anytime A* with Provable Bounds on Sub-Optimality -* [Lifelong Planning A*: ](https://www.cs.cmu.edu/~maxim/files/aij04.pdf) Lifelong Planning A* +### Search-base Planning * [D*: ](http://web.mit.edu/16.412j/www/html/papers/original_dstar_icra94.pdf) Optimal and Efficient Path Planning for Partially-Known Environments -* [Focussed D*: ](http://robotics.caltech.edu/~jwb/courses/ME132/handouts/Dstar_ijcai95.pdf) The Focussed D* Algorithm for Real-Time Replanning +* [Lifelong Planning A*: ](https://www.cs.cmu.edu/~maxim/files/aij04.pdf) Lifelong Planning A* +* [Anytime Repairing A*: ](https://papers.nips.cc/paper/2382-ara-anytime-a-with-provable-bounds-on-sub-optimality.pdf) ARA*: Anytime A* with Provable Bounds on Sub-Optimality * [D* Lite: ](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf) D* Lite * [Field D*: ](http://robots.stanford.edu/isrr-papers/draft/stentz.pdf) Field D*: An Interpolation-based Path Planner and Replanner * [Anytime D*: ](http://www.cs.cmu.edu/~ggordon/likhachev-etal.anytime-dstar.pdf) Anytime Dynamic A*: An Anytime, Replanning Algorithm -* [Theta* & AP Theta*: ](http://idm-lab.org/bib/abstracts/papers/aaai07a.pdf) Theta*: Any-Angle Path Planning on Grids -* [Lazy Theta*: ](https://www.aaai.org/ocs/index.php/AAAI/AAAI10/paper/download/1930/1945) Lazy Theta*: Any-Angle Path Planning and Path Length Analysis in 3D -* [Incremental Phi*: ](http://www.cs.cmu.edu/~maxim/files/inctheta_ijcai09.pdf) Incremental Phi*: Incremental Any-Angle Path Planning on Grids +* [Focussed D*: ](http://robotics.caltech.edu/~jwb/courses/ME132/handouts/Dstar_ijcai95.pdf) The Focussed D* Algorithm for Real-Time Replanning +* [Potential Field, ](https://journals.sagepub.com/doi/abs/10.1177/027836498600500106) [[PPT]: ](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf) Real-Time Obstacle Avoidance for Manipulators and Mobile Robots +* [Hybrid A*: ](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) Practical Search Techniques in Path Planning for Autonomous Driving + +### Sampling-based Planning +* [RRT: ](http://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf) Rapidly-Exploring Random Trees: A New Tool for Path Planning +* [RRT-Connect: ](http://www-cgi.cs.cmu.edu/afs/cs/academic/class/15494-s12/readings/kuffner_icra2000.pdf) RRT-Connect: An Efficient Approach to Single-Query Path Planning +* [Extended-RRT: ](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.1.7617&rep=rep1&type=pdf) Real-Time Randomized Path Planning for Robot Navigation +* [Dynamic-RRT: ](https://www.ri.cmu.edu/pub_files/pub4/ferguson_david_2006_2/ferguson_david_2006_2.pdf) Replanning with RRTs diff --git a/Sampling-based Planning/gif/Extended_RRT_2D.gif b/Sampling-based Planning/gif/Extended_RRT_2D.gif new file mode 100644 index 0000000..619ebf5 Binary files /dev/null and b/Sampling-based Planning/gif/Extended_RRT_2D.gif differ diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc index bb459c5..bfd65b2 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc 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 d3f1a64..da349a5 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 new file mode 100644 index 0000000..0df804c --- /dev/null +++ b/Sampling-based Planning/rrt_2D/dynamic_rrt.py @@ -0,0 +1,260 @@ +""" +DYNAMIC_RRT_2D +@author: huiming zhou +""" + +import os +import sys +import math +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Sampling-based Planning/") + +from rrt_2D import env +from rrt_2D import plotting +from rrt_2D import utils + + +class Node: + def __init__(self, n): + self.x = n[0] + self.y = n[1] + self.parent = None + + +class Edge: + def __init__(self, n_p, n_c): + self.parent = n_p + self.child = n_c + + +class DynamicRrt: + def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max): + self.s_start = Node(s_start) + self.s_goal = Node(s_goal) + self.step_len = step_len + self.goal_sample_rate = goal_sample_rate + self.waypoint_sample_rate = waypoint_sample_rate + self.iter_max = iter_max + self.vertex = [self.s_start] + self.edges = set() + + self.env = env.Env() + self.plotting = plotting.Plotting(s_start, s_goal) + self.utils = utils.Utils() + self.fig, self.ax = plt.subplots() + + self.x_range = self.env.x_range + self.y_range = self.env.y_range + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + + self.path = [] + self.waypoint = [] + + def planning(self): + for i in range(self.iter_max): + node_rand = self.generate_random_node(self.goal_sample_rate) + node_near = self.nearest_neighbor(self.vertex, node_rand) + node_new = self.new_state(node_near, node_rand) + + if node_new and not self.utils.is_collision(node_near, node_new): + self.vertex.append(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_visited() + self.plot_path(path) + self.path = path + self.waypoint = self.extract_waypoint(node_new) + self.fig.canvas.mpl_connect('button_press_event', self.on_press) + plt.show() + + return + + return None + + def on_press(self, event): + x, y = event.xdata, event.ydata + if x < 0 or x > 50 or y < 0 or y > 30: + print("Please choose right area!") + else: + x, y = int(x), int(y) + print("Add circle obstacle at: x =", x, ",", "y =", y) + self.obs_circle.append([x, y, 2]) + self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle) + path, waypoint = self.replanning() + + 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 replanning(self): + self.vertex = [self.s_start] + + for i in range(self.iter_max): + node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate) + node_near = self.nearest_neighbor(self.vertex, node_rand) + node_new = self.new_state(node_near, node_rand) + + if node_new and not self.utils.is_collision(node_near, node_new): + self.vertex.append(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) + + return path, waypoint + + return None + + def generate_random_node(self, goal_sample_rate): + delta = self.utils.delta + + if np.random.random() > goal_sample_rate: + 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))) + + return self.s_goal + + def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate): + delta = self.utils.delta + p = np.random.random() + + 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)] + 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) + for nd in node_list]))] + + def new_state(self, node_start, node_end): + dist, theta = self.get_distance_and_angle(node_start, node_end) + + dist = min(self.step_len, dist) + node_new = Node((node_start.x + dist * math.cos(theta), + node_start.y + dist * math.sin(theta))) + node_new.parent = node_start + + return node_new + + def extract_path(self, node_end): + path = [(self.s_goal.x, self.s_goal.y)] + node_now = node_end + + while node_now.parent is not None: + node_now = node_now.parent + path.append((node_now.x, node_now.y)) + + return path + + def extract_waypoint(self, node_end): + waypoint = [self.s_goal] + node_now = node_end + + while node_now.parent is not None: + node_now = node_now.parent + waypoint.append(node_now) + + return waypoint + + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + + def plot_grid(self, name): + + for (ox, oy, w, h) in self.obs_boundary: + self.ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='black', + fill=True + ) + ) + + for (ox, oy, w, h) in self.obs_rectangle: + self.ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + for (ox, oy, r) in self.obs_circle: + self.ax.add_patch( + patches.Circle( + (ox, oy), r, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3) + plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3) + + plt.title(name) + plt.axis("equal") + + def plot_visited(self): + animation = True + if animation: + count = 0 + for node in self.vertex: + count += 1 + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + 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) + else: + for node in self.vertex: + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + + @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) + plt.pause(0.01) + + +def main(): + x_start = (2, 2) # Starting node + x_goal = (49, 24) # Goal node + + drrt = DynamicRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000) + drrt.planning() + + +if __name__ == '__main__': + main() diff --git a/Sampling-based Planning/rrt_2D/extended_rrt.py b/Sampling-based Planning/rrt_2D/extended_rrt.py new file mode 100644 index 0000000..12c6a4e --- /dev/null +++ b/Sampling-based Planning/rrt_2D/extended_rrt.py @@ -0,0 +1,253 @@ +""" +EXTENDED_RRT_2D +@author: huiming zhou +""" + +import os +import sys +import math +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Sampling-based Planning/") + +from rrt_2D import env +from rrt_2D import plotting +from rrt_2D import utils + + +class Node: + def __init__(self, n): + self.x = n[0] + self.y = n[1] + self.parent = None + + +class ExtendedRrt: + def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max): + self.s_start = Node(s_start) + self.s_goal = Node(s_goal) + self.step_len = step_len + self.goal_sample_rate = goal_sample_rate + self.waypoint_sample_rate = waypoint_sample_rate + self.iter_max = iter_max + self.vertex = [self.s_start] + + self.env = env.Env() + self.plotting = plotting.Plotting(s_start, s_goal) + self.utils = utils.Utils() + self.fig, self.ax = plt.subplots() + + self.x_range = self.env.x_range + self.y_range = self.env.y_range + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + + self.path = [] + self.waypoint = [] + + def planning(self): + for i in range(self.iter_max): + node_rand = self.generate_random_node(self.goal_sample_rate) + node_near = self.nearest_neighbor(self.vertex, node_rand) + node_new = self.new_state(node_near, node_rand) + + if node_new and not self.utils.is_collision(node_near, node_new): + self.vertex.append(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_visited() + self.plot_path(path) + self.path = path + self.waypoint = self.extract_waypoint(node_new) + self.fig.canvas.mpl_connect('button_press_event', self.on_press) + plt.show() + + return + + return None + + def on_press(self, event): + x, y = event.xdata, event.ydata + if x < 0 or x > 50 or y < 0 or y > 30: + print("Please choose right area!") + else: + x, y = int(x), int(y) + print("Add circle obstacle at: x =", x, ",", "y =", y) + self.obs_circle.append([x, y, 2]) + self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle) + path, waypoint = self.replanning() + + 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 replanning(self): + self.vertex = [self.s_start] + + for i in range(self.iter_max): + node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate) + node_near = self.nearest_neighbor(self.vertex, node_rand) + node_new = self.new_state(node_near, node_rand) + + if node_new and not self.utils.is_collision(node_near, node_new): + self.vertex.append(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) + + return path, waypoint + + return None + + def generate_random_node(self, goal_sample_rate): + delta = self.utils.delta + + if np.random.random() > goal_sample_rate: + 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))) + + return self.s_goal + + def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate): + delta = self.utils.delta + p = np.random.random() + + 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)] + 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) + for nd in node_list]))] + + def new_state(self, node_start, node_end): + dist, theta = self.get_distance_and_angle(node_start, node_end) + + dist = min(self.step_len, dist) + node_new = Node((node_start.x + dist * math.cos(theta), + node_start.y + dist * math.sin(theta))) + node_new.parent = node_start + + return node_new + + def extract_path(self, node_end): + path = [(self.s_goal.x, self.s_goal.y)] + node_now = node_end + + while node_now.parent is not None: + node_now = node_now.parent + path.append((node_now.x, node_now.y)) + + return path + + def extract_waypoint(self, node_end): + waypoint = [self.s_goal] + node_now = node_end + + while node_now.parent is not None: + node_now = node_now.parent + waypoint.append(node_now) + + return waypoint + + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + + def plot_grid(self, name): + + for (ox, oy, w, h) in self.obs_boundary: + self.ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='black', + fill=True + ) + ) + + for (ox, oy, w, h) in self.obs_rectangle: + self.ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + for (ox, oy, r) in self.obs_circle: + self.ax.add_patch( + patches.Circle( + (ox, oy), r, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3) + plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3) + + plt.title(name) + plt.axis("equal") + + def plot_visited(self): + animation = True + if animation: + count = 0 + for node in self.vertex: + count += 1 + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + 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) + else: + for node in self.vertex: + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + + @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) + plt.pause(0.01) + + +def main(): + x_start = (2, 2) # Starting node + x_goal = (49, 24) # Goal node + + errt = ExtendedRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000) + errt.planning() + + +if __name__ == '__main__': + main() diff --git a/Sampling-based Planning/rrt_2D/test.py b/Sampling-based Planning/rrt_2D/test.py deleted file mode 100644 index 65b48c5..0000000 --- a/Sampling-based Planning/rrt_2D/test.py +++ /dev/null @@ -1,14 +0,0 @@ -import math -import numpy as np -import os -import sys - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") - -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils - -a = [0, 0] -r = 1 diff --git a/Sampling-based Planning/rrt_2D/utils.py b/Sampling-based Planning/rrt_2D/utils.py index 5dca98a..ca230a6 100644 --- a/Sampling-based Planning/rrt_2D/utils.py +++ b/Sampling-based Planning/rrt_2D/utils.py @@ -23,7 +23,11 @@ class Utils: self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary - self.obs_vertex = self.get_obs_vertex() + + def update_obs(self, obs_cir, obs_bound, obs_rec): + self.obs_circle = obs_cir + self.obs_boundary = obs_bound + self.obs_rectangle = obs_rec def get_obs_vertex(self): delta = self.delta @@ -81,8 +85,9 @@ class Utils: return True o, d = self.get_ray(start, end) + obs_vertex = self.get_obs_vertex() - for (v1, v2, v3, v4) in self.obs_vertex: + for (v1, v2, v3, v4) in obs_vertex: if self.is_intersect_rec(start, end, o, d, v1, v2): return True if self.is_intersect_rec(start, end, o, d, v2, v3):