Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms

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])

Search-based Planning/Search_3D/bidirectionalAstar3D.py → 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)

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])

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):
     '''

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]