From c2eb2b8f1c4523248154c6a24135eaaeefee3e5b Mon Sep 17 00:00:00 2001 From: zhm-real Date: Tue, 30 Jun 2020 15:10:38 -0700 Subject: [PATCH] update 3D --- Search-based Planning/.idea/workspace.xml | 102 +++++++++--------- Search-based Planning/Search_3D/Astar3D.py | 18 ++-- .../Search_3D/LRT_Astar3D.py | 42 ++++---- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3471 -> 3421 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 4337 -> 4303 bytes 8 files changed, 80 insertions(+), 82 deletions(-) diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 27c3740..7da76db 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -21,12 +21,8 @@ - - - - - - + + - + + + + + + + - - - - - - - - + + + + - - diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 702c8f7..c0a8df6 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -54,6 +54,7 @@ class Weighted_A_star(object): def run(self, N=None): xt = self.xt + strxi = self.x0 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) @@ -85,25 +86,29 @@ class Weighted_A_star(object): self.lastpoint = strxi # if the path finding is finished - if xt in self.CLOSED and N is None: + if xt in self.CLOSED: self.done = True self.Path = self.path() - visualization(self) - plt.show() + if N is None: + visualization(self) + plt.show() + return True + + return False def path(self): path = [] strx = self.lastpoint - #strstart = hash3D(getNearest(self.Space, self.env.start)) + # 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) + # path = np.flip(path, axis=0) return path # utility used in LRTA* - def reset(self,xj): + 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 @@ -113,6 +118,7 @@ class Weighted_A_star(object): # self.h = Heuristic(self.Space, self.goal) + if __name__ == '__main__': Astar = Weighted_A_star(1) Astar.run() diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index d414d57..a6b3ca3 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -101,43 +101,43 @@ class LRT_A_star2: allchild = [] resolution = self.Astar.env.resolution for direc in self.Astar.Alldirec: - child = np.array(list(map(np.add,x,np.multiply(direc,resolution)))) + child = np.array(list(map(np.add, x, np.multiply(direc, resolution)))) allchild.append(hash3D(child)) return allchild - + def updateHeuristic(self): # Initialize at infinity for strxi in self.Astar.CLOSED: self.Astar.h[strxi] = np.inf # initialize difference Diff = True - while Diff: # repeat until converge + while Diff: # repeat until converge hvals, lasthvals = [], [] for strxi in self.Astar.CLOSED: xi = dehash(strxi) - lasthvals.append(self.Astar.h[strxi]) + lasthvals.append(self.Astar.h[strxi]) # update h values if they are smaller 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 - hvals.append(self.Astar.h[strxi]) + hvals.append(self.Astar.h[strxi]) if lasthvals == hvals: Diff = False def move(self): 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 + 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 + 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(): + 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 @@ -146,21 +146,17 @@ class LRT_A_star2: 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) + while True: + if self.Astar.run(N=self.N): + self.Astar.Path = self.Astar.Path + self.path + self.Astar.done = True + visualization(self.Astar) + plt.show() + break self.updateHeuristic() self.move() - print(hash3D(self.Astar.goal) in self.Astar.CLOSED) - self.updateHeuristic() - self.move() - self.Astar.Path = self.path # previous path (determined from DP) + last path (determined from A*) - self.Astar.done = True - visualization(self.Astar) - plt.show() if __name__ == '__main__': - T = LRT_A_star2(resolution=0.5, N=1) + T = LRT_A_star2(resolution=1, N=30) T.run() 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 faf44864dcbbcca64df898d793d48cf3c5807c87..72db8badde1b280cb15e1c161a30c816a1343ae8 100644 GIT binary patch delta 716 zcmZ`%PiqrV5TAMbfAhzh)~0DZ1U-~!1kpo81i=OkSV|#^rEU#tcWu&blkM&nio7L< zd;kM`)61@ihaLp!!H*zRsz;=mw$HKI$Lz2fMYc-tfq3xuF*geap9_Ud`XP2Lq9^_jiXuKk6NX zR>!r2o!*U?cIY~_jTvDU?cQ!#r%#l19DPCn4to$YvKUJfJl^60Gf$gf&?7{#bDzo? z#8RYiX^gSVrE^xWMqW)dE@y~Bi={f31da?2<7q54MK)HBPq@;8?f0=7<+w^JBaLe_ z8J~QfOBErJ=UN3A#CmLSJt}aS$gB*p$xUu}5cuS$tb0H?*ei04kjb@I;_Fg+Ap=hh zeogTlRWKbSd{L+_Kp8TIa470Yqx8S&XTXst|Y9EDoxPWi*R7s8b*%CL delta 699 zcmZuvJ#Q015S_X2+sm2Q33i+a8X_SRTZafz8VZC=C_oV+5=cnIx^RxqKF4;>hvZ^U zE?gsmwxYX3spwE@{sDi0NcjthhKhz+C!}JF-M91JzS)`G^VJV)g-5Q-fX1hjKYKrm zUkl&0&6yVt58IyKZ%@2tdmJUhk=N-a;k$ir>%-~v%C|xi!xhipDahT0xnJw8=pM2qZ6W)AIjf8Q7Tug4#i76$EdjuS=R;7i* z%_=C6TB*&gWPxiWRw|G(&bS=`Y1xJ90T2g0mz#tbHxK01m8_6MWW>Mb_!^~P0TN!6 zuFFt?TtPUJ`IKzVPX9;00*+L9Jt>k9XUMS(ffC7X)St^smIC!E@*-IZv}Ad%)n}xx zTvo109d1!EtPH3x(McA;j7 z`_>(;wie$Kp>?i5lpjPqv$w1=8pzN>@zLJEm*SiKWIZu@Ci!EayjN1HfR3v>F+Dca z@J#b3-EqwlF1zW(OB6fP4*K1Z*kE_7d$b)d(gdiXp-AHuEMWmf%zk4}gw6;biyuxM TZ=V_q2I97G-KToXqWpgWch#w! 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 43d7047d1395c5b4bfb3d7e402f34dbbb060d864..bd5a61d0615f01f713bb3f3d5c527b731395d4a5 100644 GIT binary patch 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 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 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 3fca6abb3642e8b320b9af2495f86ea43bc56100..9d17d49e7405f45160cdba930392ca56357c2463 100644 GIT binary patch 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- delta 164 zcmeBGU9ZaR#LLUY00cYTer@DtWU6o1&&bbB)lbYxtj$SM7A0rsCM6c9rYHnJ6+lJfja~Hh^gxJ^s)C&S XlK9P;OfUFYqnL^+qBe&Knll0b;_Ema 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 d267c527b49eddd51e65a9a60cc0c97ea30bc686..7e1431acf44a7185c406cbadcdf3b97ae0e14846 100644 GIT binary patch delta 129 zcmaDW@==7>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)jD9y~x%u82@%E&K`aY-%C zF3B&5DM&2IP$