update RRT_2D

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

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)
 

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!")
 

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]
+        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(d) for d in dist_table if d <= r]
+        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!")
 

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

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