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 d76f6ad..8c180c5 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ 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 c63c4fd..61a7bf0 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ 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 7010ce9..f1face4 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ 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]