update informed rrt*

Sampling_based_Planning/rrt_2D/batch_informed_trees.py

@@ -6,7 +6,6 @@ Batch Informed Trees (BIT*)
 import os
 import sys
 import math
-import copy
 import random
 import numpy as np
 import matplotlib.pyplot as plt
@@ -66,7 +65,6 @@ class BITStar:
         self.g_T = dict()
 
     def init(self):
-        print("init")
         self.Tree.V.add(self.x_start)
         self.X_sample.add(self.x_goal)
 
@@ -141,33 +139,13 @@ class BITStar:
                 self.Tree.QV = set()
 
             if k % 5 == 0:
-                self.draw(xCenter, self.g_T[self.x_goal], cMin, theta)
+                self.animation(xCenter, self.g_T[self.x_goal], cMin, theta)
 
         path_x, path_y = self.ExtractPath()
         plt.plot(path_x, path_y, linewidth=2, color='r')
         plt.pause(0.01)
-        # test
         plt.show()
 
-    def draw(self, xCenter, cMax, cMin, theta):
-        plt.cla()
-        self.plot_grid("Batch Informed Trees (BIT*)")
-
-        plt.gcf().canvas.mpl_connect(
-            'key_release_event',
-            lambda event: [exit(0) if event.key == 'escape' else None])
-
-        for v in self.X_sample:
-            plt.plot(v.x, v.y, marker='.', color='lightgrey', markersize='2')
-
-        if cMax < np.inf:
-            self.draw_ellipse(xCenter, cMax, cMin, theta)
-
-        for v, w in self.Tree.E:
-            plt.plot([v.x, w.x], [v.y, w.y], '-g')
-
-        plt.pause(0.01)
-
     def ExtractPath(self):
         node = self.x_goal
         path_x, path_y = [node.x], [node.y]
@@ -335,9 +313,23 @@ class BITStar:
         dy = node_end.y - node_start.y
         return math.hypot(dx, dy), math.atan2(dy, dx)
 
-    def animation(self, name, cBest):
-        theta, cMin, xCenter, C = self.init()
-        self.draw_ellipse(xCenter, cBest, cMin, theta)
+    def animation(self, xCenter, cMax, cMin, theta):
+        plt.cla()
+        self.plot_grid("Batch Informed Trees (BIT*)")
+
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [exit(0) if event.key == 'escape' else None])
+
+        for v in self.X_sample:
+            plt.plot(v.x, v.y, marker='.', color='lightgrey', markersize='2')
+
+        if cMax < np.inf:
+            self.draw_ellipse(xCenter, cMax, cMin, theta)
+
+        for v, w in self.Tree.E:
+            plt.plot([v.x, w.x], [v.y, w.y], '-g')
+
         plt.pause(0.001)
 
     def plot_grid(self, name):

Sampling_based_Planning/rrt_2D/informed_rrt_star.py

@@ -51,42 +51,55 @@ class IRrtStar:
         self.X_soln = set()
         self.path = None
 
+    def init(self):
+        cMin, theta = self.get_distance_and_angle(self.x_start, self.x_goal)
+        C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin)
+        xCenter = 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
+
+        return theta, cMin, xCenter, C, x_best
+
     def planning(self):
+        theta, dist, x_center, C, x_best = self.init()
         c_best = np.inf
-        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):
-            x_rand = self.Sample(self.x_start, self.x_goal, c_best, x_center, C)
+            if self.X_soln:
+                cost = {node: self.Cost(node) for node in self.X_soln}
+                x_best = min(cost, key=cost.get)
+                c_best = cost[x_best]
+
+            x_rand = self.Sample(c_best, dist, 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):
                 X_near = self.Near(self.V, x_new)
-                c_min = self.Cost(x_new)
+                c_min = self.Cost(x_nearest) + self.Line(x_nearest, x_new)
                 self.V.append(x_new)
 
+                # choose parent
                 for x_near in X_near:
                     c_new = self.Cost(x_near) + self.Line(x_near, x_new)
                     if c_new < c_min:
                         x_new.parent = x_near
                         c_min = c_new
 
+                # rewire
                 for x_near in X_near:
                     c_near = self.Cost(x_near)
-                    c_new = c_min + self.Line(x_new, x_near)
+                    c_new = self.Cost(x_new) + 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)
-                    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
+                    if not self.utils.is_collision(x_new, self.x_goal):
+                        self.X_soln.add(x_new)
+                        # 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
 
             if k % 20 == 0:
                 self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)
@@ -108,34 +121,40 @@ class IRrtStar:
 
     def Near(self, nodelist, node):
         n = len(nodelist) + 1
-        r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
+        r = 50 * math.sqrt((math.log(n) / n))
 
         dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
         X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
-                  not self.utils.is_collision(node, nodelist[ind])]
+                  not self.utils.is_collision(nodelist[ind], node)]
 
         return X_near
 
-    def Sample(self, x_start, x_goal, c_max, x_center, C):
+    def Sample(self, c_max, c_min, x_center, C):
         if c_max < np.inf:
-            c_min = self.Line(x_start, x_goal)
             r = [c_max / 2.0,
                  math.sqrt(c_max ** 2 - c_min ** 2) / 2.0,
                  math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]
             L = np.diag(r)
 
             while True:
-                x_ball = self.SampleUnitNBall()
-                x_rand = C @ L @ x_ball + x_center
+                x_ball = self.SampleUnitBall()
+                x_rand = np.dot(np.dot(C, L), x_ball) + x_center
                 if self.x_range[0] + self.delta <= x_rand[0] <= self.x_range[1] - self.delta and \
                         self.y_range[0] + self.delta <= x_rand[1] <= self.y_range[1] - self.delta:
                     break
-            x_rand = Node((x_rand[0], x_rand[1]))
+            x_rand = Node((x_rand[(0, 0)], x_rand[(1, 0)]))
         else:
             x_rand = self.SampleFreeSpace()
 
         return x_rand
 
+    @staticmethod
+    def SampleUnitBall():
+        while True:
+            x, y = random.uniform(-1, 1), random.uniform(-1, 1)
+            if x ** 2 + y ** 2 < 1:
+                return np.array([[x], [y], [0.0]])
+
     def SampleFreeSpace(self):
         delta = self.delta
 
@@ -173,14 +192,6 @@ class IRrtStar:
 
         return C
 
-    @staticmethod
-    def SampleUnitNBall():
-        while True:
-            x, y = random.uniform(-1, 1), random.uniform(-1, 1)
-
-            if x ** 2 + y ** 2 < 1:
-                return np.array([[x], [y], [0.0]])
-
     @staticmethod
     def Nearest(nodelist, n):
         return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
@@ -190,12 +201,14 @@ class IRrtStar:
     def Line(x_start, x_goal):
         return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)
 
-    @staticmethod
-    def Cost(node):
-        cost = 0.0
+    def Cost(self, node):
+        if node == self.x_start:
+            return 0.0
+
         if node.parent is None:
-            return cost
+            return np.inf
 
+        cost = 0.0
         while node.parent:
             cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
             node = node.parent
@@ -215,13 +228,13 @@ class IRrtStar:
             '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")
 
+        if c_best != np.inf:
+            self.draw_ellipse(x_center, c_best, dist, theta)
+
         plt.pause(0.01)
 
     def plot_grid(self, name):
@@ -284,7 +297,7 @@ def main():
     x_start = (18, 8)  # Starting node
     x_goal = (37, 18)  # Goal node
 
-    rrt_star = IRrtStar(x_start, x_goal, 10, 0.10, 20, 1000)
+    rrt_star = IRrtStar(x_start, x_goal, 1, 0.10, 12, 1000)
     rrt_star.planning()