diff --git a/Sampling-based Planning/RRT.py b/Sampling-based Planning/RRT.py index 2b7e264..172610a 100644 --- a/Sampling-based Planning/RRT.py +++ b/Sampling-based Planning/RRT.py @@ -1,49 +1,201 @@ import env import plotting +import node + +import numpy as np +import math + import matplotlib.pyplot as plt import matplotlib.patches as patches +class Node: + def __init__(self, x, y): + self.x = x + self.y = y + self.path_x = [] + self.path_y = [] + self.parent = None + + class RRT: def __init__(self, xI, xG): - # Plotting = plotting.Plotting(xI, xG) - # Plotting.animation([xI, xG], [xI, xG], "zhou") - fig, ax = plt.subplots() + self.xI = Node(xI[0], xI[1]) + self.xG = node.Node(xG[0], xG[1]) - plt.axis([-5, 5, -5, 5]) + self.env = env.Env() + # self.plotting = plotting.Plotting(xI, xG) - ax.plot() + self.x_range = self.env.x_range + self.y_range = self.env.y_range - ax.add_patch( - patches.Rectangle( - (1, 1), - 0.5, - 0.5, - edgecolor='black', - facecolor='black', - fill=True - )) + # self.obs_boundary = self.env.obs_boundary + # self.obs_circle = self.env.obs_circle - ax.add_patch( - patches.Circle( - (3, 3), - 0.5, - edgecolor='black', - facecolor='black', - fill=True - ) - ) + self.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] - plt.axis("equal") + self.expand_range = 0.8 + self.goal_sample_rate = 0.05 + self.iterations = 500 + self.node_list = [] + + path = self.planning() + + if path is None: + print("No path!") + else: + print("get it!") + + self.draw_graph() + + plt.plot([x[0] for x in path], [x[1] for x in path], '-r') + plt.grid(True) + plt.pause(0.01) # Need for Mac plt.show() - def planning(self): - return + self.node_list = [self.xI] + for i in range(self.iterations): + node_rand = self.generate_random_node() + node_near = self.get_nearest_node(self.node_list, node_rand) + + node_new = self.new_node(node_near, node_rand, self.expand_range) + + if not self.check_collision(node_new, self.obstacleList): + self.node_list.append(node_new) + + self.draw_graph(node_rand) + + if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_range: + node_end = self.new_node(self.node_list[-1], node.Node(self.xG.x, self.xG.y), self.expand_range) + if not self.check_collision(node_end, self.obstacleList): + return self.extract_path(self.node_list) + + return None + + 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]) + if rnd is not None: + plt.plot(rnd.x, rnd.y, "^k") + for node_x in self.node_list: + if node_x.parent: + plt.plot(node_x.path_x, node_x.path_y, "-g") + + for (ox, oy, size) in self.obstacleList: + self.plot_circle(ox, oy, size) + + plt.plot(self.xI.x, self.xI.y, "xr") + plt.plot(self.xG.x, self.xG.y, "xr") + plt.axis("equal") + plt.axis([-2, 15, -2, 15]) + plt.grid(True) + plt.pause(0.01) + + @staticmethod + def plot_circle(x, y, size, color="-b"): # pragma: no cover + deg = list(range(0, 360, 5)) + deg.append(0) + xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] + yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] + plt.plot(xl, yl, color) + + def extract_path(self, nodelist): + path = [(self.xG.x, self.xG.y)] + node_now = nodelist[-1] + + while node_now.parent is not None: + node_now = node_now.parent + path.append((node_now.x, node_now.y)) + + return path + + def cal_dis_to_goal(self, node_cal): + return math.hypot((node_cal.x - self.xG.x), (node_cal.y - self.xG.y)) + + def new_node(self, node_start, node_goal, expand_range): + new_node = node.Node(node_start.x, node_start.y) + d, theta = self.calc_distance_and_angle(new_node, node_goal) + + new_node.path_x = [new_node.x] + new_node.path_y = [new_node.y] + + if d < expand_range: + expand_range = d + + new_node.x += expand_range * math.cos(theta) + new_node.y += expand_range * math.sin(theta) + new_node.path_x.append(new_node.x) + new_node.path_y.append(new_node.y) + + new_node.parent = node_start + + return new_node + + def generate_random_node(self): + if np.random.random() > self.goal_sample_rate: + return node.Node(np.random.uniform(self.x_range[0], self.x_range[1]), + np.random.uniform(self.y_range[0], self.y_range[1])) + else: + return node.Node(self.xG.x, self.xG.y) + + def get_nearest_node(self, node_list, node_random): + dlist = [(nod.x - node_random.x) ** 2 + (nod.y - node_random.y) ** 2 + for nod in node_list] + minind = dlist.index(min(dlist)) + + return self.node_list[minind] + + @staticmethod + def calc_distance_and_angle(from_node, to_node): + dx = to_node.x - from_node.x + dy = to_node.y - from_node.y + d = math.hypot(dx, dy) + theta = math.atan2(dy, dx) + return d, theta + + def check_collision(self, node_check, obstacleList): + + if node_check is None: + return True + + for (ox, oy, size) in obstacleList: + dx_list = [ox - x for x in node_check.path_x] + dy_list = [oy - y for y in node_check.path_y] + d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)] + + if min(d_list) <= size ** 2: + return True # collision + + return False # safe + # + # def check_collision(self, node_check): + # for obs in self.obs_boundary: + # dx = node_check.x - obs[0] + # dy = node_check.y - obs[1] + # if 0 <= dx <= obs[2] and 0 <= dy <= obs[2]: + # return True + # + # for obs in self.obs_circle: + # d = (node_check.x - obs[0]) ** 2 + (node_check.y - obs[1]) ** 2 + # if d <= obs[2] ** 2: + # return True + # + # return False if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Start = (0, 0) # Starting node + x_Goal = (6, 10) # Goal node rrt = RRT(x_Start, x_Goal) diff --git a/Sampling-based Planning/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/__pycache__/env.cpython-37.pyc index de7dec1..07e6575 100644 Binary files a/Sampling-based Planning/__pycache__/env.cpython-37.pyc and b/Sampling-based Planning/__pycache__/env.cpython-37.pyc differ diff --git a/Sampling-based Planning/__pycache__/node.cpython-37.pyc b/Sampling-based Planning/__pycache__/node.cpython-37.pyc new file mode 100644 index 0000000..80b51ca Binary files /dev/null and b/Sampling-based Planning/__pycache__/node.cpython-37.pyc differ diff --git a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc index 667c4d5..ab08889 100644 Binary files a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc and b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc differ diff --git a/Sampling-based Planning/env.py b/Sampling-based Planning/env.py index 9ee379f..8a8ecb7 100644 --- a/Sampling-based Planning/env.py +++ b/Sampling-based Planning/env.py @@ -1,30 +1,48 @@ import numpy as np + class Env: def __init__(self): - self.x_range = (0, 50) # size of background - self.y_range = (0, 30) - self.obs = self.obs_map() - - def obs_map(self): - """ - Initialize obstacles' positions - - :return: map of obstacles - """ - - x = self.x_range - y = self.y_range - w = 2 + self.x_range = (-2, 15) # size of background + self.y_range = (-2, 15) + # self.obs_boundary = self.obs_boundary(self.x_range, self.y_range) + # self.obs_circle = self.obs_circle() + @staticmethod + def obs_boundary(x, y): + w = 1 obs_boundary = [] - for i in np.linspace(x[0], x[1], (x[1]-x[0])//w+1): + for i in np.linspace(x[0], x[1], (x[1] - x[0]) // w + 1): obs_boundary.append((i, y[0], w)) - for i in np.linspace(x[0], x[1], (x[1]-x[0])//w+1): + for i in np.linspace(x[0], x[1], (x[1] - x[0]) // w + 1): obs_boundary.append((i, y[1], w)) - for j in np.linspace(y[0], y[1], (y[1]-y[0])//w+1): - obs_boundary.append((j, x[0], w)) - for j in np.linspace(y[0], y[1], (y[1]-y[0])//w+1): - obs_boundary.append((j, x[1], w)) + for j in np.linspace(y[0] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1): + obs_boundary.append((x[0], j, w)) + for j in np.linspace(y[0] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1): + obs_boundary.append((x[1], j, w)) + for i in np.linspace(10, 20, 10 // w + 1): + obs_boundary.append((i, 15, w)) + for j in np.linspace(1, 14, 13 // w + 1): + obs_boundary.append((20, j, w)) + + for j in np.linspace(15, 29, 14 // w + 1): + obs_boundary.append((30, j, w)) + for j in np.linspace(1, 14, 13 // w + 1): + obs_boundary.append((40, j, w)) + + return obs_boundary + + @staticmethod + def obs_circle(): + obs_cir = [ + (8, 8, 3), + (10, 23, 3), + (20, 25, 2.5), + (30, 7, 3), + (40, 25, 2), + (43, 20, 2.5) + ] + + return obs_cir diff --git a/Sampling-based Planning/node.py b/Sampling-based Planning/node.py new file mode 100644 index 0000000..76e419a --- /dev/null +++ b/Sampling-based Planning/node.py @@ -0,0 +1,7 @@ +class Node: + def __init__(self, x, y): + self.x = x + self.y = y + self.path_x = [] + self.path_y = [] + self.parent = None diff --git a/Sampling-based Planning/plotting.py b/Sampling-based Planning/plotting.py index d552103..d19181a 100644 --- a/Sampling-based Planning/plotting.py +++ b/Sampling-based Planning/plotting.py @@ -1,4 +1,5 @@ import matplotlib.pyplot as plt +import matplotlib.patches as patches import env @@ -6,22 +7,58 @@ class Plotting: def __init__(self, xI, xG): self.xI, self.xG = xI, xG self.env = env.Env() - self.obs = self.env.obs_map() + self.obs_bound = self.env.obs_boundary + self.obs_circle = self.env.obs_circle - def animation(self, path, visited, name): - self.plot_grid(name) - self.plot_visited(visited) - self.plot_path(path) + def animation(self, nodelist, node_rand=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]) + if node_rand is not None: + plt.plot(node_rand.x, node_rand.y, "^k") + + for node in nodelist: + if node.parent: + plt.plot(node.path_x, node.path_y, "-g") + + plt.plot(self.xI[0], self.xI[1], "b*") + plt.plot(self.xG[0], self.xG[1], "g*") + + plt.axis("equal") + plt.axis([-5, 55, -5, 35]) + plt.title("RRT") + plt.grid(True) + plt.pause(0.01) def plot_grid(self, name): - obs_x = [self.obs[i][0] for i in range(len(self.obs))] - obs_y = [self.obs[i][1] for i in range(len(self.obs))] + fig, ax = plt.subplots() + for x in self.obs_bound: + ax.add_patch( + patches.Rectangle( + (x[0], x[1]), x[2], x[2], + edgecolor='black', + facecolor='black', + fill=True + ) + ) + + for x in self.obs_circle: + ax.add_patch( + patches.Circle( + (x[0], x[1]), x[2], + edgecolor='gray', + facecolor='gray', + fill=True + ) + ) + + plt.plot(self.xI[0], self.xI[1], "b*") + plt.plot(self.xG[0], self.xG[1], "g*") - plt.plot(self.xI[0], self.xI[1], "bs") - plt.plot(self.xG[0], self.xG[1], "gs") - plt.plot(obs_x, obs_y, "sk") plt.title(name) plt.axis("equal") + plt.show() def plot_visited(self, visited): visited.remove(self.xI) @@ -30,8 +67,8 @@ class Plotting: for x in visited: count += 1 plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o') - 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 count < len(visited) / 3: length = 15 diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 92bf920..54ec76f 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,12 +20,10 @@ - - - - - - + + + +