diff --git a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc index c96512a..3bd1dd8 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc 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 c6021a6..093cf47 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 b1a9d01..0b0996c 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/__pycache__/utils.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc new file mode 100644 index 0000000..032c162 Binary files /dev/null and b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_2D/env.py b/Sampling-based Planning/rrt_2D/env.py index 5810f28..0a41212 100644 --- a/Sampling-based Planning/rrt_2D/env.py +++ b/Sampling-based Planning/rrt_2D/env.py @@ -15,37 +15,36 @@ class Env: @staticmethod 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) + [0, 0, 1, 30], + [0, 30, 50, 1], + [1, 0, 50, 1], + [50, 1, 1, 30] ] return obs_boundary @staticmethod def obs_rectangle(): obs_rectangle = [ - (13, 10, 5, 3), - (18, 4, 5, 4), - (22, 13, 6, 3), - (33, 15, 5, 3), - (42, 6, 5, 3) + [13, 10, 5, 3], + [18, 4, 5, 4], + [18, 20, 8, 5], + [22, 13, 6, 3], + [33, 15, 5, 3], + [42, 6, 5, 3] + # [42, 25, 1, 2], + # [45, 23, 2, 1], + # [46, 27, 1, 2] ] return obs_rectangle @staticmethod def obs_circle(): obs_cir = [ - (5, 10, 3), - (10, 22, 3.5), - (21, 23, 3), - (34, 9, 4), - (37, 23, 3), - (45, 20, 2) + [5, 10, 3], + [10, 22, 3.5], + [34, 9, 4], + [37, 23, 3], + [45, 20, 2] ] return obs_cir diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling-based Planning/rrt_2D/plotting.py index f8a2129..c61b1b7 100644 --- a/Sampling-based Planning/rrt_2D/plotting.py +++ b/Sampling-based Planning/rrt_2D/plotting.py @@ -70,12 +70,14 @@ class Plotting: @staticmethod def plot_visited(nodelist, animation): if animation: + count = 0 for node in nodelist: + 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]) - plt.pause(0.001) + if count % 5 == 0: plt.pause(0.001) else: for node in nodelist: if node.parent: diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling-based Planning/rrt_2D/rrt.py index 508b2a1..a17aee3 100644 --- a/Sampling-based Planning/rrt_2D/rrt.py +++ b/Sampling-based Planning/rrt_2D/rrt.py @@ -13,6 +13,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + from rrt_2D import env from rrt_2D import plotting +from rrt_2D import utils class Node: @@ -23,16 +24,17 @@ class Node: class Rrt: - def __init__(self, x_start, x_goal, expand_len, goal_sample_rate, iter_limit): + def __init__(self, x_start, x_goal, step_len, goal_sample_rate, iter_max): self.xI = Node(x_start) self.xG = Node(x_goal) - self.expand_len = expand_len + self.step_len = step_len self.goal_sample_rate = goal_sample_rate - self.iter_limit = iter_limit + self.iter_max = iter_max self.vertex = [self.xI] self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) + self.utils = utils.Utils() self.x_range = self.env.x_range self.y_range = self.env.y_range @@ -41,25 +43,28 @@ class Rrt: self.obs_boundary = self.env.obs_boundary def planning(self): - for i in range(self.iter_limit): + for i in range(self.iter_max): node_rand = self.random_state(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.check_collision(node_new): + 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.xG) - if dist <= self.expand_len: + if dist <= self.step_len: self.new_state(node_new, self.xG) return self.extract_path(node_new) return None def random_state(self, goal_sample_rate): + delta = self.utils.delta + if np.random.random() > goal_sample_rate: - 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 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.xG def nearest_neighbor(self, node_list, n): @@ -67,12 +72,11 @@ class Rrt: for nd in node_list]))] def new_state(self, node_start, node_end): - node_new = Node((node_start.x, node_start.y)) - dist, theta = self.get_distance_and_angle(node_new, node_end) + dist, theta = self.get_distance_and_angle(node_start, node_end) - dist = min(self.expand_len, dist) - node_new.x += dist * math.cos(theta) - node_new.y += dist * math.sin(theta) + 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 @@ -87,21 +91,6 @@ class Rrt: return path - def check_collision(self, node_end): - for (ox, oy, r) in self.obs_circle: - if math.hypot(node_end.x - ox, node_end.y - oy) <= r: - return True - - for (ox, oy, w, h) in self.obs_rectangle: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - for (ox, oy, w, h) in self.obs_boundary: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - return False - @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x @@ -113,11 +102,11 @@ def main(): x_start = (2, 2) # Starting node x_goal = (49, 28) # Goal node - rrt = Rrt(x_start, x_goal, 0.4, 0.05, 2000) + rrt = Rrt(x_start, x_goal, 0.6, 0.05, 3000) path = rrt.planning() if path: - rrt.plotting.animation(rrt.vertex, path) + rrt.plotting.animation(rrt.vertex, path, True) else: print("No Path Found!") diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py index 58f7976..3653faf 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star.py +++ b/Sampling-based Planning/rrt_2D/rrt_star.py @@ -13,6 +13,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + from rrt_2D import env from rrt_2D import plotting +from rrt_2D import utils class Node: @@ -24,18 +25,19 @@ class Node: class RrtStar: - def __init__(self, x_start, x_goal, expand_len, - goal_sample_rate, search_radius, iter_limit): + def __init__(self, x_start, x_goal, step_len, + goal_sample_rate, search_radius, iter_max): self.xI = Node(x_start) self.xG = Node(x_goal) - self.expand_len = expand_len + self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius - self.iter_limit = iter_limit + self.iter_max = iter_max self.vertex = [self.xI] self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) + self.utils = utils.Utils() self.x_range = self.env.x_range self.y_range = self.env.y_range @@ -44,12 +46,12 @@ class RrtStar: self.obs_boundary = self.env.obs_boundary def planning(self): - for k in range(self.iter_limit): + for k in range(self.iter_max): node_rand = self.random_state(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.check_collision(node_new): + if node_new and not self.utils.is_collision(node_near, node_new): neighbor_index = self.find_near_neighbor(node_new) if neighbor_index: node_new = self.choose_parent(node_new, neighbor_index) @@ -59,10 +61,28 @@ class RrtStar: index = self.search_goal_parent() return self.extract_path(self.vertex[index]) + def check_collision(self, node_end): + for (ox, oy, r) in self.obs_circle: + if math.hypot(node_end.x - ox, node_end.y - oy) <= r: + return True + + for (ox, oy, w, h) in self.obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + for (ox, oy, w, h) in self.obs_boundary: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + return False + def random_state(self, goal_sample_rate): + delta = self.utils.delta + if np.random.random() > goal_sample_rate: - 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 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.xG def nearest_neighbor(self, node_list, n): @@ -70,19 +90,18 @@ class RrtStar: for nd in node_list]))] def new_state(self, node_start, node_goal): - node_new = Node((node_start.x, node_start.y)) - dist, theta = self.get_distance_and_angle(node_new, node_goal) - dist = min(self.expand_len, dist) + dist, theta = self.get_distance_and_angle(node_start, node_goal) - node_new.x += dist * math.cos(theta) - node_new.y += dist * math.sin(theta) + 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 find_near_neighbor(self, node_new): n = len(self.vertex) + 1 - r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.expand_len) + r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex] @@ -103,7 +122,7 @@ class RrtStar: def search_goal_parent(self): dist_list = [math.hypot(n.x - self.xG.x, n.y - self.xG.y) for n in self.vertex] - node_index = [dist_list.index(i) for i in dist_list if i <= self.expand_len] + node_index = [dist_list.index(i) for i in dist_list if i <= self.step_len] if node_index: cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index] @@ -140,21 +159,6 @@ class RrtStar: return path - def check_collision(self, node_end): - for (ox, oy, r) in self.obs_circle: - if math.hypot(node_end.x - ox, node_end.y - oy) <= r: - return True - - for (ox, oy, w, h) in self.obs_rectangle: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - for (ox, oy, w, h) in self.obs_boundary: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - return False - @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x @@ -166,7 +170,7 @@ def main(): x_start = (2, 2) # Starting node x_goal = (49, 28) # Goal node - rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 5000) + rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 20000) path = rrt_star.planning() if path: diff --git a/Sampling-based Planning/rrt_2D/utils.py b/Sampling-based Planning/rrt_2D/utils.py new file mode 100644 index 0000000..b8a4dc4 --- /dev/null +++ b/Sampling-based Planning/rrt_2D/utils.py @@ -0,0 +1,106 @@ +""" +utils for collision check +@author: huiming zhou +""" + +import math +import numpy as np +import pyrr +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.rrt import Node + + +class Utils: + def __init__(self): + self.env = env.Env() + + self.delta = 0.2 + 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 get_obs_vertex(self): + delta = self.delta + obs_list = [] + + for (ox, oy, w, h) in self.obs_rectangle: + vertex_list = [[ox - delta, oy - delta], + [ox + w + delta, oy - delta], + [ox + w + delta, oy + h + delta], + [ox - delta, oy + h + delta]] + obs_list.append(vertex_list) + + return obs_list + + def is_intersect_segment(self, start, end, a, b): + o, d = self.get_ray(start, end) + + v1 = [o[0] - a[0], o[1] - a[1]] + v2 = [b[0] - a[0], b[1] - a[1]] + v3 = [-d[1], d[0]] + + div = np.dot(v2, v3) + + if div == 0: + div = 0.01 + + t1 = np.linalg.norm(np.cross(v2, v1)) / div + t2 = np.dot(v1, v3) / div + + if t1 >= 0 and 0 <= t2 <= 1: + shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1])) + dist_obs = self.get_dist(start, shot) + dist_seg = self.get_dist(start, end) + if dist_obs <= dist_seg: + return True + + return False + + def is_collision(self, start, end): + if self.is_inside_obs(start) or self.is_inside_obs(end): + return True + + for (v1, v2, v3, v4) in self.obs_vertex: + if self.is_intersect_segment(start, end, v1, v2) \ + or self.is_intersect_segment(start, end, v2, v3) \ + or self.is_intersect_segment(start, end, v3, v4) \ + or self.is_intersect_segment(start, end, v4, v1): + return True + + return False + + def is_inside_obs(self, node): + delta = self.delta + + for (x, y, r) in self.obs_circle: + if math.hypot(node.x - x, node.y - y) <= r + delta: + return True + + for (x, y, w, h) in self.obs_rectangle: + if 0 <= node.x - (x - delta) <= w + 2 * delta \ + and 0 <= node.y - (y - delta) <= h + 2 * delta: + return True + + for (x, y, w, h) in self.obs_boundary: + if 0 <= node.x - (x - delta) <= w + 2 * delta \ + and 0 <= node.y - (y - delta) <= h + 2 * delta: + return True + + return False + + @staticmethod + def get_ray(start, end): + orig = [start.x, start.y] + direc = [end.x - start.x, end.y - start.y] + return orig, direc + + @staticmethod + def get_dist(start, end): + return math.hypot(end.x - start.x, end.y - start.y)