From 4da4112cde2d20d1ab16ff79cfbb6c641a6fc33a Mon Sep 17 00:00:00 2001 From: zhm-real Date: Mon, 29 Jun 2020 12:49:24 -0700 Subject: [PATCH] update 3D --- Search-based Planning/.idea/workspace.xml | 27 ++++---- Search-based Planning/Search_2D/LPAstar.py | 1 + Search-based Planning/Search_3D/Astar3D.py | 63 ++++++++++-------- .../Search_3D/LRT_Astar3D.py | 30 +++------ .../__pycache__/Astar3D.cpython-37.pyc | Bin 3114 -> 3095 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 1868 -> 1834 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4783 -> 4749 bytes .../__pycache__/queue.cpython-37.pyc | Bin 2669 -> 2673 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 3864 -> 3830 bytes 9 files changed, 61 insertions(+), 60 deletions(-) diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index bc974b6..b97c0f9 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,9 +20,10 @@ - - + + + - + - + - + - - + + + + - - diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py index 8e7b0a2..ee37b0c 100644 --- a/Search-based Planning/Search_2D/LPAstar.py +++ b/Search-based Planning/Search_2D/LPAstar.py @@ -15,3 +15,4 @@ from Search_2D import env class LpaStar: def __init__(self): + return diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 305051b..75b0a57 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -12,46 +12,51 @@ 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 getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost +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 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.env = env(resolution = resolution) - self.Space = StateSpace(self) # key is the point, store g value - self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal) + 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.env = env(resolution=resolution) + self.Space = StateSpace(self) # key is the point, store g value + 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) + self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0 + + self.h = Heuristic(self.Space, self.goal) self.Parent = {} self.CLOSED = set() self.V = [] self.done = False self.Path = [] self.ind = 0 + self.x0, self.xt = hash3D(self.start), hash3D(self.goal) + self.OPEN = queue.QueuePrior() # store [point,priority] + self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h - def children(self,x): + def children(self, x): allchild = [] for j in self.Alldirec: - collide,child = isCollide(self,x,j) + collide, child = isCollide(self, x, j) if not collide: allchild.append(child) return allchild 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 - while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty - strxi = self.OPEN.get() + xt = self.xt + 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) - self.CLOSED.add(strxi) # add the point in CLOSED set + self.CLOSED.add(strxi) # add the point in CLOSED set self.V.append(xi) visualization(self) allchild = self.children(xi) @@ -59,22 +64,23 @@ class Weighted_A_star(object): strxj = hash3D(xj) if strxj not in self.CLOSED: gi, gj = self.Space[strxi], self.Space[strxj] - a = gi + cost(xi,xj) + a = gi + cost(xi, xj) if a < gj: self.Space[strxj] = a self.Parent[strxj] = xi if (a, strxj) in self.OPEN.enumerate(): # update priority of xj - self.OPEN.put(strxj, a+1*self.h[strxj]) + self.OPEN.put(strxj, a + 1 * self.h[strxj]) else: # add xj in to OPEN set - self.OPEN.put(strxj, a+1*self.h[strxj]) + 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: + if N: + if len(self.CLOSED) % N == 0: break - if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) + if self.ind % 100 == 0: print('iteration number = ' + str(self.ind)) self.ind += 1 + # if the path finding is finished if xt in self.CLOSED: self.done = True @@ -87,11 +93,12 @@ class Weighted_A_star(object): strx = hash3D(self.goal) strstart = hash3D(self.start) while strx != strstart: - path.append([dehash(strx),self.Parent[strx]]) + path.append([dehash(strx), self.Parent[strx]]) strx = hash3D(self.Parent[strx]) - path = np.flip(path,axis=0) + path = np.flip(path, axis=0) return path + if __name__ == '__main__': Astar = Weighted_A_star(1) - Astar.run() \ No newline at end of file + Astar.run() diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index a0f854c..9c56ffd 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -12,8 +12,9 @@ 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 import Astar3D +from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ + cost from Search_3D.plot_util3D import visualization import queue @@ -91,31 +92,22 @@ import queue # return path class LRT_A_star2(): - def __init__(self,resolution=0.5, N=7): + 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) + self.Astar = Astar3D.Weighted_A_star() + + while True: + 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)]) - + 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): - xt = hash3D(self.Astar.goal) - while xt not in self.Astar.CLOSED: - self.expand() - #self.updateHeuristic() if __name__ == '__main__': - T = LRT_A_star2(resolution = 1, N = 2) - T.run() + T = LRT_A_star2(resolution=1, N=50) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index b60701231db1978095e88ce1702ad857d412d101..49718a2dc866f2984ab6148bee5ab79515060052 100644 GIT binary patch delta 1280 zcmZ`&-D@LN6u)=AGig3LP4i{D33b1sYppEUt`wHGDTqkzvWgO;W41GGCP|a@&JCt; zL%@K~3-jQM$ix?Q!3Xgl@MV1$e40OCK@j#)SP(pC8hvwy^KpOY+>dka?>;GhRZQ=v zQYL}#ox>NO{+anH{hY4-L1BC%AqpSbYaj<8{x3hTO9yl+NJj-RW|AOw+9RMdJUc_X ze1_U`Aa1Y3e$K(HRlWo>^cjt!*ex(GS_f}6Kk!}VH{Tw4=U)G;+Gr0(&ARJ%`C!-_ zws^Zb?6vxR47GY^1LpDex!-)~w%AGgwMQ-Abv~*dFESReeYf7+^LdNit#1x5Bg3}6 zzQ=7lk4?P|iqKuUMGcT0X^EQjCQZ|#l%*PA+Tt%$Gq|H(&|L71dW$xKU)9@mGkC7< zOp~yH0TSQQmvDX1Ffx%I)A73kn%|*a{V`?lg9q4^4dpqR$XpTfj7}8H2~{D_DaE;3 zs0#&sZU{1qu`q$iL>)g7YMp%Y?L_0I&|t1EbmRkU!9D|<6mooLNuig3I}>A)5C+`2 z0?AT=OiW>lM3+FW8E5Oe1Y)F<66l#iKg9z}VR}xw8haM|8KDc)q2On6)!c#!7r37k zX2kS=!XFYrq23L?42g6`1S7NzjQs3_s~N9QhUahKgdu!Nx8bNj4{o;qY+pGUIK8N-JfDacV)iL-`14jgX} zKCdO&O+@HXe2z%LCs?Do0%(SU{KqLblY|YC$=4_(1zh9vx2|PN@fRwku>n3?n@t*cs2QT mXQ!K03Q!dV&y3?D?K#xIZE51u9OCE;}$a`u4%@9G~Q1uXsm delta 1359 zcmaJ>O>7%Q6rQ&~_IlTG9NY0vt0Zkn(il-3R3Vb8qDtzDBBV4BpeI<|?8aW(v7PZ) zIMP^=@P!kqv5j94hV?@M|$GSZBKAQa46#13%oZ@Do%Ly{=WHU_Pw`n{CV-~ zVrqA8&Sv;)zWVILpHknY{(>vNQ_R?}cMi3g$H*?D+(qes@_k9W3vWzPQzn@^FVd1V zO%!XaVZ0C!ni1JC*<79UqKE5lUik%&SVfN=uySng4+wwLm!K5>qu*PNjGdj`T~6sl z=E=Yx_>uiKnSCB~d1O4;zqc1YFkZP|)*_?tw~u%Rsb4XE9nG_GrO`e1E48*itW^De zOLTjcdLSBuN0nYpG;Z|TwN5AK)Nj<<^)APS<9@gRg~WkhHhPmveOqe zezW>=?=-SpH|PYybr-4rYp599hAr4cNrDaQkb)xGG{nF{!6$rZp1@-Gvw0n&@NaVi zHo{A>t+OO5<7*R)k?&Pku9*P2JA5m-;uO$x-EWMc(RQiP0R!*67)%0BpV z6vtHJxY?4{>=pbow5Bvv8m6>LI1Z!4$d(B)FHPiX1vYY|BkdMLuA|u2ZH8;CnU>@^ z(mJAI&h*K`l(k}*Z5~6a(UOh_=*Q^b+v!4DO8H$`|h{Xef$51%0-5?UYhV{7FlPx@S)>KOGhuO$VHYunvFJ5#^lqQ#^ z4A}Vq^K|$$zIt|9O)W3rr{zUbOQ<5fHxN8eAzdDRRbh2sa7TGH&*L;q{6$i8q&7*> zknj~$QNr&Gj(uJeex$ek4qqeC?D3!@BE2s-e}Mp_cMAucuad9#+5#^}qx*3=$=3;i zy+|8wqKL@EEyDoafWJgb+OQdgPpq{&^xFRGs2Jp7SOl{Cs z;=C8G+uld2C)Eq=KRmkjT#-}gm=JdI)bS`>F(uY2ga zk%_(So$_q>r@b9;KG&A*QcI)jD9y~x%u82@%E&K`aY-%CF3B&5 zDM&2IP$X#wa^kjID)D1!!rJ>f{A%@uF%#W)xdqYI$m2*)7(BqRhOKA}OGZ_+)K%MF8V4 BHNOA= delta 205 zcmZ3*cZQG4iI&`-}S$tX?IFGwuO z&@ISG%*)HnOV>@zNzX3=O5_&n2d5?$C1>a+B^IZqCx_ Ig`hbj05IkxegFUf delta 163 zcmeBGU9ZaJ#LLUY00f+>8@ZU6>f7}*@^e%56LV54^j%VmvrF;|^wTp-GD?&53ld8* zbPIA4^YSwD(sdJa(({Xe61m0t!KsNw$r-vyiN&cY3IR|BP|iIG&46dFI^!jBfm7pCAB!a zB)=f0Ah9Gvp&%zQFApS@n3JAglv$FITO1Rdnpl*ap_`OgoSLE#0961LjW>3Q*?gJt UG#lfs&GR{Y85!j!zvXNI0O=qv=>Px# delta 125 zcmew;@>Yb`iI_?Dmj{wc%t_BL$}GvqEshCJO)N^z&`nA#PEAn=fGU8B#v8lDY(B&o G#tQ%tPAPi; delta 118 zcmew+J424wiI