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

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

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,20 +93,62 @@ import queue
 
 class LRT_A_star2:
     def __init__(self, resolution=0.5, N=7):
-        self.Astar = Astar3D.Weighted_A_star(resolution)
-
-        while True:
-            self.Astar.run(N)
+        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)
+    T = LRT_A_star2(resolution=1, N=100)
+    T.run()

Search-based Planning/Search_3D/utils3D.py

@@ -14,6 +14,9 @@ def getAABB(blocks):
 def getDist(pos1, pos2):
     return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
 
+def getManDist(pos1, pos2):
+    return sum([abs(pos1[0] - pos2[0]),abs(pos1[1] - pos2[1]),abs(pos1[2] - pos2[2])])
+
 def getNearest(Space,pt):
     '''get the nearest point on the grid'''
     mindis,minpt = 1000,None
@@ -88,8 +91,20 @@ def isCollide(initparams, x, direc):
                 return True, child
     return False, child
 
-def cost(i,j):
-    return getDist(i,j)
+def obstacleFree(initparams,x):
+    for i in initparams.env.blocks:
+        if isinbound(i,x):
+            return False
+    for i in initparams.env.balls:
+        if isinball(i,x):
+            return False
+    return True
+
+def cost(i,j,settings=0):
+    if settings == 0:
+        return getDist(i,j)
+    if settings == 1:
+        return getManDist(i,j)
     
 if __name__ == "__main__":
     from env3D import env