update

CurvesGenerator/cubic_spline.py

@@ -28,10 +28,10 @@ class Spline:
         self.nx = len(x)  # dimension of x
         h = np.diff(x)
 
-        # calc coefficient c
+        # calc coefficient cBest
         self.a = [iy for iy in y]
 
-        # calc coefficient c
+        # calc coefficient cBest
         A = self.__calc_A(h)
         B = self.__calc_B(h)
         self.c = np.linalg.solve(A, B)
@@ -104,7 +104,7 @@ class Spline:
 
     def __calc_A(self, h):
         u"""
-        calc matrix A for spline coefficient c
+        calc matrix A for spline coefficient cBest
         """
         A = np.zeros((self.nx, self.nx))
         A[0, 0] = 1.0
@@ -122,7 +122,7 @@ class Spline:
 
     def __calc_B(self, h):
         u"""
-        calc matrix B for spline coefficient c
+        calc matrix B for spline coefficient cBest
         """
         B = np.zeros(self.nx)
         for i in range(self.nx - 2):

Sampling_based_Planning/rrt_2D/batch_informed_trees.py

@@ -6,11 +6,12 @@ Batch Informed Trees (BIT*)
 import os
 import sys
 import math
-import random
 import copy
+import random
 import numpy as np
 import matplotlib.pyplot as plt
 import matplotlib.patches as patches
+from scipy.spatial.transform import Rotation as Rot
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Sampling_based_Planning/")
@@ -63,15 +64,14 @@ class BITStar:
         self.Tree = Tree(self.x_start, self.x_goal)
         self.X_sample = set()
         self.g_T = dict()
-        self.f_T = dict()
 
     def init(self):
+        print("init")
         self.Tree.V.add(self.x_start)
         self.X_sample.add(self.x_goal)
-        self.g_T[self.x_goal] = np.inf
-        self.f_T[self.x_goal] = 0.0
+
         self.g_T[self.x_start] = 0.0
-        self.f_T[self.x_start] = self.f_estimated(self.x_start)
+        self.g_T[self.x_goal] = np.inf
 
         cMin, theta = self.calc_dist_and_angle(self.x_start, self.x_goal)
         C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin)
@@ -81,12 +81,12 @@ class BITStar:
         return theta, cMin, xCenter, C
 
     def planning(self):
-        eTheta, cMin, xCenter, C = self.init()
+        m = 200
+        theta, cMin, xCenter, C = self.init()
 
         for k in range(self.iter_max):
             if not self.Tree.QE and not self.Tree.QV:
                 self.Prune(self.g_T[self.x_goal])
-                m = 200
                 self.X_sample.update(self.Sample(m, self.g_T[self.x_goal], cMin, xCenter, C))
                 self.Tree.V_old = copy.deepcopy(self.Tree.V)
                 self.Tree.QV = copy.deepcopy(self.Tree.V)
@@ -99,13 +99,14 @@ class BITStar:
             self.Tree.QE.remove((vm, xm))
 
             if self.g_T[vm] + self.calc_dist(vm, xm) + self.h_estimated(xm) < self.g_T[self.x_goal]:
-                if self.g_estimated(vm) + self.cost(vm, xm) + self.h_estimated(xm) < self.g_T[self.x_goal]:
-                    if self.g_T[vm] + self.cost(vm, xm) < self.g_T[xm]:
+                actual_cost = self.cost(vm, xm)
+                if self.g_estimated(vm) + actual_cost + self.h_estimated(xm) < self.g_T[self.x_goal]:
+                    if self.g_T[vm] + actual_cost < self.g_T[xm]:
                         if xm in self.Tree.V:
                             # remove edges
-                            for vl, vr in self.Tree.E:
-                                if vl == xm or vr == xm:
-                                    self.Tree.E.remove((vl, vr))
+                            for v, x in self.Tree.E:
+                                if x == xm:
+                                    self.Tree.E.remove((v, x))
                         else:
                             self.X_sample.remove(xm)
                             self.Tree.V.add(xm)
@@ -113,64 +114,41 @@ class BITStar:
 
                         self.Tree.E.add((vm, xm))
 
-    def ExpandVertex(self, v):
-        self.Tree.QV.remove(v)
-        X_near = {x for x in self.X_sample if self.calc_dist(x, v) <= self.Tree.r}
-
-        for x in X_near:
-            if self.g_estimated(v) + self.calc_dist(v, x) + self.h_estimated(x) < self.g_T[self.x_goal]:
-                self.Tree.QE.add((v, x))
-
-        if v not in self.Tree.V_old:
-            V_near = {w for w in self.Tree.V if self.calc_dist(w, v) <= self.Tree.r}
-
-            for w in V_near:
-                if (v, w) not in self.Tree.E and (w, v) not in self.Tree.E and \
-                        self.g_estimated(v) + self.calc_dist(v, w) + self.h_estimated(w) < self.g_T[self.x_goal] and \
-                        self.g_T[v] + self.calc_dist(v, w) < self.g_T[w]:
-                    self.Tree.QE.add((v, w))
-
-    def BestVertexQueueValue(self):
-        if not self.Tree.QV:
-            return np.inf
+                        for v, x in self.Tree.QE:
+                            if x == xm and self.g_T[v] + self.calc_dist(v, xm) >= self.g_T[xm]:
+                                self.Tree.QE.remove((v, xm))
+            else:
+                self.Tree.QE = set()
+                self.Tree.QV = set()
 
-        return min(self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV)
+    def Prune(self, cBest):
+        self.X_sample = {x for x in self.X_sample if self.f_estimated(x) < cBest}
+        self.Tree.V = {v for v in self.Tree.V if self.f_estimated(v) <= cBest}
+        self.Tree.E = {(v, w) for v, w in self.Tree.E
+                       if self.f_estimated(v) <= cBest and self.f_estimated(w) <= cBest}
+        self.X_sample.update({v for v in self.Tree.V if self.g_T[v] == np.inf})
+        self.Tree.V = {v for v in self.Tree.V if self.g_T[v] < np.inf}
 
-    def BestEdgeQueueValue(self):
-        if not self.Tree.QE:
+    def cost(self, start, end):
+        if self.utils.is_collision(start, end):
             return np.inf
 
-        return min(self.g_T[el] + self.calc_dist(el, er) + self.h_estimated(er)
-                   for el, er in self.Tree.QE)
-
-    def BestInVertexQueue(self):
-        v_value = {v: self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV}
-
-        return min(v_value, key=v_value.get)
-
-    def BestInEdgeQueue(self):
-        e_value = {(el, er): self.g_T[el] + self.calc_dist(el, er) + self.h_estimated(er)
-                   for el, er in self.Tree.QE}
-
-        return min(e_value, key=e_value.get)
+        return self.calc_dist(start, end)
 
-    def radius(self, q):
-        lambda_X = 0
-        sigma = math.pi ** 2
+    def f_estimated(self, node):
+        return self.g_estimated(node) + self.h_estimated(node)
 
-        for x in self.Tree.V:
-            if self.f_estimated(x) <= self.g_T[self.x_goal]:
-                lambda_X += 1
+    def g_estimated(self, node):
+        return self.calc_dist(self.x_start, node)
 
-        return 2 * self.eta * 1.5 ** 0.5 * (lambda_X / sigma * math.log(q) / q) ** 0.5
+    def h_estimated(self, node):
+        return self.calc_dist(node, self.x_goal)
 
     def Sample(self, m, cMax, cMin, xCenter, C):
         if cMax < np.inf:
-            Sample = self.SampleEllipsoid(m, cMax, cMin, xCenter, C)
+            return self.SampleEllipsoid(m, cMax, cMin, xCenter, C)
         else:
-            Sample = self.SampleFreeSpace(m)
-
-        return Sample
+            return self.SampleFreeSpace(m)
 
     def SampleEllipsoid(self, m, cMax, cMin, xCenter, C):
         r = [cMax / 2.0,
@@ -184,13 +162,13 @@ class BITStar:
 
         while ind < m:
             xBall = self.SampleUnitNBall()
-            x_rand = C @ L @ xBall + xCenter
-            node = Node(x_rand[0], x_rand[1])
-            not_in_obs = ~self.utils.is_inside_obs(node)
+            x_rand = np.dot(np.dot(C, L), xBall) + xCenter
+            node = Node(x_rand[(0, 0)], x_rand[(1, 0)])
+            in_obs = self.utils.is_inside_obs(node)
             in_x_range = self.x_range[0] + delta <= node.x <= self.x_range[1] - delta
             in_y_range = self.y_range[0] + delta <= node.y <= self.y_range[1] - delta
 
-            if not_in_obs and in_x_range and in_y_range:
+            if not in_obs and in_x_range and in_y_range:
                 Sample.add(node)
                 ind += 1
 
@@ -202,8 +180,8 @@ class BITStar:
 
         ind = 0
         while ind < m:
-            node = Node((random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
-                         random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+            node = Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                        random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))
             if self.utils.is_inside_obs(node):
                 continue
             else:
@@ -212,53 +190,61 @@ class BITStar:
 
         return Sample
 
-    @staticmethod
-    def SampleUnitNBall():
-        theta, r = random.uniform(0.0, 2 * math.pi), random.random()
-        x = r * math.cos(theta)
-        y = r * math.sin(theta)
-
-        return np.array([[x], [y], [0.0]])
+    def radius(self, q):
+        cBest = self.g_T[self.x_goal]
+        lambda_X = len([1 for v in self.Tree.V if self.f_estimated(v) <= cBest])
+        radius = 2 * self.eta * (1.5 * lambda_X / math.pi * math.log(q) / q) ** 0.5
 
-    def Prune(self, c):
-        for x in self.X_sample:
-            if self.f_estimated(x) >= c:
-                self.X_sample.remove(x)
+        return radius
 
-        for v in self.Tree.V:
-            if self.f_estimated(v) > c:
-                self.Tree.V.remove(v)
+    def ExpandVertex(self, v):
+        self.Tree.QV.remove(v)
+        X_near = {x for x in self.X_sample if self.calc_dist(x, v) <= self.Tree.r}
+        edges_add = {(v, x) for x in X_near
+                     if self.g_estimated(v) + self.calc_dist(v, x) + self.h_estimated(x) < self.g_T[self.x_goal]}
+        self.Tree.QE.update(edges_add)
 
-        for v, w in self.Tree.E:
-            if self.f_estimated(v) > c or self.f_estimated(w) > c:
-                self.Tree.E.remove((v, w))
+        if v not in self.Tree.V_old:
+            V_near = {w for w in self.Tree.V if self.calc_dist(w, v) <= self.Tree.r}
+            edges_add = {(v, w) for w in V_near if (v, w) not in self.Tree.E and
+                         self.g_estimated(v) + self.calc_dist(v, w) + self.h_estimated(w) < self.g_T[self.x_goal] and
+                         self.g_T[v] + self.calc_dist(v, w) < self.g_T[w]}
+            self.Tree.QE.update(edges_add)
 
-        for v in self.Tree.V:
-            if v.g_T == np.inf:
-                self.X_sample.add(v)
+    def BestVertexQueueValue(self):
+        if not self.Tree.QV:
+            return np.inf
 
-        for v in self.Tree.V:
-            if v.g_T == np.inf:
-                self.Tree.V.remove(v)
+        return min(self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV)
 
-    def cost(self, start, end):
-        if self.utils.is_collision(start, end):
+    def BestEdgeQueueValue(self):
+        if not self.Tree.QE:
             return np.inf
 
-        return self.calc_dist(start, end)
+        return min(self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x)
+                   for v, x in self.Tree.QE)
 
-    def f_estimated(self, node):
-        return self.g_estimated(node) + self.h_estimated(node)
+    def BestInVertexQueue(self):
+        v_value = {v: self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV}
 
-    def g_estimated(self, node):
-        return self.calc_dist(self.x_start, node)
+        return min(v_value, key=v_value.get)
 
-    def h_estimated(self, node):
-        return self.calc_dist(node, self.x_goal)
+    def BestInEdgeQueue(self):
+        e_value = {(v, x): self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x)
+                   for v, x in self.Tree.QE}
+
+        return min(e_value, key=e_value.get)
+
+    @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 RotationToWorldFrame(x_start, x_goal, L):
-        a1 = np.array([[(x_start.x - x_start.x) / L],
+        a1 = np.array([[(x_goal.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
@@ -276,3 +262,83 @@ class BITStar:
         dx = node_end.x - node_start.x
         dy = node_end.y - node_start.y
         return math.hypot(dx, dy), math.atan2(dy, dx)
+
+    def animation(self, name):
+        theta, cMin, xCenter, C = self.init()
+        cBest = 30
+        self.plot_grid(name)
+        sample = self.Sample(300, cBest, cMin, xCenter, C)
+        for node in sample:
+            plt.plot(node.x, node.y, marker='.', color='lightgrey')
+        self.draw_ellipse(xCenter, cBest, cMin, theta)
+        plt.pause(0.001)
+        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.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, marker='.', color='darkorange')
+        plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2)
+
+
+def main():
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
+    eta = 2
+    iter_max = 200
+    print("start!!!")
+    bit = BITStar(x_start, x_goal, eta, iter_max)
+    # bit.planning()
+    bit.animation("Batch Informed Trees (BIT*)")
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/rrt_2D/informed_rrt_star.py

@@ -164,7 +164,7 @@ class IRrtStar:
 
     @staticmethod
     def RotationToWorldFrame(x_start, x_goal, L):
-        a1 = np.array([[(x_start.x - x_start.x) / L],
+        a1 = np.array([[(x_goal.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
@@ -175,11 +175,11 @@ class IRrtStar:
 
     @staticmethod
     def SampleUnitNBall():
-        theta, r = random.uniform(0.0, 2 * math.pi), random.random()
-        x = r * math.cos(theta)
-        y = r * math.sin(theta)
+        while True:
+            x, y = random.uniform(-1, 1), random.uniform(-1, 1)
 
-        return np.array([[x], [y], [0.0]])
+            if x ** 2 + y ** 2 < 1:
+                return np.array([[x], [y], [0.0]])
 
     @staticmethod
     def Nearest(nodelist, n):
@@ -284,7 +284,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, 10000)
+    rrt_star = IRrtStar(x_start, x_goal, 10, 0.10, 20, 1000)
     rrt_star.planning()
 
 

Search_based_Planning/Search_3D/Anytime_Dstar3D.py

@@ -209,7 +209,7 @@ class Anytime_Dstar(object):
         '''After ComputeShortestPath()
         returns, one can then follow a shortest path from x_init to
         x_goal by always moving from the current vertex s, starting
-        at x_init. , to any successor s' that minimizes c(s,s') + g(s')
+        at x_init. , to any successor s' that minimizes cBest(s,s') + g(s')
         until x_goal is reached (ties can be broken arbitrarily).'''
         path = []
         s_goal = self.xt

Search_based_Planning/Search_3D/DstarLite3D.py

@@ -165,7 +165,7 @@ class D_star_Lite(object):
             self.env.start = self.x0
             # ---------------------------------- if any Cost changed, update km, reset slast,
             #                                    for all directed edgees (u,v) with  chaged edge costs, 
-            #                                    update the edge Cost c(u,v) and update vertex u. then replan
+            #                                    update the edge Cost cBest(u,v) and update vertex u. then replan
             if ischanged:
                 self.km += heuristic_fun(self, self.x0, s_last)
                 s_last = self.x0
@@ -188,7 +188,7 @@ class D_star_Lite(object):
         '''After ComputeShortestPath()
         returns, one can then follow a shortest path from x_init to
         x_goal by always moving from the current vertex s, starting
-        at x_init. , to any successor s' that minimizes c(s,s') + g(s')
+        at x_init. , to any successor s' that minimizes cBest(s,s') + g(s')
         until x_goal is reached (ties can be broken arbitrarily).'''
         path = []
         s_goal = self.xt

Search_based_Planning/Search_3D/LRT_Astar3D.py

@@ -36,7 +36,7 @@ class LRT_A_star2:
                 # update h values if they are smaller
                 Children = children(self.Astar,xi)
                 minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children])
-                # h(s) = h(s') if h(s) > c(s,s') + h(s') 
+                # h(s) = h(s') if h(s) > cBest(s,s') + h(s') 
                 if self.Astar.h[xi] >= minfval:
                     self.Astar.h[xi] = minfval
                 hvals.append(self.Astar.h[xi])