diff --git a/Sampling-based Planning/RRT.py b/Sampling-based Planning/RRT.py index 172610a..15db973 100644 --- a/Sampling-based Planning/RRT.py +++ b/Sampling-based Planning/RRT.py @@ -1,18 +1,14 @@ 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 + def __init__(self, n): + self.x = n[0] + self.y = n[1] self.path_x = [] self.path_y = [] self.parent = None @@ -20,96 +16,39 @@ class Node: class RRT: def __init__(self, xI, xG): - self.xI = Node(xI[0], xI[1]) - self.xG = node.Node(xG[0], xG[1]) + self.xI = Node(xI) + self.xG = Node(xG) + self.expand_len = 0.8 + self.goal_sample_rate = 0.05 + self.iterations = 5000 + self.node_list = [self.xI] self.env = env.Env() - # self.plotting = plotting.Plotting(xI, xG) + self.plotting = plotting.Plotting(xI, xG) self.x_range = self.env.x_range self.y_range = self.env.y_range + self.obs_circle = self.env.obs + self.obs_rectangle = self.env.obs_boundary - # self.obs_boundary = self.env.obs_boundary - # self.obs_circle = self.env.obs_circle - - 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] - - 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() + self.path = self.planning() + self.plotting.animation(self.node_list, self.path) def planning(self): - 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_len) - node_new = self.new_node(node_near, node_rand, self.expand_range) - - if not self.check_collision(node_new, self.obstacleList): + if not self.check_collision(node_new, self.obs_circle, self.obs_rectangle): 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) + if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_len: + self.new_node(self.node_list[-1], self.xG, self.expand_len) + 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] @@ -121,20 +60,20 @@ class RRT: 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)) + 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) + def new_node(self, node_start, node_goal, expand_len): + new_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 + if d < expand_len: + expand_len = d - new_node.x += expand_range * math.cos(theta) - new_node.y += expand_range * math.sin(theta) + new_node.x += expand_len * math.cos(theta) + new_node.y += expand_len * math.sin(theta) new_node.path_x.append(new_node.x) new_node.path_y.append(new_node.y) @@ -144,58 +83,38 @@ class RRT: 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) + return Node((np.random.uniform(self.x_range[0], self.x_range[1]), + np.random.uniform(self.y_range[0], self.y_range[1]))) + return self.xG - 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] + def get_nearest_node(self, node_list, n): + return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) + for nd in node_list]))] @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 + return math.hypot(dx, dy), math.atan2(dy, dx) - def check_collision(self, node_check, obstacleList): - - if node_check is None: + @staticmethod + def check_collision(node_end, obs_circle, obs_rectangle): + if node_end 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)] + for (ox, oy, r) in obs_circle: + if math.hypot(node_end.x - ox, node_end.y - oy) <= r: + return True - if min(d_list) <= size ** 2: - return True # collision + for (ox, oy, w, h) in obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True - 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 + return False if __name__ == '__main__': - x_Start = (0, 0) # Starting node - x_Goal = (6, 10) # Goal node + x_Start = (15, 5) # Starting node + x_Goal = (45, 25) # 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 07e6575..e743b73 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 deleted file mode 100644 index 80b51ca..0000000 Binary files a/Sampling-based Planning/__pycache__/node.cpython-37.pyc and /dev/null differ diff --git a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc index ab08889..59dd57e 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 8a8ecb7..85a0aba 100644 --- a/Sampling-based Planning/env.py +++ b/Sampling-based Planning/env.py @@ -1,48 +1,34 @@ -import numpy as np - - class Env: def __init__(self): - 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() + self.x_range = (0, 50) + self.y_range = (0, 30) + self.obs_boundary = self.obs_boundary() + self.obs = 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): - obs_boundary.append((i, y[0], w)) - 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] + 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)) + def obs_boundary(): + obs_boundary = [ + (0, 0, 1, 30), + (0, 30, 50, 1), + (1, 0, 50, 1), + (50, 1, 1, 30), + (20, 1, 1, 15), + (10, 15, 10, 1), + (30, 15, 1, 15), + (40, 1, 1, 15) + ] 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) + (5, 10, 3), + (10, 22, 3.5), + (21, 23, 3), + (34, 9, 4), + (37, 23, 3), + (45, 20, 2) ] return obs_cir diff --git a/Sampling-based Planning/node.py b/Sampling-based Planning/node.py deleted file mode 100644 index 76e419a..0000000 --- a/Sampling-based Planning/node.py +++ /dev/null @@ -1,7 +0,0 @@ -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 d19181a..de838bc 100644 --- a/Sampling-based Planning/plotting.py +++ b/Sampling-based Planning/plotting.py @@ -8,35 +8,23 @@ class Plotting: self.xI, self.xG = xI, xG self.env = env.Env() self.obs_bound = self.env.obs_boundary - self.obs_circle = self.env.obs_circle + self.obs_circle = self.env.obs - 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") + def animation(self, nodelist, path): + if path is None: + print("No path found!") + return - 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) + self.plot_visited(nodelist) + self.plot_path(path) def plot_grid(self, name): fig, ax = plt.subplots() + for x in self.obs_bound: ax.add_patch( patches.Rectangle( - (x[0], x[1]), x[2], x[2], + (x[0], x[1]), x[2], x[3], edgecolor='black', facecolor='black', fill=True @@ -47,44 +35,28 @@ class Plotting: ax.add_patch( patches.Circle( (x[0], x[1]), x[2], - edgecolor='gray', + edgecolor='black', 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", linewidth=3) + plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3) plt.title(name) plt.axis("equal") - plt.show() - def plot_visited(self, visited): - visited.remove(self.xI) - count = 0 + @staticmethod + def plot_visited(nodelist): + for node in nodelist: + if node.parent: + plt.plot(node.path_x, node.path_y, "-g") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.pause(0.001) - 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]) - - if count < len(visited) / 3: - length = 15 - elif count < len(visited) * 2 / 3: - length = 30 - else: - length = 45 - - if count % length == 0: plt.pause(0.001) - - def plot_path(self, path): - path.remove(self.xI) - path.remove(self.xG) - path_x = [path[i][0] for i in range(len(path))] - path_y = [path[i][1] for i in range(len(path))] - - plt.plot(path_x, path_y, linewidth='3', color='r', marker='o') + @staticmethod + def plot_path(path): + plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) plt.pause(0.01) plt.show() diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 54ec76f..ac72159 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,9 +20,9 @@ - +