diff --git a/Sampling-based Planning/rrt_2D/RRT_star.jpeg b/Sampling-based Planning/rrt_2D/RRT_star.jpeg new file mode 100644 index 0000000..1646ec8 Binary files /dev/null and b/Sampling-based Planning/rrt_2D/RRT_star.jpeg differ 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 3bd1dd8..12d1bb9 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 d989363..8eee08a 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 8436bd6..3083612 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 index 892e236..c7c08f6 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/env.py b/Sampling-based Planning/rrt_2D/env.py index 0a41212..fbc5ae3 100644 --- a/Sampling-based Planning/rrt_2D/env.py +++ b/Sampling-based Planning/rrt_2D/env.py @@ -25,26 +25,21 @@ class Env: @staticmethod def obs_rectangle(): obs_rectangle = [ - [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] + [14, 12, 8, 2], + [18, 22, 8, 3], + [26, 7, 2, 12], + [32, 14, 10, 2] ] return obs_rectangle @staticmethod def obs_circle(): obs_cir = [ - [5, 10, 3], - [10, 22, 3.5], - [34, 9, 4], - [37, 23, 3], - [45, 20, 2] + [7, 12, 3], + [46, 20, 2], + [15, 5, 2], + [37, 7, 3], + [37, 23, 3] ] return obs_cir diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling-based Planning/rrt_2D/plotting.py index 775f975..a885317 100644 --- a/Sampling-based Planning/rrt_2D/plotting.py +++ b/Sampling-based Planning/rrt_2D/plotting.py @@ -22,8 +22,8 @@ class Plotting: self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle - def animation(self, nodelist, path, animation=False): - self.plot_grid("RRT") + def animation(self, nodelist, path, name, animation=False): + self.plot_grid(name) self.plot_visited(nodelist, animation) self.plot_path(path) diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling-based Planning/rrt_2D/rrt.py index 616fddd..016d879 100644 --- a/Sampling-based Planning/rrt_2D/rrt.py +++ b/Sampling-based Planning/rrt_2D/rrt.py @@ -100,13 +100,13 @@ class Rrt: def main(): x_start = (2, 2) # Starting node - x_goal = (49, 28) # Goal node + x_goal = (49, 24) # Goal node - rrt = Rrt(x_start, x_goal, 5, 0.05, 3000) + rrt = Rrt(x_start, x_goal, 0.5, 0.03, 5000) path = rrt.planning() if path: - rrt.plotting.animation(rrt.vertex, path, True) + rrt.plotting.animation(rrt.vertex, path, "RRT", 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 487665f..70db37e 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star.py +++ b/Sampling-based Planning/rrt_2D/rrt_star.py @@ -47,6 +47,9 @@ class RrtStar: def planning(self): for k in range(self.iter_max): + if k % 500 == 0: + print(k) + 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) @@ -87,9 +90,12 @@ class RrtStar: def find_near_neighbor(self, node_new): n = len(self.vertex) + 1 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] - return [dist_table.index(d) for d in dist_table if d <= r] + dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex] + dist_table_index = [dist_table.index(d) for d in dist_table if d <= r and + not self.utils.is_collision(node_new, self.vertex[dist_table.index(d)])] + + return dist_table_index def choose_parent(self, node_new, neighbor_index): cost = [] @@ -109,7 +115,8 @@ class RrtStar: 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] + cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index + if not self.utils.is_collision(self.vertex[i], self.xG)] return node_index[int(np.argmin(cost_list))] return None @@ -152,13 +159,13 @@ class RrtStar: def main(): x_start = (2, 2) # Starting node - x_goal = (49, 28) # Goal node + x_goal = (49, 24) # Goal node - rrt_star = RrtStar(x_start, x_goal, 5, 0.2, 20, 20000) + rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 20000) path = rrt_star.planning() if path: - rrt_star.plotting.animation(rrt_star.vertex, path) + rrt_star.plotting.animation(rrt_star.vertex, path, "RRT*") else: print("No Path Found!") diff --git a/Sampling-based Planning/rrt_2D/test.py b/Sampling-based Planning/rrt_2D/test.py new file mode 100644 index 0000000..65b48c5 --- /dev/null +++ b/Sampling-based Planning/rrt_2D/test.py @@ -0,0 +1,14 @@ +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 28ed399..5dca98a 100644 --- a/Sampling-based Planning/rrt_2D/utils.py +++ b/Sampling-based Planning/rrt_2D/utils.py @@ -38,9 +38,7 @@ class Utils: return obs_list - def is_intersect_segment(self, start, end, a, b): - o, d = self.get_ray(start, end) - + def is_intersect_rec(self, start, end, o, d, a, b): v1 = [o[0] - a[0], o[1] - a[1]] v2 = [b[0] - a[0], b[1] - a[1]] v3 = [-d[1], d[0]] @@ -48,7 +46,7 @@ class Utils: div = np.dot(v2, v3) if div == 0: - div = 0.01 + return False t1 = np.linalg.norm(np.cross(v2, v1)) / div t2 = np.dot(v1, v3) / div @@ -62,15 +60,40 @@ class Utils: return False + def is_intersect_circle(self, o, d, a, r): + d2 = np.dot(d, d) + delta = self.delta + + if d2 == 0: + return False + + t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2 + + if 0 <= t <= 1: + shot = Node((o[0] + t * d[0], o[1] + t * d[1])) + if self.get_dist(shot, Node(a)) <= r + delta: + return True + + return False + def is_collision(self, start, end): if self.is_inside_obs(start) or self.is_inside_obs(end): return True + o, d = self.get_ray(start, end) + 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): + if self.is_intersect_rec(start, end, o, d, v1, v2): + return True + if self.is_intersect_rec(start, end, o, d, v2, v3): + return True + if self.is_intersect_rec(start, end, o, d, v3, v4): + return True + if self.is_intersect_rec(start, end, o, d, v4, v1): + return True + + for (x, y, r) in self.obs_circle: + if self.is_intersect_circle(o, d, [x, y], r): return True return False