update RRT_2D

Sampling-based Planning/rrt_2D/plotting.py

@@ -5,17 +5,14 @@ from rrt_2D import env
 
 
 class Plotting:
-    def __init__(self, xI, xG):
-        self.xI, self.xG = xI, xG
+    def __init__(self, x_start, x_goal):
+        self.xI, self.xG = x_start, x_goal
         self.env = env.Env()
         self.obs_bound = self.env.obs_boundary
         self.obs_circle = self.env.obs_circle
         self.obs_rectangle = self.env.obs_rectangle
 
     def animation(self, nodelist, path, animation=False):
-        if path is None:
-            print("No path found!")
-            return
         self.plot_grid("RRT")
         self.plot_visited(nodelist, animation)
         self.plot_path(path)
@@ -55,6 +52,7 @@ class Plotting:
 
         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")
 
@@ -72,9 +70,8 @@ class Plotting:
                 if node.parent:
                     plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
 
-
     @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()
+        plt.show()

Sampling-based Planning/rrt_2D/rrt*.py

@@ -1,200 +0,0 @@
-from rrt_2D import env
-from rrt_2D import plotting
-
-import numpy as np
-import math
-
-
-class Node:
-    def __init__(self, n):
-        self.x = n[0]
-        self.y = n[1]
-        self.cost = 0.0
-        self.parent = None
-
-
-class RRT:
-    def __init__(self, xI, xG):
-        self.xI = Node(xI)
-        self.xG = Node(xG)
-        self.expand_len = 1
-        self.goal_sample_rate = 0.05
-        self.connect_dist = 10
-        self.iterations = 5000
-        self.node_list = [self.xI]
-
-        self.env = env.Env()
-        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_circle
-        self.obs_rectangle = self.env.obs_rectangle
-        self.obs_boundary = self.env.obs_boundary
-
-        self.path = self.planning()
-        self.plotting.animation(self.node_list, self.path, False)
-
-    def planning(self):
-        for k in range(self.iterations):
-            node_rand = self.random_state()
-            node_near = self.nearest_neighbor(self.node_list, node_rand)
-            node_new = self.new_state(node_near, node_rand)
-
-            if not self.check_collision(node_new):
-                neighbor_index = self.find_near_neighbor(node_new)
-                node_new = self.choose_parent(node_new, neighbor_index)
-                if node_new:
-                    self.node_list.append(node_new)
-                    self.rewire(node_new, neighbor_index)
-
-            # if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
-            #     self.new_state(self.node_list[-1], self.xG)
-            #     return self.extract_path()
-
-        index = self.search_best_goal_node()
-        self.xG.parent = self.node_list[index]
-        return self.extract_path()
-
-    def random_state(self):
-        if np.random.random() > self.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 self.xG
-
-    def nearest_neighbor(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]))]
-
-    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)
-
-        node_new.x += dist * math.cos(theta)
-        node_new.y += dist * math.sin(theta)
-        node_new.parent = node_start
-
-        return node_new
-
-    def find_near_neighbor(self, node_new):
-        n = len(self.node_list) + 1
-        r = min(self.connect_dist * math.sqrt((math.log(n) / n)), self.expand_len)
-
-        dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.node_list]
-        node_index = [dist_table.index(d) for d in dist_table if d <= r]
-
-        return node_index
-
-    def choose_parent(self, node_new, neighbor_index):
-        if not neighbor_index:
-            return None
-
-        cost = []
-
-        for i in neighbor_index:
-            node_near = self.node_list[i]
-            node_mid = self.new_state(node_near, node_new)
-
-            if node_mid and not self.check_collision(node_mid):
-                cost.append(self.update_cost(node_near, node_mid))
-            else:
-                cost.append(float("inf"))
-
-        if min(cost) != float('inf'):
-            index = int(np.argmin(cost))
-            neighbor_min = neighbor_index[index]
-            node_new = self.new_state(self.node_list[neighbor_min], node_new)
-            node_new.cost = min(cost)
-            return node_new
-
-        return None
-
-    def search_best_goal_node(self):
-        dist_to_goal_list = [self.dis_to_goal(n) for n in self.node_list]
-        goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_len]
-
-        return goal_inds[0]
-        # safe_goal_inds = []
-        # for goal_ind in goal_inds:
-        #     t_node = self.new_state(self.node_list[goal_ind], self.xG)
-        #     if self.check_collision(t_node):
-        #         safe_goal_inds.append(goal_ind)
-        #
-        # if not safe_goal_inds:
-        #     print('hahhah')
-        #     return None
-        #
-        # min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
-        # for i in safe_goal_inds:
-        #     if self.node_list[i].cost == min_cost:
-        #         self.xG.parent = self.node_list[i]
-
-    def rewire(self, node_new, neighbor_index):
-        for i in neighbor_index:
-            node_near = self.node_list[i]
-            node_edge = self.new_state(node_new, node_near)
-            if not node_edge:
-                continue
-
-            node_edge.cost = self.update_cost(node_new, node_near)
-            collision = self.check_collision(node_edge)
-            improved_cost = node_near.cost > node_edge.cost
-
-            if not collision and improved_cost:
-                self.node_list[i] = node_edge
-                self.propagate_cost_to_leaves(node_new)
-
-    def update_cost(self, node_start, node_end):
-        dist, theta = self.get_distance_and_angle(node_start, node_end)
-        return node_start.cost + dist
-
-    def propagate_cost_to_leaves(self, parent_node):
-        for node in self.node_list:
-            if node.parent == parent_node:
-                node.cost = self.update_cost(parent_node, node)
-                self.propagate_cost_to_leaves(node)
-
-    def extract_path(self):
-        path = [[self.xG.x, self.xG.y]]
-        node = self.xG
-        while node.parent is not None:
-            path.append([node.x, node.y])
-            node = node.parent
-        path.append([node.x, node.y])
-
-        return path
-
-    def dis_to_goal(self, node_cal):
-        return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
-
-    def check_collision(self, node_end):
-        if node_end is None:
-            return True
-
-        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
-        dy = node_end.y - node_start.y
-        return math.hypot(dx, dy), math.atan2(dy, dx)
-
-
-if __name__ == '__main__':
-    x_Start = (2, 2)  # Starting node
-    x_Goal = (49, 28)  # Goal node
-
-    rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/rrt_2D/rrt.py

@@ -12,17 +12,17 @@ class Node:
         self.parent = None
 
 
-class RRT:
-    def __init__(self, xI, xG):
-        self.xI = Node(xI)
-        self.xG = Node(xG)
-        self.expand_len = 0.4
-        self.goal_sample_rate = 0.05
-        self.iterations = 5000
-        self.node_list = [self.xI]
+class Rrt:
+    def __init__(self, x_start, x_goal, expand_len, goal_sample_rate, iter_limit):
+        self.xI = Node(x_start)
+        self.xG = Node(x_goal)
+        self.expand_len = expand_len
+        self.goal_sample_rate = goal_sample_rate
+        self.iter_limit = iter_limit
+        self.vertex = [self.xI]
 
         self.env = env.Env()
-        self.plotting = plotting.Plotting(xI, xG)
+        self.plotting = plotting.Plotting(x_start, x_goal)
 
         self.x_range = self.env.x_range
         self.y_range = self.env.y_range
@@ -30,33 +30,31 @@ class RRT:
         self.obs_rectangle = self.env.obs_rectangle
         self.obs_boundary = self.env.obs_boundary
 
-        self.path = self.planning()
-        self.plotting.animation(self.node_list, self.path)
-
     def planning(self):
-        for i in range(self.iterations):
-            node_rand = self.random_state()
-            node_near = self.nearest_neighbor(self.node_list, node_rand)
+        for i in range(self.iter_limit):
+            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 not self.check_collision(node_new):
-                self.node_list.append(node_new)
+            if node_new and not self.check_collision(node_new):
+                self.vertex.append(node_new)
+                dist, _ = self.get_distance_and_angle(node_new, self.xG)
 
-            if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
-                self.new_state(self.node_list[-1], self.xG)
-                return self.extract_path(self.node_list)
+                if dist <= self.expand_len:
+                    self.new_state(node_new, self.xG)
+                    return self.extract_path(node_new)
 
         return None
 
-    def random_state(self):
-        if np.random.random() > self.goal_sample_rate:
+    def random_state(self, goal_sample_rate):
+        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 self.xG
 
     def nearest_neighbor(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]))]
+        return self.vertex[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):
         node_new = Node((node_start.x, node_start.y))
@@ -69,9 +67,9 @@ class RRT:
 
         return node_new
 
-    def extract_path(self, nodelist):
+    def extract_path(self, node_end):
         path = [(self.xG.x, self.xG.y)]
-        node_now = nodelist[-1]
+        node_now = node_end
 
         while node_now.parent is not None:
             node_now = node_now.parent
@@ -79,13 +77,7 @@ class RRT:
 
         return path
 
-    def dis_to_goal(self, node_cal):
-        return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
-
     def check_collision(self, node_end):
-        if node_end is None:
-            return True
-
         for (ox, oy, r) in self.obs_circle:
             if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
                 return True
@@ -107,8 +99,18 @@ class RRT:
         return math.hypot(dx, dy), math.atan2(dy, dx)
 
 
-if __name__ == '__main__':
-    x_Start = (2, 2)  # Starting node
-    x_Goal = (49, 28)  # Goal node
+def main():
+    x_start = (2, 2)  # Starting node
+    x_goal = (49, 28)  # Goal node
 
-    rrt = RRT(x_Start, x_Goal)
+    rrt = Rrt(x_start, x_goal, 0.4, 0.05, 2000)
+    path = rrt.planning()
+
+    if path:
+        rrt.plotting.animation(rrt.vertex, path)
+    else:
+        print("No Path Found!")
+
+
+if __name__ == '__main__':
+    main()

Sampling-based Planning/rrt_2D/rrt_star.py

@@ -0,0 +1,169 @@
+from rrt_2D import env
+from rrt_2D import plotting
+
+import numpy as np
+import math
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.cost = 0.0
+        self.parent = None
+
+
+class RrtStar:
+    def __init__(self, x_start, x_goal, expand_len,
+                 goal_sample_rate, search_radius, iter_limit):
+        self.xI = Node(x_start)
+        self.xG = Node(x_goal)
+        self.expand_len = expand_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_limit = iter_limit
+        self.vertex = [self.xI]
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(x_start, x_goal)
+
+        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 k in range(self.iter_limit):
+            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):
+                neighbor_index = self.find_near_neighbor(node_new)
+                if neighbor_index:
+                    node_new = self.choose_parent(node_new, neighbor_index)
+                    self.vertex.append(node_new)
+                    self.rewire(node_new, neighbor_index)
+
+        index = self.search_goal_parent()
+        return self.extract_path(self.vertex[index])
+
+    def random_state(self, goal_sample_rate):
+        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 self.xG
+
+    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]))]
+
+    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)
+
+        node_new.x += dist * math.cos(theta)
+        node_new.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)
+
+        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]
+
+    def choose_parent(self, node_new, neighbor_index):
+        cost = []
+
+        for i in neighbor_index:
+            node_neighbor = self.vertex[i]
+            cost.append(self.get_new_cost(node_neighbor, node_new))
+
+        cost_min_index = neighbor_index[int(np.argmin(cost))]
+        node_new = self.new_state(self.vertex[cost_min_index], node_new)
+        node_new.cost = min(cost)
+
+        return node_new
+
+    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]
+
+        if node_index:
+            cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index]
+            return node_index[int(np.argmin(cost_list))]
+
+        return None
+
+    def rewire(self, node_new, neighbor_index):
+        for i in neighbor_index:
+            node_neighbor = self.vertex[i]
+            new_cost = self.get_new_cost(node_new, node_neighbor)
+
+            if node_neighbor.cost > new_cost:
+                self.vertex[i] = self.new_state(node_new, node_neighbor)
+                self.propagate_cost_to_leaves(node_new)
+
+    def get_new_cost(self, node_start, node_end):
+        dist, _ = self.get_distance_and_angle(node_start, node_end)
+        return node_start.cost + dist
+
+    def propagate_cost_to_leaves(self, parent_node):
+        for node in self.vertex:
+            if node.parent == parent_node:
+                node.cost = self.get_new_cost(parent_node, node)
+                self.propagate_cost_to_leaves(node)
+
+    def extract_path(self, node_end):
+        path = [[self.xG.x, self.xG.y]]
+        node = node_end
+        while node.parent is not None:
+            path.append([node.x, node.y])
+            node = node.parent
+        path.append([node.x, node.y])
+
+        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
+        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, 28)  # Goal node
+
+    rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 5000)
+    path = rrt_star.planning()
+
+    if path:
+        rrt_star.plotting.animation(rrt_star.vertex, path)
+    else:
+        print("No Path Found!")
+
+
+if __name__ == '__main__':
+    main()