diff --git a/Sampling-based Planning/gif/RRT_2D.gif b/Sampling-based Planning/gif/RRT_2D.gif index f7e4757..07e5f6c 100644 Binary files a/Sampling-based Planning/gif/RRT_2D.gif and b/Sampling-based Planning/gif/RRT_2D.gif differ diff --git a/Sampling-based Planning/gif/RRT_CONNECT_2D.gif b/Sampling-based Planning/gif/RRT_CONNECT_2D.gif new file mode 100644 index 0000000..6cf37f4 Binary files /dev/null and b/Sampling-based Planning/gif/RRT_CONNECT_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 bbadd8b..bb459c5 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__/rrt.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc index 68d12de..7a43f6e 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_2D/RRT_star.jpeg b/Sampling-based Planning/rrt_2D/gif/RRT_star.jpeg similarity index 100% rename from Sampling-based Planning/rrt_2D/RRT_star.jpeg rename to Sampling-based Planning/rrt_2D/gif/RRT_star.jpeg diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling-based Planning/rrt_2D/plotting.py index a885317..37a3178 100644 --- a/Sampling-based Planning/rrt_2D/plotting.py +++ b/Sampling-based Planning/rrt_2D/plotting.py @@ -27,6 +27,11 @@ class Plotting: self.plot_visited(nodelist, animation) self.plot_path(path) + def animation_connect(self, V1, V2, path, name): + self.plot_grid(name) + self.plot_visited_connect(V1, V2) + self.plot_path(path) + def plot_grid(self, name): fig, ax = plt.subplots() @@ -76,12 +81,33 @@ class Plotting: 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) + if count % 10 == 0: + plt.pause(0.001) else: for node in nodelist: if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + @staticmethod + def plot_visited_connect(V1, V2): + len1, len2 = len(V1), len(V2) + + for k in range(max(len1, len2)): + if k < len1: + if V1[k].parent: + plt.plot([V1[k].x, V1[k].parent.x], [V1[k].y, V1[k].parent.y], "-g") + if k < len2: + if V2[k].parent: + plt.plot([V2[k].x, V2[k].parent.x], [V2[k].y, V2[k].parent.y], "-g") + + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + + if k % 2 == 0: + plt.pause(0.001) + + plt.pause(0.01) + @staticmethod def plot_path(path): plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling-based Planning/rrt_2D/rrt.py index 5d9621a..2f10809 100644 --- a/Sampling-based Planning/rrt_2D/rrt.py +++ b/Sampling-based Planning/rrt_2D/rrt.py @@ -3,10 +3,10 @@ RRT_2D @author: huiming zhou """ -import math -import numpy as np import os import sys +import math +import numpy as np sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") @@ -44,7 +44,7 @@ class Rrt: def planning(self): for i in range(self.iter_max): - node_rand = self.random_state(self.goal_sample_rate) + 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) @@ -58,7 +58,7 @@ class Rrt: return None - def random_state(self, goal_sample_rate): + def generate_random_node(self, goal_sample_rate): delta = self.utils.delta if np.random.random() > goal_sample_rate: @@ -67,16 +67,17 @@ class Rrt: return self.s_goal - def nearest_neighbor(self, node_list, n): - return self.vertex[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) - for nd in node_list]))] + @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_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new diff --git a/Sampling-based Planning/rrt_2D/rrt_connect.py b/Sampling-based Planning/rrt_2D/rrt_connect.py new file mode 100644 index 0000000..d5e22f7 --- /dev/null +++ b/Sampling-based Planning/rrt_2D/rrt_connect.py @@ -0,0 +1,158 @@ +""" +RRT_CONNECT_2D +@author: huiming zhou +""" + +import os +import sys +import math +import copy +import numpy as np +import matplotlib.pyplot as plt + +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 RrtConnect: + def __init__(self, s_start, s_goal, step_len, goal_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.iter_max = iter_max + self.V1 = [self.s_start] + self.V2 = [self.s_goal] + + self.env = env.Env() + self.plotting = plotting.Plotting(s_start, s_goal) + self.utils = utils.Utils() + + 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 + + def planning(self): + for i in range(self.iter_max): + node_rand = self.generate_random_node(self.s_goal, self.goal_sample_rate) + node_near = self.nearest_neighbor(self.V1, 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.V1.append(node_new) + node_near_prim = self.nearest_neighbor(self.V2, node_new) + node_new_prim = self.new_state(node_near_prim, node_new) + + if node_new_prim and not self.utils.is_collision(node_new_prim, node_new_prim): + self.V2.append(node_new_prim) + + while True: + node_new_prim2 = self.new_state(node_new_prim, node_new) + if node_new_prim2 and not self.utils.is_collision(node_new_prim2, node_new_prim): + self.V2.append(node_new_prim2) + node_new_prim = self.change_node(node_new_prim, node_new_prim2) + else: + break + + if self.is_node_same(node_new_prim, node_new): + break + + if self.is_node_same(node_new_prim, node_new): + return self.extract_path(node_new, node_new_prim) + + if len(self.V2) < len(self.V1): + list_mid = copy.deepcopy(self.V1) + self.V1 = copy.deepcopy(self.V2) + self.V2 = copy.deepcopy(list_mid) + + return None + + @staticmethod + def change_node(node_new_prim, node_new_prim2): + node_new = Node((node_new_prim2.x, node_new_prim2.y)) + node_new.parent = node_new_prim + + return node_new + + @staticmethod + def is_node_same(node_new_prim, node_new): + if node_new_prim.x == node_new.x and \ + node_new_prim.y == node_new.y: + return True + + return False + + def generate_random_node(self, sample_goal, 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 sample_goal + + @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 + + @staticmethod + def extract_path(node_new, node_new_prim): + path1 = [(node_new.x, node_new.y)] + node_now = node_new + + while node_now.parent is not None: + node_now = node_now.parent + path1.append((node_now.x, node_now.y)) + + path2 = [(node_new_prim.x, node_new_prim.y)] + node_now = node_new_prim + + while node_now.parent is not None: + node_now = node_now.parent + path2.append((node_now.x, node_now.y)) + + return list(list(reversed(path1)) + path2) + + @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 main(): + x_start = (2, 2) # Starting node + x_goal = (49, 24) # Goal node + + rrt_conn = RrtConnect(x_start, x_goal, 0.8, 0.03, 5000) + path = rrt_conn.planning() + + rrt_conn.plotting.animation_connect(rrt_conn.V1, rrt_conn.V2, path, "RRT_CONNECT") + + +if __name__ == '__main__': + main() diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py index af36877..132f3d5 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star.py +++ b/Sampling-based Planning/rrt_2D/rrt_star.py @@ -3,10 +3,10 @@ RRT_star 2D @author: huiming zhou """ -import math -import numpy as np import os import sys +import math +import numpy as np sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") @@ -50,7 +50,7 @@ class RrtStar: if k % 500 == 0: print(k) - node_rand = self.random_state(self.goal_sample_rate) + 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) @@ -64,7 +64,7 @@ class RrtStar: index = self.search_goal_parent() return self.extract_path(self.vertex[index]) - def random_state(self, goal_sample_rate): + def generate_random_node(self, goal_sample_rate): delta = self.utils.delta if np.random.random() > goal_sample_rate: @@ -161,7 +161,7 @@ def main(): x_start = (2, 2) # Starting node x_goal = (49, 24) # Goal node - rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 10000) + rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000) path = rrt_star.planning() if path: diff --git a/nano.save b/nano.save deleted file mode 100644 index 8b13789..0000000 --- a/nano.save +++ /dev/null @@ -1 +0,0 @@ - diff --git a/nano.save.1 b/nano.save.1 deleted file mode 100644 index 8b13789..0000000 --- a/nano.save.1 +++ /dev/null @@ -1 +0,0 @@ -