From 3f1388fba078d75af75a13a47fa983bd44cb48f3 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Mon, 29 Jun 2020 20:46:59 -0700 Subject: [PATCH] LRTA* --- Search-based Planning/Search_3D/Astar3D.py | 24 +++++-- .../Search_3D/LRT_Astar3D.py | 59 +++++++++++++++--- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3095 -> 3471 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 1834 -> 1868 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4749 -> 4783 bytes .../__pycache__/queue.cpython-37.pyc | Bin 2673 -> 2669 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 3830 -> 4337 bytes Search-based Planning/Search_3D/utils3D.py | 19 +++++- 8 files changed, 85 insertions(+), 17 deletions(-) diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 75b0a57..8555add 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -42,6 +42,7 @@ class Weighted_A_star(object): 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 + self.lastpoint = self.x0 def children(self, x): allchild = [] @@ -56,8 +57,9 @@ class Weighted_A_star(object): 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) + if strxi not in self.CLOSED: + self.V.append(xi) self.CLOSED.add(strxi) # add the point in CLOSED set - self.V.append(xi) visualization(self) allchild = self.children(xi) for xj in allchild: @@ -78,11 +80,12 @@ class Weighted_A_star(object): 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('number node expanded = ' + str(len(self.V))) self.ind += 1 + self.lastpoint = strxi # if the path finding is finished - if xt in self.CLOSED: + if xt in self.CLOSED and N is None: self.done = True self.Path = self.path() visualization(self) @@ -90,15 +93,24 @@ class Weighted_A_star(object): def path(self): path = [] - strx = hash3D(self.goal) - strstart = hash3D(self.start) + strx = self.lastpoint + #strstart = hash3D(getNearest(self.Space, self.env.start)) + strstart = self.x0 while strx != strstart: path.append([dehash(strx), self.Parent[strx]]) strx = hash3D(self.Parent[strx]) path = np.flip(path, axis=0) return path + # utility used in LRTA* + def reset(self,xj): + self.Space = StateSpace(self) # key is the point, store g value + self.start = xj + self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0 + self.x0 = hash3D(xj) + self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h + self.CLOSED = set() if __name__ == '__main__': - Astar = Weighted_A_star(1) + Astar = Weighted_A_star(0.5) Astar.run() diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index 9c56ffd..94a2edf 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ - cost + cost, obstacleFree from Search_3D.plot_util3D import visualization import queue @@ -93,21 +93,62 @@ import queue class LRT_A_star2(): def __init__(self, resolution=0.5, N=7): - self.lookahead = N - self.Astar = Astar3D.Weighted_A_star() - - while True: - self.Astar.run(self.lookahead) + self.N = N + self.Astar = Astar3D.Weighted_A_star(resolution=resolution) + self.path = [] + def children(self, x): + allchild = [] + resolution = self.Astar.env.resolution + for direc in self.Astar.Alldirec: + child = np.array(list(map(np.add,x,np.multiply(direc,resolution)))) + allchild.append(hash3D(child)) + return allchild + 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)]) + minfval = min([cost(xi, xj, settings=1) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)]) + if self.Astar.h[strxi] >= minfval: + self.Astar.h[strxi] = minfval def move(self): - print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()])) + #self.Astar.Parent[xj] = dehash(self.Astar.lastpoint) + strst = self.Astar.x0 + st = self.Astar.start + ind = 0 + while strst in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop + # strChildren = self.children(st) + strChildren = [hash3D(i) for i in self.Astar.children(st)] + minh , minchild = np.inf , None + for child in strChildren: + h = self.Astar.h[child] + if h <= minh: + minh , minchild = h , dehash(child) + self.path.append([st,minchild]) + strst, st = hash3D(minchild), minchild + for (_,strp) in self.Astar.OPEN.enumerate(): + if strp == strst: + break + ind += 1 + if ind > 1000: + break + self.Astar.reset(st) + + def run(self): + while self.Astar.lastpoint != hash3D(self.Astar.goal): + self.Astar.run(N=self.N) + #print(path) + visualization(self.Astar) + self.updateHeuristic() + self.move() + self.Astar.Path = self.path + self.Astar.done = True + visualization(self.Astar) + plt.show() if __name__ == '__main__': - T = LRT_A_star2(resolution=1, N=50) \ No newline at end of file + T = LRT_A_star2(resolution=1, N=100) + T.run() \ 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 49718a2dc866f2984ab6148bee5ab79515060052..339ed0a0505ccc63e50df3bf32ec6770c09bcf1c 100644 GIT binary patch delta 1382 zcmZ8hO>7%Q6rMNxIdH_05T(b8V-LM@;EY;JNO0o9fpaeK-nz6>R+?|#-_G0jy*K;a+_wu8 z>*M1YhR?5$k8eMnd_3_JTzwl=pVdIH8W-b&3*$c23}FiEKC78xLZpO)7E7c>1}(P8 ziZR?%!ro%#Q~GaMfnR>k^QYj%tThue_l$mKEJH#6YdqWg<0+-bUN8#)Vx;!CZ1JyM=r+qlrZJ9Jz(2m|H1GngnkuUD-K6!o_C4V3inRxwZc zlaG*Pl!neuGiVFxhgCgq-#qsNFoyN81O5PdwV0y}yQv2t>nNd3*7eu+>4Gt`%Uifx z13Na40O&m!q+&xqwBLf3er{I~t<)0yqbsQb%7|B5+bjk=XGKj?d>>+%VS?|{*()E> z898CRw0x{i3sX%cu~r}S_o;=;nA@Fx@ntr?HHfi?yADahLLc^GFnfe2xpNA8)5kAU z`N6#YGqv!+Y`DKy_vK>P5&ojz>w2N^#p0F4Ps$7O91(Mdph(0R4PVJDsRe>cV)KNM z&}gLmq~n`JSMeGzL5lV+X`FW?0i+u$`V;%GdX-{MN0O9A@2opRX=g&ODB3^ z$o=p#z7SnQU|_=>WWa%pK6Xl}8OU*)&*W1eRXPW@5H&PeaW0sk_me@vGSlM>G2c}az%g6B!4`yz7)L-S5M=$6f- z7D?BpW>O(T2a)DZjm9Y{hGr^>xs5{f8Uh0oY;I#EbC81xxHq-!2aTrkg?rtN^oJSe zaEDVIKD67oZs3aQ2nyRsWwS_`0P5;kBo`^smFC@Wbkf$)R44Y*~f00@G>^y4nBEdBR zYJl7zc$MoF^%Llz3L5@KqV@lKt`qtRZmDpp&_bK9T}Adsew#@23AQb%zwft zeE!%YeF8qKNBN~3gDxIX-aSf5HN~-t+pHNAUl2Bk%cVJYEBn~7n3G*RwD;^%KA9<$ z53}=@v+I`kv-_r7NbVQSQc1YXV!q65x3K4A`MBEJkX^M*wpGYY&1d@l>HWB*kAm)1O3 z?Omy)H!y-kZH?y-n(Dah$gW98HXG$V!9@R+ znaJ$0G>ro1HUfbFbbtXm-wY0_QHaVRej1!iucIqZD&UI#@lGm|Z%Js$N0T@?c~@pj z9#qAgki{s~lj0;Vi8=YP{$3~#5?nV1C&XiC?E=~a_P7?wF1&?molE8? diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index bd5a61d0615f01f713bb3f3d5c527b731395d4a5..43d7047d1395c5b4bfb3d7e402f34dbbb060d864 100644 GIT binary patch delta 415 zcmaivyH3L}6ozq0($Je|xfH}jMP(_X4N~6#RSX?ar4C3DlI64}iRw6!okCg!h?&uE z!G@R^*ccdif;vS4KGnSVIrc!5VGe6sx Yt!+?zBmbW25%X!xw&jAlSjiv!0ok2+8UO$Q delta 376 zcmXYr!Ab)$6h%$ROo!GfVryFjVT3AF5d;^4ORbFmR zNiYbL8R}3u&5{(QBnyL-5zg%wVUt8eSr|*yqeP6tqal%W9Ner7D;1w~kh(7PfmSbt zF7Q8p3oYQ5O{!Q|HFylF>Jr|9P4x`FdcKhd?vN#;sT5UnXlFyA=puMx4W@rL+cg|i%pff2X4!*d+OC~qMBI) zGZGqjpyUawXb%LGNf$95CnJot!)I~25Jh7Uwsqb%_ltdV4qQ$jDSyyTDk7eV LZ4=N?hsE80Y`|~W 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 9d17d49e7405f45160cdba930392ca56357c2463..3fca6abb3642e8b320b9af2495f86ea43bc56100 100644 GIT binary patch delta 164 zcmeBGU9ZaR#LLUY00cYTer@DtWU6o1&&bbB)lbYxtj$SM7A0rsCM6c9rYHnJ6+lJfja~Hh^gxJ^s)C&S XlK9P;OfUFYqnL^+qBe&Knll0b;_Ema delta 109 zcmZ3l+N;X##LLUY00d3uKQ?kRGFhZLTg8MHrxq2*c$8-5X6B_UL}lcc#<-*wXP4v` z#1tf!WGEEmBoRxCKe@U#2dTBY;I$E%g1_)si@-C J<_bY`MgWfxC3gS- diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc index 7e1431acf44a7185c406cbadcdf3b97ae0e14846..d267c527b49eddd51e65a9a60cc0c97ea30bc686 100644 GIT binary patch delta 125 zcmew;@>Yb`iIiIG&46dFI^!jBfm7pCAB!a zB)=f0Ah9Gvp&%zQFApS@n3JAglv$FITO1Rdnpl*ap_`OgoSLE#0961LjW>3Q*?gJt UG#lfs&GR{Y85!j!zvXNI0O=qv=>Px# 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 6b0b616c0f7b6732513d3de8771fa89e93deccea..60291af54bad4b0758bb923088da78e75c021e16 100644 GIT binary patch delta 1265 zcmYjQ&uN)!~rC3+_(hM|Djh--3vz!J#p!wr@pt&57wS<-hA)P%$qmg{5$b2pSqMx znhd{RKmPaVpX6RD3vc#jA6yyY!_U}Sf}i3i`N%U^OY$rqMJB~_d<^>&JkQ7Z1bPPC zG@sm7#B0v@R3p-Tk?OHw9e7tVAonW)r zZaAJ-_i9C_QELfwG<{qC0mEr~)pf+iW_7V_&(G7J*#7U>{dcef@lb2G{8A{VR7<|p z)4Opz%Vli-d4zxk(7|LzSqBKStP2sNL=55)qecuOF*UVI2heT^z*U4opdwg`7{)GC zopm4Wlf;h@^c}*aplXI@%?++QUb*fEX&n2s0t1ng*VMO=m0#7$=n(NK;uQl@V1S7& z1tKpiT5V(u`A}^-J7N?qF^*tqVnY6<6$U49;2Q``(Qw8%h=NpO-Q*1M2Rm6(#@>J> z7xY<}mG|^wnwI$CyNGf5O8=7nkr*9;K?5`p3-WpV7G_w9PvwldU-#CXMq}YPC`Uo1 z{j~8X^v@$5l||U{bKlP}Cr$G-?rEDjeUW@i@}_waCgh%(hqH3uoPm}7zs)BwlSMDq zk2Vis%K||e^omI36F(u`mqDVapP}KBe4i-x(xwi)g9hIKd3jM@s1lCDB^<181Y*E(_Ov zgaJKC;I;PTKqzh_zd;9}k9um+LFhs|3I}|DnSgar=3Pt+8D(3!AWoCkdlY9;w#RzL z@<#>D;R={#l-pIbL&JB203W0;Po#!-Ye@C9cA!vkCkm=on@-)UR`KrW@YvByIqZhQ znD@BZek!QqqC(J5Fi3E;t;o}`KwuG&LVE=)k+p&_E$9-X0vv7ZIZd}(4P&nNWU(yH j$PcMPVH%?hFcd>GbVJ3TX~d&GX2i^lF=F(Yx&i+KMH=o( delta 815 zcmYLHO=uHQ5Z=k|CeKZGo3yrR(nkB68g<*of>H%R8=-D{D0eyiPHI~!>&}3aMr*j;PG&X2A32gU*hqNEauOs5 zN>V<%v-kX_J%83(3S1so3u|?+?pKG)wdR^tbc5A)vt_mHc5S%TuzjCWcB9(l^>)n* ztdeWfLO%Y*W z00Rw|0X!>AwVFyHAIhG+5z6**zy~nQD?BaUs=3}l3<8EAMHK890zM|bs$1G9@e$(1 z7qwSxN1s7XBx92>F6LsUN_$L*yYi5D9Q%|!M#WJoh6X6WO;KgnQJTq)W>`I_`^$Et zVICA^sAy{UCwmX^6G(HJ;G}q`orX#AO&iCv67fuQhUkKriC-8?qZ`6HKSWdq=+Q#0 z)VAAhsb#OYr^Ji+1Qd5a#Mfb>ACvhACt)Gn$pa^K2Yoz`;SI5*o6%`X6vPwVj8hXQ znS4fk)X%|`_@j>`=7{LD(=}nKX_TNS9vFr5i%6_=E(sm7G{-!yfW)<>O=%OhBKT{- zB1+iT)PpI02|;}C?z?}BsykZ%zlwgyDizPJ`;`iIkTx_xrYBVWHLrD#D;V+11aX31 zf{yYmd1C}Q0(zo<(@c?9Bp|{6kMV8K6)zJSOp7;(+~^U^F<=p