A*3D complete

Search-based Planning/Search_3D/Astar3D.py

@@ -5,6 +5,7 @@
 @author: yue qi
 """
 import numpy as np
+import matplotlib.pyplot as plt
 
 import os
 import sys
@@ -17,15 +18,15 @@ import queue
 
 
 class Weighted_A_star(object):
-    def __init__(self,resolution=0.2):
+    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) # key is the point, store g value
-        self.start = getNearest(self.Space,self.env.start)
-        self.goal = getNearest(self.Space,self.env.goal)
+        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 # set g(x0) = 0
         self.OPEN = queue.QueuePrior() # store [point,priority]
         self.h = Heuristic(self.Space,self.goal)
@@ -44,9 +45,8 @@ class Weighted_A_star(object):
         return allchild
 
     def run(self):
-        x0 = hash3D(self.start)
-        xt = hash3D(self.goal)
-        self.OPEN.put(x0,self.Space[x0] + self.h[x0]) # item, priority = g + h
+        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()           
@@ -58,37 +58,35 @@ class Weighted_A_star(object):
             for xj in allchild:
                 strxj = hash3D(xj)
                 if strxj not in self.CLOSED:
-                    gi,gj = self.Space[strxi], self.Space[strxj]
+                    gi, gj = self.Space[strxi], self.Space[strxj]
                     a = gi + cost(xi,xj)
                     if a < gj:
                         self.Space[strxj] = a
                         self.Parent[strxj] = xi
-                        if strxj in self.OPEN.enumerate():
-                            #TODO: update priority of xj
-                            # self.OPEN.put(strxj, a+1*self.h[strxj])
+                        if (a, strxj) in self.OPEN.enumerate():
+                            # update priority of xj
+                            self.OPEN.put(strxj, a+1*self.h[strxj])
                             pass
                         else:
-                            #TODO: add xj in to OPEN set
+                            # add xj in to OPEN set
                             self.OPEN.put(strxj, a+1*self.h[strxj])
             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
             self.ind += 1
         self.done = True
-        #self.Path = self.path()
-        #visualization(self)
+        self.Path = self.path()
+        visualization(self)
+        plt.show()
 
     def path(self):
-        path = [self.goal]
+        path = []
         strx = hash3D(self.goal)
         strstart = hash3D(self.start)
         while strx != strstart:
-            path.append(self.Parent[strx])
+            path.append([dehash(strx),self.Parent[strx]])
             strx = hash3D(self.Parent[strx])
-        path = np.array(np.flip(path,axis=0))
+        path = np.flip(path,axis=0)
         return path
 
-
 if __name__ == '__main__':
-    Astar = Weighted_A_star(1)
-    Astar.run()
-    PATH = Astar.path()
-    print(PATH)
+    Astar = Weighted_A_star(0.5)
+    Astar.run()

Search-based Planning/Search_3D/plot_util3D.py

@@ -52,7 +52,7 @@ def draw_line(ax,SET,visibility=1,color=None):
             ax.add_line(line)
 
 def visualization(initparams):
-    if initparams.ind % 10 == 0 or initparams.done:
+    if initparams.ind % 20 == 0 or initparams.done:
         V = np.array(initparams.V)
         # E = initparams.E
         Path = np.array(initparams.Path)
@@ -61,7 +61,7 @@ def visualization(initparams):
         # edges = E.get_edge()
         # generate axis objects
         ax = plt.subplot(111, projection='3d')
-        ax.view_init(elev=0., azim=90)
+        ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
         ax.clear()
         # drawing objects
         draw_Spheres(ax, initparams.env.balls)

Search-based Planning/Search_3D/utils3D.py

@@ -64,11 +64,10 @@ def isCollide(initparams, x, direc):
     '''see if line intersects obstacle'''
     resolution = initparams.env.resolution
     child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
-    ray = getRay(x, direc)
-    dist = getDist(x, child)
+    ray , dist = getRay(x, child) ,  getDist(x, child)
     if not isinbound(initparams.env.boundary,child):
         return True, child
-    for i in getAABB(initparams.env.blocks):
+    for i in initparams.AABB:
         shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
         if shot is not None:
             dist_wall = getDist(x, shot)
@@ -85,6 +84,5 @@ def isCollide(initparams, x, direc):
 def cost(i,j):
     return getDist(i,j)
     
-
 if __name__ == "__main__":
     from env3D import env