From 80f8e3e8cd5fdbad2f9c06c852a98ffb3fa8ba58 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sat, 27 Jun 2020 22:16:12 -0700 Subject: [PATCH] updated A* utils --- Search-based Planning/Search_3D/Astar3D.py | 5 ++--- .../Search_3D/LRT_Astar3D.py | 0 .../__pycache__/env3D.cpython-37.pyc | Bin 1868 -> 1868 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4783 -> 4783 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 3683 -> 3864 bytes ...nalAstar3D.py => bidirectional_Astar3D.py} | 12 ++++++------ Search-based Planning/Search_3D/env3D.py | 2 +- .../Search_3D/plot_util3D.py | 2 +- Search-based Planning/Search_3D/utils3D.py | 7 +++++++ 9 files changed, 17 insertions(+), 11 deletions(-) create mode 100644 Search-based Planning/Search_3D/LRT_Astar3D.py rename Search-based Planning/Search_3D/{bidirectionalAstar3D.py => bidirectional_Astar3D.py} (92%) diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index d514936..734f944 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -31,7 +31,7 @@ class Weighted_A_star(object): self.OPEN = queue.QueuePrior() # store [point,priority] self.h = Heuristic(self.Space,self.goal) self.Parent = {} - self.CLOSED = {} + self.CLOSED = set() self.V = [] self.done = False self.Path = [] @@ -51,7 +51,7 @@ 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) - self.CLOSED[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) @@ -66,7 +66,6 @@ class Weighted_A_star(object): if (a, strxj) in self.OPEN.enumerate(): # update priority of xj self.OPEN.put(strxj, a+1*self.h[strxj]) - pass else: # add xj in to OPEN set self.OPEN.put(strxj, a+1*self.h[strxj]) diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py new file mode 100644 index 0000000..e69de29 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 d76f6adb5d7348b5f676b68440cf6b8495e1ef61..8c180c56ad2b0ac234342525aa02c5a3c75ce25a 100644 GIT binary patch delta 27 hcmX@ZcZQGKiI3ZdD201NEVJbzLiXlb<*8TJp)Wajg^a^ zKK!xPIh8a>9+C7W3HmIdxa4nmo+WU~=|$ENU`eKGOd6DFRuPDa9>Lan88E93?TLbD z^;+?>MyIo&Ygq&g3Sa;(e!j9-xV20Uysf@pJAgZC&6ysnkmDewd30P7{XzZi{Z_A? zi|PG@wOKn|VS7(c?0zP(6XTTQ7YT`R$ZM?cw}Y!tEx1!KtTx?C*j7K?c`<@ZRDv4a z^zOm3+V$?6xJVxL-Ycb7bx4nf;vea&SF`jSy*X9%i)jZ01SBDNbg`;FT6q=uH{h{) z<>yv*G*hpP0d5cuL=<%!tvHHk3%V^Sy_#}3Fyj~9=f|jbi%--y|JIBi0(CE}YWSZY qH@eLz8ko)a<=qM{sbVlQSt5b}!G$3#krIxu2~)xprW1-$vfwYymuL$B delta 363 zcmbOs_gIF{iIv1y`uh_O_PP>OI1V-$CaNQ!6+LljSnSc*81 z=1t{Gk;rB$s!L%=k<8)G6^IgGWJqBQX3&)0xa}X?W_u1DCN_Pbc|``33%QaRbthlt zQe||W{F_UX@zrEe?is>9H4Iq{SpqH$u@1HDCA?XD3j`-$;#Ou9n*5D>KBNESX*}8- z!bN%zBTn*|GwM%fsE!Mo!+=9v? z79g+4V{#pzj-oS&=?fxYTJsWfQ{&@rv8Lvg8M_qeO}@aVrXT}Uz`@AD#KFSB#=*?N S#la3FnK@Xwgg8WjG$R0Yu25?L diff --git a/Search-based Planning/Search_3D/bidirectionalAstar3D.py b/Search-based Planning/Search_3D/bidirectional_Astar3D.py similarity index 92% rename from Search-based Planning/Search_3D/bidirectionalAstar3D.py rename to Search-based Planning/Search_3D/bidirectional_Astar3D.py index 6cde140..cd05ff7 100644 --- a/Search-based Planning/Search_3D/bidirectionalAstar3D.py +++ b/Search-based Planning/Search_3D/bidirectional_Astar3D.py @@ -35,7 +35,7 @@ class Weighted_A_star(object): self.h1 = Heuristic(self.Space,self.goal) # tree NO.1 self.h2 = Heuristic(self.Space,self.start) # tree NO.2 self.Parent1, self.Parent2 = {}, {} - self.CLOSED1, self.CLOSED2 = {}, {} + self.CLOSED1, self.CLOSED2 = set(), set() self.V = [] self.done = False self.Path = [] @@ -53,20 +53,20 @@ class Weighted_A_star(object): self.OPEN1.put(x0, self.Space[x0] + self.h1[x0]) # item, priority = g + h self.OPEN2.put(xt, self.Space[xt] + self.h2[xt]) # item, priority = g + h self.ind = 0 - while not any(check in self.CLOSED1 for check in self.CLOSED2): # while xt not reached and open is not empty + while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty strxi1, strxi2 = self.OPEN1.get(), self.OPEN2.get() xi1, xi2 = dehash(strxi1), dehash(strxi2) - self.CLOSED1[strxi1] = [] # add the point in CLOSED set - self.CLOSED2[strxi2] = [] + self.CLOSED1.add(strxi1) # add the point in CLOSED set + self.CLOSED2.add(strxi2) self.V.append(xi1) self.V.append(xi2) - visualization(self) + # visualization(self) allchild1, allchild2 = self.children(xi1), self.children(xi2) self.evaluation(allchild1,strxi1,xi1,conf=1) self.evaluation(allchild2,strxi2,xi2,conf=2) if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) self.ind += 1 - self.common = set(self.CLOSED1).intersection(self.CLOSED2) + self.common = self.CLOSED1.intersection(self.CLOSED2) self.done = True self.Path = self.path() visualization(self) diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py index d71b524..f3700fa 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/Search-based Planning/Search_3D/env3D.py @@ -23,7 +23,7 @@ def getblocks(): return np.array(Obstacles) def getballs(): - spheres = [[16,2.5,3,2],[10,2.5,1,1]] + spheres = [[16,2.5,4,2],[10,2.5,1,1]] Obstacles = [] for i in spheres: Obstacles.append([j for j in i]) diff --git a/Search-based Planning/Search_3D/plot_util3D.py b/Search-based Planning/Search_3D/plot_util3D.py index 3b4a1e3..3bf0e8a 100644 --- a/Search-based Planning/Search_3D/plot_util3D.py +++ b/Search-based Planning/Search_3D/plot_util3D.py @@ -80,7 +80,7 @@ def visualization(initparams): ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy) plt.xlabel('x') plt.ylabel('y') - plt.pause(0.001) + plt.pause(0.0001) def make_get_proj(self, rx, ry, rz): ''' diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index dc27f90..e0b208b 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -42,6 +42,11 @@ def isinbound(i, x): return True return False +def isinball(i, x): + if getDist(i[0:3], x) <= i[3]: + return True + return False + def StateSpace(initparams,factor=0): '''This function is used to get nodes and discretize the space. State space is by x*y*z,3 where each 3 is a point in 3D.''' @@ -74,6 +79,8 @@ def isCollide(initparams, x, direc): if dist_wall <= dist: # collide return True, child for i in initparams.env.balls: + if isinball(i, child): + return True, child shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) if shot != []: dists_ball = [getDist(x, j) for j in shot]