diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index c83db3a..ac341d9 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/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) \ No newline at end of file + Astar = Weighted_A_star(0.5) + Astar.run() \ No newline at end of file diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index 637b3cd..c63c4fd 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index 8a0c0df..7010ce9 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/plot_util3D.py b/Search-based Planning/Search_3D/plot_util3D.py index 0da691c..3b4a1e3 100644 --- a/Search-based Planning/Search_3D/plot_util3D.py +++ b/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) diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index d34aaa2..dc27f90 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/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 \ No newline at end of file