From 1b5271fdf0335ae0a46d4c5e488f1effd7c81581 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sat, 27 Jun 2020 17:16:40 -0700 Subject: [PATCH] A*3D complete --- Search-based Planning/Search_3D/Astar3D.py | 40 +++++++++--------- .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4729 -> 4783 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 3692 -> 3683 bytes .../Search_3D/plot_util3D.py | 4 +- Search-based Planning/Search_3D/utils3D.py | 6 +-- 5 files changed, 23 insertions(+), 27 deletions(-) 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 637b3cd172846ef2e614d4fb4ea2e33eaa907ca3..c63c4fddcc6b0128ac91bb4eaaa56cd279573f5b 100644 GIT binary patch delta 1116 zcmZvaPiP}m9LL{#$xJ8HWRj+7+O%31+!p5ILRS`h*{xa;UA8K#s|;+zByZZCbeb7{ zGi^6f*K^ZWDt{T}ms^Y_hf zSDd#h6`SGu^^d={*XF)+o*NZyM`1rW3T5^r)yg#2%es^XS^F@63jtk$9}7|U6YN#0~q?1{9A9{*B+sld8>La7FmRwjDo z23unDOqS%-|3vL`!C7vMDi@5i%*hSZn7!&LMzh+)n|a}7VpJIC>%07dOChJv3#gk# zoseZ&Aul(2jr@d5CfS*gVn?)C{=#$q<5}cMOIBs=+!zf*J!c&#pCNA_qepZk}bzN#*%@s-tVF~N?_W10zNoa@Z|IAa}`CK>BG zv!H%2t(-J-LpnK56G!+`@0w&uiahrIyO za2V+7l^w|4pc_XqX!V=DI=iF`VPEQ^jQastSPpBdI{oSgdMZ8WBvG7srNb~dYKQ$W z1Nu|7(TxHhV1=|d2pfcTgtj{IL0|eXfK7D0Qrh*iEP&gaTIJKrceMF5@;gDK&B4{% zNc@8|aI4y;_K6SvehLq%-N%GSgiqBk)34)yZIs{OchueT4IX{pE-x3%_bGc{omSuG zYwE`;tad3N2?1e;K!buO2+urLT?P%i;baSw10wSaxtrc>q-ZV^5wJ0~0yNDm8y$B0jG zr)2~)%iLo46zlFdc3w*cWNFj7GT<}y!_0g9O=Z_# SscoQsJaCA}daTy#8~*?XM;y2S delta 1034 zcmYk4-%Aux6vywqJ2N}G&g?HuT>~@9U|&qApc2chFtbeh%@vZ-J8rX%JImei$I6Hg z;-e5`=KT2HGc)JhH-mppXU1(iP2l?6 z`2Fl>*W1i@-R5cEtGa8R2%p!vGK@r8XN$f(3tPEr8Z-2= z`eDSBt4~QuN7Yrc*!PtZ5|IiKw3`UF#OmGP(HB&HQPRBfSeUzvl1Oh_dxVnEj&w*H zmCQCo!))8j9eN>aCR+J?neI`+L~1tyx&r8!NQ(~iBE8b7hEhFS)*>sChzcUe4ih$d z!WJ2k-PK#Ft|$Q&tQ}M(geB~@WuqRnIgu9y(Yb51)!LC-J5p;<@j%`6pBn!lKPIqn z%M{0ogpo~SGuig7qfB=zV{WF}8T%;HqqOv3`B&<5JbZ7tI0y2DhnNA&Jd zXh5pjD~fHaDEiP6@u z2l{z>*^x`Gta}?S&mP#WD7y=RA4sl}9cMcG;)GWfu_l75D^1uhht*iesV$zYugur{ zAmpi4&t04Is$LlD3%=_}If$|W#AU<aQs)o&qgBc_yLore=SVqK&e6jT`fPsgIM~KIWCy1v2Zmb2;7k9ZX9cUX*I`tYf@NP`+ z$85dESp(KaEv7f+gnFMl32%&ZgLLL)KG>?@ekRvpHDGPPT9z|dxd%|vB+fuSM|cR- z%U;Ai0Der}0uaNZCV^Wklv7t%x; z#qG-o60lyr==AX)`aZJiiAgAYX(bLIIfg-|5M1Jq%&U;&Z_SbOS#;#n!m4gngB_Q- zo4(N3-Ey$a*k$5Q@SkR`U#Jb2u@!YU#^;CvmRIEEWi0KSb-RBSsb$ipmBdYNM0fu; ze`!s;P@8gKzL7#aWhv4kJ+MNlCe@PU@s~UZ)pfZjHLj!Ao|7I-MTx+vrQV+jwJ`BP zj*QUodxtlEJJH`)qmQkXiHs7Q5%wO&i>;3=N^ z3PtCnCPY?{2`v(90&AVhwRk028I8v!vkTaYfLb1yjutZss701XSmb|`V@4+%C&vWO z+Jz2EV+Dfv5z~CbF6O4`9_Je|6G3R@jp`F`MPcr)P`#=juna%4XD*IlP?8N8f^JBG z0d|a5+agZ}eKxRp9Cl&0U1o-K=Lq5oPs` t`ljztF3pd162xr`k~K^~e#lVYWh+^# zuAn!Cck&?AUPVw8y?F88h~Pz!B6#xZOlmm z@84SU01reFmeisjcqpDhc8`64&)|r*JO{nvmHZI~L^<&yGe9SE$w*z^^vYF7WRwvK z$|)ydMAVe^p+VY?5>2liCA>8shd5fnVewsAfUFo-pTeYgt#-f`dF?z*2-FHT2TjxT zWdaFsNW9TLQA9I22ea>Xk{=;;m^LuMkP~ah378Vkjj4%Aa^&pDthegHriWhBceI*U z4sIbH;n;%sYRrr}O4~zpl&u)=5k|1$$SW(9R@S^QdWu5Jtj!}ulV68z|EG9p9^IC^ zaz_n}CsL>o(*iS;8d4+0mVdbdp;D8VrFJPu$9}5|CF$C+PBO?e4(#-VYM6W}cl1!N z$QuT(#yUFb7@-kr8-T0(3b$9UO6`lWhD{1*_7r*-{!6S&8wuQfEY+Zrs3?y}v?O(* zV+MVp$zol=-G6esqMBNtj>{z@KW_w$9H2%F4FqyKjzs{5L4;G03X9y zhWH_Sne_-Sy*zWAX)K~MD-c9#`9}R_#Yv#I6)BavAD}J1q>FtlF3CD%!K5z(I(1UP z8iqwG@Im2P{nlQ3HcOor_pRaWD@6AjVTpL8yKd8~w_KN!=L~59UhGJ1-Dz%ap+Xj( zW8mUpoNJu_2%A|3Zj`te|K5o!j*ATYe`tBls_RB-RcqayFJWGcXNu~euITUwdjGb~ 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