update

Sampling-based Planning/rrt_2D/plotting.py

@@ -27,6 +27,11 @@ class Plotting:
         self.plot_visited(nodelist, animation)
         self.plot_path(path)
 
+    def animation_connect(self, V1, V2, path, name):
+        self.plot_grid(name)
+        self.plot_visited_connect(V1, V2)
+        self.plot_path(path)
+
     def plot_grid(self, name):
         fig, ax = plt.subplots()
 
@@ -76,12 +81,33 @@ class Plotting:
                     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])
-                    if count % 10 == 0: plt.pause(0.001)
+                    if count % 10 == 0:
+                        plt.pause(0.001)
         else:
             for node in nodelist:
                 if node.parent:
                     plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
 
+    @staticmethod
+    def plot_visited_connect(V1, V2):
+        len1, len2 = len(V1), len(V2)
+
+        for k in range(max(len1, len2)):
+            if k < len1:
+                if V1[k].parent:
+                    plt.plot([V1[k].x, V1[k].parent.x], [V1[k].y, V1[k].parent.y], "-g")
+            if k < len2:
+                if V2[k].parent:
+                    plt.plot([V2[k].x, V2[k].parent.x], [V2[k].y, V2[k].parent.y], "-g")
+
+            plt.gcf().canvas.mpl_connect('key_release_event',
+                                         lambda event: [exit(0) if event.key == 'escape' else None])
+
+            if k % 2 == 0:
+                plt.pause(0.001)
+
+        plt.pause(0.01)
+
     @staticmethod
     def plot_path(path):
         plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)

Sampling-based Planning/rrt_2D/rrt.py

@@ -3,10 +3,10 @@ RRT_2D
 @author: huiming zhou
 """
 
-import math
-import numpy as np
 import os
 import sys
+import math
+import numpy as np
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Sampling-based Planning/")
@@ -44,7 +44,7 @@ class Rrt:
 
     def planning(self):
         for i in range(self.iter_max):
-            node_rand = self.random_state(self.goal_sample_rate)
+            node_rand = self.generate_random_node(self.goal_sample_rate)
             node_near = self.nearest_neighbor(self.vertex, node_rand)
             node_new = self.new_state(node_near, node_rand)
 
@@ -58,7 +58,7 @@ class Rrt:
 
         return None
 
-    def random_state(self, goal_sample_rate):
+    def generate_random_node(self, goal_sample_rate):
         delta = self.utils.delta
 
         if np.random.random() > goal_sample_rate:
@@ -67,16 +67,17 @@ class Rrt:
 
         return self.s_goal
 
-    def nearest_neighbor(self, node_list, n):
-        return self.vertex[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
-                                          for nd in node_list]))]
+    @staticmethod
+    def nearest_neighbor(node_list, n):
+        return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                        for nd in node_list]))]
 
     def new_state(self, node_start, node_end):
         dist, theta = self.get_distance_and_angle(node_start, node_end)
 
         dist = min(self.step_len, dist)
         node_new = Node((node_start.x + dist * math.cos(theta),
-                        node_start.y + dist * math.sin(theta)))
+                         node_start.y + dist * math.sin(theta)))
         node_new.parent = node_start
 
         return node_new

Sampling-based Planning/rrt_2D/rrt_connect.py

@@ -0,0 +1,158 @@
+"""
+RRT_CONNECT_2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import copy
+import numpy as np
+import matplotlib.pyplot as plt
+
+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
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+
+
+class RrtConnect:
+    def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):
+        self.s_start = Node(s_start)
+        self.s_goal = Node(s_goal)
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.iter_max = iter_max
+        self.V1 = [self.s_start]
+        self.V2 = [self.s_goal]
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(s_start, s_goal)
+        self.utils = utils.Utils()
+
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+
+    def planning(self):
+        for i in range(self.iter_max):
+            node_rand = self.generate_random_node(self.s_goal, self.goal_sample_rate)
+            node_near = self.nearest_neighbor(self.V1, node_rand)
+            node_new = self.new_state(node_near, node_rand)
+
+            if node_new and not self.utils.is_collision(node_near, node_new):
+                self.V1.append(node_new)
+                node_near_prim = self.nearest_neighbor(self.V2, node_new)
+                node_new_prim = self.new_state(node_near_prim, node_new)
+
+                if node_new_prim and not self.utils.is_collision(node_new_prim, node_new_prim):
+                    self.V2.append(node_new_prim)
+
+                    while True:
+                        node_new_prim2 = self.new_state(node_new_prim, node_new)
+                        if node_new_prim2 and not self.utils.is_collision(node_new_prim2, node_new_prim):
+                            self.V2.append(node_new_prim2)
+                            node_new_prim = self.change_node(node_new_prim, node_new_prim2)
+                        else:
+                            break
+
+                        if self.is_node_same(node_new_prim, node_new):
+                            break
+
+                if self.is_node_same(node_new_prim, node_new):
+                    return self.extract_path(node_new, node_new_prim)
+
+            if len(self.V2) < len(self.V1):
+                list_mid = copy.deepcopy(self.V1)
+                self.V1 = copy.deepcopy(self.V2)
+                self.V2 = copy.deepcopy(list_mid)
+
+        return None
+
+    @staticmethod
+    def change_node(node_new_prim, node_new_prim2):
+        node_new = Node((node_new_prim2.x, node_new_prim2.y))
+        node_new.parent = node_new_prim
+
+        return node_new
+
+    @staticmethod
+    def is_node_same(node_new_prim, node_new):
+        if node_new_prim.x == node_new.x and \
+                node_new_prim.y == node_new.y:
+            return True
+
+        return False
+
+    def generate_random_node(self, sample_goal, goal_sample_rate):
+        delta = self.utils.delta
+
+        if np.random.random() > goal_sample_rate:
+            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 sample_goal
+
+    @staticmethod
+    def nearest_neighbor(node_list, n):
+        return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                        for nd in node_list]))]
+
+    def new_state(self, node_start, node_end):
+        dist, theta = self.get_distance_and_angle(node_start, node_end)
+
+        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
+
+    @staticmethod
+    def extract_path(node_new, node_new_prim):
+        path1 = [(node_new.x, node_new.y)]
+        node_now = node_new
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            path1.append((node_now.x, node_now.y))
+
+        path2 = [(node_new_prim.x, node_new_prim.y)]
+        node_now = node_new_prim
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            path2.append((node_now.x, node_now.y))
+
+        return list(list(reversed(path1)) + path2)
+
+    @staticmethod
+    def get_distance_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+
+def main():
+    x_start = (2, 2)  # Starting node
+    x_goal = (49, 24)  # Goal node
+
+    rrt_conn = RrtConnect(x_start, x_goal, 0.8, 0.03, 5000)
+    path = rrt_conn.planning()
+
+    rrt_conn.plotting.animation_connect(rrt_conn.V1, rrt_conn.V2, path, "RRT_CONNECT")
+
+
+if __name__ == '__main__':
+    main()

Sampling-based Planning/rrt_2D/rrt_star.py

@@ -3,10 +3,10 @@ RRT_star 2D
 @author: huiming zhou
 """
 
-import math
-import numpy as np
 import os
 import sys
+import math
+import numpy as np
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Sampling-based Planning/")
@@ -50,7 +50,7 @@ class RrtStar:
             if k % 500 == 0:
                 print(k)
 
-            node_rand = self.random_state(self.goal_sample_rate)
+            node_rand = self.generate_random_node(self.goal_sample_rate)
             node_near = self.nearest_neighbor(self.vertex, node_rand)
             node_new = self.new_state(node_near, node_rand)
 
@@ -64,7 +64,7 @@ class RrtStar:
         index = self.search_goal_parent()
         return self.extract_path(self.vertex[index])
 
-    def random_state(self, goal_sample_rate):
+    def generate_random_node(self, goal_sample_rate):
         delta = self.utils.delta
 
         if np.random.random() > goal_sample_rate:
@@ -161,7 +161,7 @@ def main():
     x_start = (2, 2)  # Starting node
     x_goal = (49, 24)  # Goal node
 
-    rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 10000)
+    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)
     path = rrt_star.planning()
 
     if path:

nano.save

@@ -1 +0,0 @@
-

nano.save.1

@@ -1 +0,0 @@
-