UPDATE INFORMED RRT STAR

Sampling-based Planning/rrt_2D/informed_rrt_star.py

@@ -55,23 +55,21 @@ class IRrtStar:
 
     def planning(self):
         c_best = np.inf
-        c_min, theta = self.get_distance_and_angle(self.x_start, self.x_goal)
-        C = self.RotationToWorldFrame(self.x_start, self.x_goal, c_min)
+        dist, theta = self.get_distance_and_angle(self.x_start, self.x_goal)
+        C = self.RotationToWorldFrame(self.x_start, self.x_goal, dist)
         x_center = np.array([[(self.x_start.x + self.x_goal.x) / 2.0],
                              [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]])
+        x_best = self.x_start
 
         for k in range(self.iter_max):
-            if k % 50 == 0:
-                print(k)
-
             x_rand = self.Sample(self.x_start, self.x_goal, c_best, x_center, C)
             x_nearest = self.Nearest(self.V, x_rand)
             x_new = self.Steer(x_nearest, x_rand)
 
             if x_new and not self.utils.is_collision(x_nearest, x_new):
-                self.V.append(x_new)
                 X_near = self.Near(self.V, x_new)
                 c_min = self.Cost(x_new)
+                self.V.append(x_new)
 
                 for x_near in X_near:
                     c_new = self.Cost(x_near) + self.Line(x_near, x_new)
@@ -81,120 +79,26 @@ class IRrtStar:
 
                 for x_near in X_near:
                     c_near = self.Cost(x_near)
-                    c_new = self.Cost(x_new) + self.Line(x_new, x_near)
+                    c_new = c_min + self.Line(x_new, x_near)
                     if c_new < c_near:
                         x_near.parent = x_new
 
                 if self.InGoalRegion(x_new):
                     self.X_soln.add(x_new)
-                    c_best = min(c_best, self.Cost(x_new) + self.Line(x_new, self.x_goal))
+                    new_cost = self.Cost(x_new) + self.Line(x_new, self.x_goal)
+                    if new_cost < c_best:
+                        c_best = new_cost
+                        x_best = x_new
 
-            self.animation(x_center=x_center, c_best=c_best, c_min=c_min, theta=theta)
+            if k % 20 == 0:
+               self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)
 
-        self.animation(x_center=x_center, c_best=c_best, c_min=c_min, theta=theta)
-        # plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
+        self.path = self.ExtractPath(x_best)
+        self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)
+        plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
         plt.pause(0.01)
         plt.show()
 
-    def animation(self, x_center=None, c_best=None, c_min=None, theta=None):
-        plt.cla()
-        self.plot_grid("Informed rrt*")
-        plt.gcf().canvas.mpl_connect(
-            'key_release_event',
-            lambda event: [exit(0) if event.key == 'escape' else None])
-
-        if c_best != np.inf:
-            self.draw_ellipse(x_center, c_best, c_min, theta)
-
-        for node in self.V:
-            if node.parent:
-                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
-
-        self.plot_grid("Informed rrt*")
-        plt.pause(0.01)
-
-    def plot_grid(self, name):
-
-        for (ox, oy, w, h) in self.obs_boundary:
-            self.ax.add_patch(
-                patches.Rectangle(
-                    (ox, oy), w, h,
-                    edgecolor='black',
-                    facecolor='black',
-                    fill=True
-                )
-            )
-
-        for (ox, oy, w, h) in self.obs_rectangle:
-            self.ax.add_patch(
-                patches.Rectangle(
-                    (ox, oy), w, h,
-                    edgecolor='black',
-                    facecolor='gray',
-                    fill=True
-                )
-            )
-
-        for (ox, oy, r) in self.obs_circle:
-            self.ax.add_patch(
-                patches.Circle(
-                    (ox, oy), r,
-                    edgecolor='black',
-                    facecolor='gray',
-                    fill=True
-                )
-            )
-
-        plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3)
-        plt.plot(self.x_goal.x, self.x_goal.y, "gs", linewidth=3)
-
-        plt.title(name)
-        plt.axis("equal")
-
-    @staticmethod
-    def draw_ellipse(x_center, c_best, c_min, theta):
-        a = math.sqrt(c_best ** 2 - c_min ** 2) / 2.0
-        b = c_best / 2.0
-        angle = math.pi / 2.0 - theta
-        cx = x_center[0]
-        cy = x_center[1]
-        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
-        x = [a * math.cos(it) for it in t]
-        y = [b * math.sin(it) for it in t]
-        rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2]
-        fx = rot @ np.array([x, y])
-        px = np.array(fx[0, :] + cx).flatten()
-        py = np.array(fx[1, :] + cy).flatten()
-        plt.plot(cx, cy, "xc")
-        plt.plot(px, py, "--c")
-
-    def RotationToWorldFrame(self, x_start, x_goal, L):
-        a1 = np.array([[(self.x_goal.x - self.x_start.x) / L],
-                       [(self.x_goal.y - self.x_start.y) / L], [0.0]])
-        e1 = np.array([[1.0], [0.0], [0.0]])
-        M = a1 @ e1.T
-        U, _, V_T = np.linalg.svd(M, True, True)
-        C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T
-
-        return C
-
-    def ExtractPath(self, node):
-        path = [[self.x_goal.x, self.x_goal.y]]
-
-        while node.parent:
-            path.append([node.x, node.y])
-            node = node.parent
-
-        path.append([self.x_start.x, self.x_start.y])
-
-        return path
-
-    def InGoalRegion(self, node):
-        if self.Line(node, self.x_goal) < self.step_len:
-            return True
-
-        return False
-
     def Steer(self, x_start, x_goal):
         dist, theta = self.get_distance_and_angle(x_start, x_goal)
         dist = min(self.step_len, dist)
@@ -222,7 +126,7 @@ class IRrtStar:
                  math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]
             L = np.diag(r)
             x_ball = self.SampleUnitNBall()
-            # x_rand = C @ L @ x_ball + x_center
+
             while True:
                 x_rand = C @ L @ x_ball + x_center
                 if self.x_range[0] + self.delta <= x_rand[0] <= self.x_range[1] + self.delta:
@@ -234,7 +138,7 @@ class IRrtStar:
         return x_rand
 
     def SampleFreeSpace(self):
-        delta = self.utils.delta
+        delta = self.delta
 
         if np.random.random() > self.goal_sample_rate:
             return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
@@ -242,9 +146,37 @@ class IRrtStar:
 
         return self.x_goal
 
+    def ExtractPath(self, node):
+        path = [[self.x_goal.x, self.x_goal.y]]
+
+        while node.parent:
+            path.append([node.x, node.y])
+            node = node.parent
+
+        path.append([self.x_start.x, self.x_start.y])
+
+        return path
+
+    def InGoalRegion(self, node):
+        if self.Line(node, self.x_goal) < self.step_len:
+            return True
+
+        return False
+
+    @staticmethod
+    def RotationToWorldFrame(x_start, x_goal, L):
+        a1 = np.array([[(x_start.x - x_start.x) / L],
+                       [(x_goal.y - x_start.y) / L], [0.0]])
+        e1 = np.array([[1.0], [0.0], [0.0]])
+        M = a1 @ e1.T
+        U, _, V_T = np.linalg.svd(M, True, True)
+        C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T
+
+        return C
+
     @staticmethod
     def SampleUnitNBall():
-        theta, r = random.uniform(0.0, 2*math.pi), random.random()
+        theta, r = random.uniform(0.0, 2 * math.pi), random.random()
         x = r * math.cos(theta)
         y = r * math.sin(theta)
 
@@ -260,9 +192,10 @@ class IRrtStar:
         return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)
 
     @staticmethod
-    def Cost(node_p):
-        node = node_p
+    def Cost(node):
         cost = 0.0
+        if node.parent is None:
+            return cost
 
         while node.parent:
             cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
@@ -276,12 +209,83 @@ class IRrtStar:
         dy = node_end.y - node_start.y
         return math.hypot(dx, dy), math.atan2(dy, dx)
 
+    def animation(self, x_center=None, c_best=None, dist=None, theta=None):
+        plt.cla()
+        self.plot_grid("Informed rrt*, N = " + str(self.iter_max))
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [exit(0) if event.key == 'escape' else None])
+
+        if c_best != np.inf:
+            self.draw_ellipse(x_center, c_best, dist, theta)
+
+        for node in self.V:
+            if node.parent:
+                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
+
+        plt.pause(0.01)
+
+    def plot_grid(self, name):
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3)
+        plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+    @staticmethod
+    def draw_ellipse(x_center, c_best, dist, theta):
+        a = math.sqrt(c_best ** 2 - dist ** 2) / 2.0
+        b = c_best / 2.0
+        angle = math.pi / 2.0 - theta
+        cx = x_center[0]
+        cy = x_center[1]
+        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
+        x = [a * math.cos(it) for it in t]
+        y = [b * math.sin(it) for it in t]
+        rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2]
+        fx = rot @ np.array([x, y])
+        px = np.array(fx[0, :] + cx).flatten()
+        py = np.array(fx[1, :] + cy).flatten()
+        plt.plot(cx, cy, ".b")
+        plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2)
+
 
 def main():
-    x_start = (2, 2)  # Starting node
-    x_goal = (49, 24)  # Goal node
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
 
-    rrt_star = IRrtStar(x_start, x_goal, 5.0, 0.10, 20, 200)
+    rrt_star = IRrtStar(x_start, x_goal, 1.0, 0.10, 0, 1000)
     rrt_star.planning()
 
 

Sampling-based Planning/rrt_2D/rrt_star.py

@@ -7,8 +7,6 @@ import os
 import sys
 import math
 import numpy as np
-import matplotlib.pyplot as plt
-import matplotlib.patches as patches
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Sampling-based Planning/")
@@ -37,12 +35,10 @@ class RrtStar:
         self.iter_max = iter_max
         self.vertex = [self.s_start]
         self.path = []
-        self.visited = []
 
         self.env = env.Env()
         self.plotting = plotting.Plotting(x_start, x_goal)
         self.utils = utils.Utils()
-        # self.fig, self.ax = plt.subplots()
 
         self.x_range = self.env.x_range
         self.y_range = self.env.y_range
@@ -63,10 +59,6 @@ class RrtStar:
                 neighbor_index = self.find_near_neighbor(node_new)
                 self.vertex.append(node_new)
 
-                # if k % 20 == 0:
-                #     self.visited.append([[[node.x, node.parent.x], [node.y, node.parent.y]]
-                #                          for node in self.vertex[1: len(self.vertex)]])
-
                 if neighbor_index:
                     self.choose_parent(node_new, neighbor_index)
                     self.rewire(node_new, neighbor_index)
@@ -74,7 +66,7 @@ class RrtStar:
         index = self.search_goal_parent()
         self.path = self.extract_path(self.vertex[index])
 
-        self.plotting.animation(self.vertex, self.path, "rrt*")
+        self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max))
 
     def new_state(self, node_start, node_goal):
         dist, theta = self.get_distance_and_angle(node_start, node_goal)
@@ -102,9 +94,9 @@ class RrtStar:
 
     def search_goal_parent(self):
         dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex]
-        node_index = [dist_list.index(i) for i in dist_list if i <= self.step_len]
+        node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len]
 
-        if node_index:
+        if len(node_index) > 0:
             cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index
                          if not self.utils.is_collision(self.vertex[i], self.s_goal)]
             return node_index[int(np.argmin(cost_list))]
@@ -125,11 +117,6 @@ class RrtStar:
 
         return self.s_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 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)
@@ -140,6 +127,11 @@ class RrtStar:
 
         return dist_table_index
 
+    @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]))]
+
     @staticmethod
     def cost(node_p):
         node = node_p
@@ -151,62 +143,6 @@ class RrtStar:
 
         return cost
 
-    def animation(self, name):
-        self.plot_grid(name)
-        plt.pause(4)
-        for edge_set in self.visited:
-            plt.cla()
-
-            self.plot_grid(name)
-            for edges in edge_set:
-                plt.plot(edges[0], edges[1], "-g")
-
-            plt.pause(0.0001)
-
-        if self.path:
-            plt.plot([x[0] for x in self.path], [x[1] for x in self.path], '-r', linewidth=2)
-
-        plt.pause(0.5)
-        plt.show()
-
-    def plot_grid(self, name):
-
-        for (ox, oy, w, h) in self.obs_boundary:
-            self.ax.add_patch(
-                patches.Rectangle(
-                    (ox, oy), w, h,
-                    edgecolor='black',
-                    facecolor='black',
-                    fill=True
-                )
-            )
-
-        for (ox, oy, w, h) in self.obs_rectangle:
-            self.ax.add_patch(
-                patches.Rectangle(
-                    (ox, oy), w, h,
-                    edgecolor='black',
-                    facecolor='gray',
-                    fill=True
-                )
-            )
-
-        for (ox, oy, r) in self.obs_circle:
-            self.ax.add_patch(
-                patches.Circle(
-                    (ox, oy), r,
-                    edgecolor='black',
-                    facecolor='gray',
-                    fill=True
-                )
-            )
-
-        plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3)
-        plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3)
-
-        plt.title(name)
-        plt.axis("equal")
-
     def update_cost(self, parent_node):
         OPEN = queue.QueueFIFO()
         OPEN.put(parent_node)
@@ -240,10 +176,10 @@ class RrtStar:
 
 
 def main():
-    x_start = (2, 2)  # Starting node
-    x_goal = (49, 24)  # Goal node
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
 
-    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 15000)
+    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)
     rrt_star.planning()