'update'

Search-based Planning/Search_3D/Astar3D.py

@@ -13,27 +13,26 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
-    cost, children, StateSpace
+    cost, children, StateSpace, heuristic_fun
 from Search_3D.plot_util3D import visualization
 import queue
 import time
 
 class Weighted_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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
 
         self.env = env(resolution=resolution)
-        self.X = StateSpace(self.env)
-        self.g = g_Space(self)  # key is the point, store g value
-        self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
-        # self.AABB = getAABB(self.env.blocks)
-        self.g[getNearest(self.g, self.start)] = 0  # set g(x0) = 0
-
-        self.h = Heuristic(self.g, self.goal)
+        self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
+        self.g = {self.start:0,self.goal:np.inf}
         self.Parent = {}
         self.CLOSED = set()
         self.V = []
@@ -42,40 +41,36 @@ class Weighted_A_star(object):
         self.ind = 0
         self.x0, self.xt = self.start, self.goal
         self.OPEN = queue.QueuePrior()  # store [point,priority]
-        self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0])  # item, priority = g + h
+        self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0))  # item, priority = g + h
         self.lastpoint = self.x0
 
-    # 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 run(self, N=None):
         xt = self.xt
         xi = self.x0
-        while xt not in self.CLOSED and self.OPEN:  # while xt not reached and open is not empty
+        while self.OPEN:  # while xt not reached and open is not empty
             xi = self.OPEN.get()
             if xi not in self.CLOSED:
                 self.V.append(np.array(xi))
             self.CLOSED.add(xi)  # add the point in CLOSED set
-            visualization(self)
-            allchild = children(self,xi)
-            for xj in allchild:
-                if xj not in self.CLOSED:
-                    gi, gj = self.g[xi], self.g[xj]
-                    a = gi + cost(self, xi, xj)
-                    if a < gj:
-                        self.g[xj] = a
-                        self.Parent[xj] = xi
-                        if (a, xj) in self.OPEN.enumerate():
-                            # update priority of xj
-                            self.OPEN.put(xj, a + 1 * self.h[xj])
-                        else:
-                            # add xj in to OPEN set
-                            self.OPEN.put(xj, a + 1 * self.h[xj])
+            if xi == xt:
+                break
+            # visualization(self)
+            for xj in children(self,xi):
+                # if xj not in self.CLOSED:
+                if xj not in self.g:
+                    self.g[xj] = np.inf
+                else:
+                    pass
+                a = self.g[xi] + cost(self, xi, xj)
+                if a < self.g[xj]:
+                    self.g[xj] = a
+                    self.Parent[xj] = xi
+                    # if (a, xj) in self.OPEN.enumerate():
+                        # update priority of xj
+                    self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj))
+                    # else:
+                        # add xj in to OPEN set
+                    # self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj))
             # For specified expanded nodes, used primarily in LRTA*
             if N:
                 if len(self.CLOSED) % N == 0:
@@ -111,7 +106,7 @@ class Weighted_A_star(object):
         self.start = xj
         self.g[getNearest(self.g, self.start)] = 0  # set g(x0) = 0
         self.x0 = xj
-        self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0])  # item, priority = g + h
+        self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0))  # item, priority = g + h
         self.CLOSED = set()
 
         # self.h = h(self.Space, self.goal)

Search-based Planning/Search_3D/Dstar3D.py

@@ -15,11 +15,15 @@ from Search_3D.plot_util3D import visualization
 
 class D_star(object):
     def __init__(self, resolution=1):
-        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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         self.env = env(resolution=resolution)
         self.X = StateSpace(self.env)
         self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
@@ -133,7 +137,6 @@ class D_star(object):
             self.insert(x, self.h[xparent] + cost(self, x, xparent))
     def modify(self, x):
         self.modify_cost(x)
-        self.V = set()
         while True:
             kmin = self.process_state()
             # visualization(self)
@@ -175,6 +178,7 @@ class D_star(object):
             self.env.move_block(a=[0, 0, -0.25], s=0.5, block_to_move=0, mode='translation')
             # travel from end to start
             s = tuple(self.env.start)
+            self.V = set()
             while s != self.xt:
                 if s == tuple(self.env.start):
                     sparent = self.b[self.x0]

Search-based Planning/Search_3D/LP_Astar3D.py

@@ -17,11 +17,15 @@ import time
 
 class Lifelong_Astar(object):
     def __init__(self,resolution = 1):
-        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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         self.env = env(resolution=resolution)
         self.g = g_Space(self)
         self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
@@ -177,7 +181,7 @@ class Lifelong_Astar(object):
 
 if __name__ == '__main__':
     sta = time.time()
-    Astar = Lifelong_Astar(0.5)
+    Astar = Lifelong_Astar(1)
     Astar.ComputePath()
     Astar.change_env()
     Astar.ComputePath()

Search-based Planning/Search_3D/bidirectional_Astar3D.py

@@ -13,26 +13,27 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
-from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun
 from Search_3D.plot_util3D import visualization
 import queue
 
 
 class Weighted_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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         self.env = env(resolution = resolution)
-        self.g = g_Space(self) # key is the point, store g value
-        self.start, self.goal = getNearest(self.g,self.env.start), getNearest(self.g,self.env.goal)
-        self.g[self.start] = 0 # set g(x0) = 0
-        self.g[self.goal] = 0 # set g(x0) = 0
+        self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
+        self.g = {self.start:0,self.goal:0}
         self.OPEN1 = queue.QueuePrior() # store [point,priority]
         self.OPEN2 = queue.QueuePrior()
-        self.h1 = Heuristic(self.g,self.goal) # tree NO.1
-        self.h2 = Heuristic(self.g,self.start) # tree NO.2
         self.Parent1, self.Parent2 = {}, {}
         self.CLOSED1, self.CLOSED2 = set(), set()
         self.V = []
@@ -41,8 +42,8 @@ class Weighted_A_star(object):
 
     def run(self):
         x0, xt = self.start, self.goal
-        self.OPEN1.put(x0, self.g[x0] + self.h1[x0]) # item, priority = g + h
-        self.OPEN2.put(xt, self.g[xt] + self.h2[xt]) # item, priority = g + h
+        self.OPEN1.put(x0, self.g[x0] + heuristic_fun(self,x0,xt)) # item, priority = g + h
+        self.OPEN2.put(xt, self.g[xt] + heuristic_fun(self,xt,x0)) # item, priority = g + h
         self.ind = 0
         while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty
             xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() 
@@ -50,7 +51,7 @@ class Weighted_A_star(object):
             self.CLOSED2.add(xi2)
             self.V.append(xi1)
             self.V.append(xi2)
-            visualization(self)
+            # visualization(self)
             allchild1,  allchild2 = children(self,xi1), children(self,xi2)
             self.evaluation(allchild1,xi1,conf=1)
             self.evaluation(allchild2,xi2,conf=2)
@@ -66,26 +67,34 @@ class Weighted_A_star(object):
         for xj in allchild:
             if conf == 1:
                 if xj not in self.CLOSED1:
-                    gi, gj = self.g[xi], self.g[xj]
+                    if xj not in self.g:
+                        self.g[xj] = np.inf
+                    else:
+                        pass
+                    gi = self.g[xi]
                     a = gi + cost(self,xi,xj)
-                    if a < gj:
+                    if a < self.g[xj]:
                         self.g[xj] = a
                         self.Parent1[xj] = xi
                         if (a, xj) in self.OPEN1.enumerate():
-                            self.OPEN1.put(xj, a+1*self.h1[xj])
+                            self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))
                         else:
-                            self.OPEN1.put(xj, a+1*self.h1[xj])
+                            self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))
             if conf == 2:
                 if xj not in self.CLOSED2:
-                    gi, gj = self.g[xi], self.g[xj]
+                    if xj not in self.g:
+                        self.g[xj] = np.inf
+                    else:
+                        pass
+                    gi = self.g[xi]
                     a = gi + cost(self,xi,xj)
-                    if a < gj:
+                    if a < self.g[xj]:
                         self.g[xj] = a
                         self.Parent2[xj] = xi
                         if (a, xj) in self.OPEN2.enumerate():
-                            self.OPEN2.put(xj, a+1*self.h2[xj])
+                            self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
                         else:
-                            self.OPEN2.put(xj, a+1*self.h2[xj])
+                            self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
             
     def path(self):
         # TODO: fix path

Search-based Planning/Search_3D/utils3D.py

@@ -30,9 +30,13 @@ def Heuristic(Space, t):
     '''Max norm distance'''
     h = {}
     for k in Space.keys():
-        h[k] = max(abs(np.array([t[0] - k[0], t[1] - k[1], t[2] - k[2]])))
+        h[k] = max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
     return h
 
+def heuristic_fun(initparams, k, t=None):
+    if t is None:
+        t = initparams.goal
+    return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
 
 def isinbound(i, x):
     if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
@@ -117,9 +121,11 @@ def g_Space(initparams):
     return g
 
 
-def isCollide(initparams, x, child):
+def isCollide(initparams, x, child, dist):
     '''see if line intersects obstacle'''
-    dist = getDist(x, child)
+    '''specified for expansion in A* 3D lookup table'''
+    if dist==None:
+        dist = getDist(x, child)
     if not isinbound(initparams.env.boundary, child): 
         return True, dist
     for i in range(len(initparams.env.AABB)):
@@ -147,6 +153,7 @@ def children(initparams, x):
             continue
         if isinbound(initparams.env.boundary, child):
             allchild.append(child)
+            # initparams.Alldirec[direc]*resolution
     return allchild
 
 
@@ -160,8 +167,9 @@ def obstacleFree(initparams, x):
     return True
 
 
-def cost(initparams, i, j, settings=0):
-    collide, dist = isCollide(initparams, i, j)
+def cost(initparams, i, j, dist=None, settings=0):
+    collide, dist = isCollide(initparams, i, j, dist)
+    # collide, dist= False, getDist(i, j)
     if settings == 0:
         if collide:
             return np.inf