LRTA*

Search-based Planning/Search_3D/Astar3D.py

@@ -35,6 +35,7 @@ class Weighted_A_star(object):
         self.V = []
         self.done = False
         self.Path = []
+        self.ind = 0
 
     def children(self,x):
         allchild = []
@@ -44,10 +45,9 @@ class Weighted_A_star(object):
                 allchild.append(child)
         return allchild
 
-    def run(self):
+    def run(self, N=None):
         x0, xt = hash3D(self.start), hash3D(self.goal)
         self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # item, priority = g + h
-        self.ind = 0
         while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty
             strxi = self.OPEN.get()           
             xi = dehash(strxi)
@@ -69,12 +69,18 @@ class Weighted_A_star(object):
                         else:
                             # add xj in to OPEN set
                             self.OPEN.put(strxj, a+1*self.h[strxj])
+            # For specified expanded nodes, used primarily in LRTA*
+            if N is not None:
+                if len(self.V) % N == 0:
+                    break
             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
             self.ind += 1
-        self.done = True
-        self.Path = self.path()
-        visualization(self)
-        plt.show()
+        # if the path finding is finished
+        if xt in self.CLOSED:
+            self.done = True
+            self.Path = self.path()
+            visualization(self)
+            plt.show()
 
     def path(self):
         path = []
@@ -87,5 +93,5 @@ class Weighted_A_star(object):
         return path
 
 if __name__ == '__main__':
-    Astar = Weighted_A_star(0.5)
+    Astar = Weighted_A_star(1)
     Astar.run()

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -1,4 +1,4 @@
-# this is the three dimensional LRTA* algo
+# this is the three dimensional near-sighted 1 neighborhood LRTA* algo
 # !/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
@@ -12,84 +12,110 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
+from Search_3D.Astar3D import Weighted_A_star
 from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
 from Search_3D.plot_util3D import visualization
 import queue
 
 
-class LRT_A_star(object):
-    def __init__(self,resolution=0.5):
-        self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
-                      [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
-                      [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
-                      [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
-        self.env = env(resolution = resolution)
-        self.Space = StateSpace(self)
-        self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
-        self.AABB = getAABB(self.env.blocks)
-        self.Space[hash3D(getNearest(self.Space,self.start))] = 0 
-        self.OPEN = queue.QueuePrior() 
-        self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0
-        self.Child = {}
-        self.CLOSED = set()
-        self.V = []
-        self.done = False
-        self.Path = []
+# class LRT_A_star1(object):
+#     def __init__(self,resolution=0.5):
+#         self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
+#                       [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
+#                       [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
+#                       [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
+#         self.env = env(resolution = resolution)
+#         self.Space = StateSpace(self)
+#         self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
+#         self.AABB = getAABB(self.env.blocks)
+#         self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # this is g
+#         self.OPEN = queue.QueuePrior() 
+#         self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0
+#         self.Child = {}
+#         self.CLOSED = set()
+#         self.V = []
+#         self.done = False
+#         self.Path = []
 
-    def children(self,x):
-        allchild = []
-        for j in self.Alldirec:
-            collide,child = isCollide(self,x,j)
-            if not collide:
-                allchild.append(child)
-        return allchild
+#     def children(self,x):
+#         allchild = []
+#         for j in self.Alldirec:
+#             collide,child = isCollide(self,x,j)
+#             if not collide:
+#                 allchild.append(child)
+#         return allchild
 
-    def step(self, xi, strxi):
-        childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1}
-        fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs]
-        xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s'))
-        strxj = hash3D(xj)
-        # add the child of xi
-        self.Child[strxi] = xj
-        if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update  h(s) = h'(s)
-            self.h[strxi] = fmin 
-        # TODO: action to move to xj
-        self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s'))
+#     def step(self, xi, strxi):
+#         childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1}
+#         fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs]
+#         xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s'))
+#         strxj = hash3D(xj)
+#         # add the child of xi
+#         self.Child[strxi] = xj
+#         if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update  h(s) = h'(s)
+#             self.h[strxi] = fmin 
+#         # TODO: action to move to xj
+#         self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s'))
+
+#     def run(self):
+#         x0 = hash3D(self.start)
+#         xt = hash3D(self.goal)
+#         self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state
+#         self.ind = 0
+#         while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do
+#             strxi = self.OPEN.get()           
+#             xi = dehash(strxi) 
+#             self.CLOSED.add(strxi) 
+#             self.V.append(xi)
+#             visualization(self)
+#             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
+#             self.ind += 1
+#         self.done = True
+#         self.Path = self.path()
+#         visualization(self)
+#         plt.show()
+
+#     def path(self):
+#         # this is a suboptimal path. 
+#         path = []
+#         strgoal = hash3D(self.goal)
+#         strx = hash3D(self.start)
+#         ind = 0
+#         while strx != strgoal:
+#             path.append([dehash(strx),self.Child[strx]])
+#             strx = hash3D(self.Child[strx])
+#             ind += 1
+#             if ind == 1000:
+#                 return np.flip(path,axis=0)
+#         path = np.flip(path,axis=0)
+#         return path
+
+class LRT_A_star2():
+    def __init__(self,resolution=0.5, N=7):
+        self.lookahead = N
+        self.Astar = Weighted_A_star()
+        self.Astar.env.resolution = resolution
+    
+    def expand(self):
+        self.Astar.run(self.lookahead) 
+
+    def updateHeuristic(self):
+        for strxi in self.Astar.CLOSED:
+            self.Astar.h[strxi] = np.inf
+            xi = dehash(strxi)
+            self.Astar.h[strxi] = min([cost(xi,xj) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)])
+    
+    #def move(self):
+    #    print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()]))
+        
 
     def run(self):
-        x0 = hash3D(self.start)
-        xt = hash3D(self.goal)
-        self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state
-        self.ind = 0
-        while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do
-            strxi = self.OPEN.get()           
-            xi = dehash(strxi) 
-            self.CLOSED.add(strxi) 
-            self.V.append(xi)
-            visualization(self)
-            self.step(xi , strxi)
-            if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
-            self.ind += 1
-        self.done = True
-        self.Path = self.path()
-        visualization(self)
-        plt.show()
+        xt = hash3D(self.Astar.goal)
+        while xt not in self.Astar.CLOSED:
+            self.expand()
+            #self.updateHeuristic()
 
-    def path(self):
-        # this is a suboptimal path. 
-        path = []
-        strgoal = hash3D(self.goal)
-        strx = hash3D(self.start)
-        ind = 0
-        while strx != strgoal:
-            path.append([dehash(strx),self.Child[strx]])
-            strx = hash3D(self.Child[strx])
-            ind += 1
-            if ind == 1000:
-                return np.flip(path,axis=0)
-        path = np.flip(path,axis=0)
-        return path
 
 if __name__ == '__main__':
-    Astar = LRT_A_star(0.5)
-    Astar.run()
+    T = LRT_A_star2(resolution = 1, N = 2)
+    T.run()