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

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -45,7 +45,7 @@ class rrtstar():
             if not isCollide(self, xnearest, xnew):
                 self.V.append(xnew)  # add point
                 self.wireup(xnew, xnearest)
-                # visualization(self)
+                visualization(self)
                 self.i += 1
             self.ind += 1
             if getDist(xnew, self.env.goal) <= 1:

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -26,7 +26,7 @@ class rrtstar():
         self.maxiter = 10000 # at least 4000 in this env
         self.stepsize = 0.5
         self.gamma = 500
-        self.eta = 1.1*self.stepsize
+        self.eta = 2*self.stepsize
         self.Path = []
         self.done = False
 
@@ -61,7 +61,7 @@ class rrtstar():
             if not isCollide(self,xnearest,xnew):
                 Xnear = near(self,xnew)
                 self.V.append(xnew) # add point
-                # visualization(self)
+                visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
                 # connecting along minimal cost path

Sampling-based Planning/rrt_3D/utils3D.py

@@ -40,9 +40,9 @@ def sampleFree(initparams):
     if isinside(initparams, x):
         return sampleFree(initparams)
     else:
-        #if i < 0.05:
-        #    return initparams.env.goal+0.01
-        #else: return np.array(x)
+        if i < 0.05:
+           return initparams.env.goal+0.01
+        else: return np.array(x)
         return np.array(x)
 
 

Search-based Planning/Search_3D/Astar3D.py

@@ -12,11 +12,11 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
-from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
     cost
 from Search_3D.plot_util3D import visualization
 import queue
-
+import time
 
 class Weighted_A_star(object):
     def __init__(self, resolution=0.5):
@@ -29,7 +29,7 @@ class Weighted_A_star(object):
         self.env = env(resolution=resolution)
         self.Space = StateSpace(self)  # key is the point, store g value
         self.start, self.goal = getNearest(self.Space, self.env.start), getNearest(self.Space, self.env.goal)
-        self.AABB = getAABB(self.env.blocks)
+        # self.AABB = getAABB(self.env.blocks)
         self.Space[hash3D(getNearest(self.Space, self.start))] = 0  # set g(x0) = 0
 
         self.h = Heuristic(self.Space, self.goal)
@@ -120,5 +120,7 @@ class Weighted_A_star(object):
 
 
 if __name__ == '__main__':
+    sta = time.time()
     Astar = Weighted_A_star(1)
     Astar.run()
+    print(time.time() - sta)

Search-based Planning/Search_3D/LP_Astar3D.py

@@ -0,0 +1,190 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+import os
+import sys
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+from Search_3D.env3D import env
+from Search_3D import Astar3D
+from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isinbound, isinball, hash3D, dehash, \
+    cost, obstacleFree
+from Search_3D.plot_util3D import visualization
+import queue
+import pyrr
+import time
+
+
+class Lifelong_Astar(object):
+    def __init__(self,resolution = 1):
+        self.Alldirec = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1], [1, 1, 1],
+                                  [-1, 0, 0], [0, -1, 0], [0, 0, -1], [-1, -1, 0], [-1, 0, -1], [0, -1, -1],
+                                  [-1, -1, -1],
+                                  [1, -1, 0], [-1, 1, 0], [1, 0, -1], [-1, 0, 1], [0, 1, -1], [0, -1, 1],
+                                  [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]])
+        self.env = env(resolution=resolution)
+        self.g = StateSpace(self)
+        self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
+        self.x0, self.xt = hash3D(self.start), hash3D(self.goal)
+        self.v = StateSpace(self) # rhs(.) = g(.) = inf
+        self.v[hash3D(self.start)] = 0 # rhs(x0) = 0
+        self.h = Heuristic(self.g, self.goal)
+        
+        self.OPEN = queue.QueuePrior()  # store [point,priority]
+        self.OPEN.put(self.x0, [self.h[self.x0],0])
+        self.CLOSED = set()
+
+        # used for A*
+        self.done = False
+        self.Path = []
+        self.V = []
+        self.ind = 0
+
+        # initialize children list
+        self.CHILDREN = {}
+        self.getCHILDRENset()
+
+        # initialize cost list
+        self.COST = {}
+        _ = self.costset()
+
+    def costset(self):
+        NodeToChange = set()
+        for strxi in self.CHILDREN.keys():
+            children = self.CHILDREN[strxi]
+            xi = dehash(strxi)
+            toUpdate = [self.cost(xj,xi) for xj in children]
+            if strxi in self.COST:
+                # if the old cost not equal to new cost
+                diff = np.not_equal(self.COST[strxi],toUpdate)
+                cd = np.array(children)[diff]
+                for i in cd:
+                    NodeToChange.add(hash3D(i))
+                self.COST[strxi] = toUpdate
+            else:
+                self.COST[strxi] = toUpdate
+        return NodeToChange
+
+    def getCOSTset(self,strxi,xj):
+        ind, children = 0, self.CHILDREN[strxi]
+        for i in children:
+            if all(i == xj):
+                return self.COST[strxi][ind]
+            ind += 1
+            
+
+    def children(self, x):
+        allchild = []
+        resolution = self.env.resolution
+        for direc in self.Alldirec:
+            child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
+            if isinbound(self.env.boundary,child):
+                allchild.append(child)
+        return allchild
+
+    def getCHILDRENset(self):
+        for strxi in self.g.keys():
+            xi = dehash(strxi)
+            self.CHILDREN[strxi] = self.children(xi)
+        
+    def isCollide(self, x, child):
+        ray , dist = getRay(x, child) ,  getDist(x, child)
+        if not isinbound(self.env.boundary,child):
+            return True, dist
+        for i in self.env.AABB:
+            shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
+            if shot is not None:
+                dist_wall = getDist(x, shot)
+                if dist_wall <= dist:  # collide
+                    return True, dist
+        for i in self.env.balls:
+            if isinball(i, child):
+                return True, dist
+            shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)
+            if shot != []:
+                dists_ball = [getDist(x, j) for j in shot]
+                if all(dists_ball <= dist):  # collide
+                    return True, dist
+        return False, dist
+
+    def cost(self, x, y):
+        collide, dist = self.isCollide(x, y)
+        if collide: return np.inf
+        else: return dist
+            
+    def key(self,strxi,epsilion = 1):
+        return [min(self.g[strxi],self.v[strxi]) + epsilion*self.h[strxi],min(self.g[strxi],self.v[strxi])]
+
+    def path(self):
+        path = []
+        strx = self.xt
+        strstart = self.x0
+        ind = 0
+        while strx != strstart:
+            j = dehash(strx)
+            nei = self.CHILDREN[strx]
+            gset = [self.g[hash3D(xi)] for xi in nei]
+            # collision check and make g cost inf
+            for i in range(len(nei)):
+                if self.isCollide(nei[i],j)[0]:
+                    gset[i] = np.inf
+            parent = nei[np.argmin(gset)]
+            path.append([dehash(strx), parent])
+            strx = hash3D(parent)
+            if ind > 100:
+                break
+            ind += 1
+        return path
+
+    #------------------Lifelong Plannning A* 
+    def UpdateMembership(self,strxi, xi, xparent=None):
+        if strxi != self.x0:
+            self.v[strxi] = min([self.g[hash3D(j)] + self.getCOSTset(strxi,j) for j in self.CHILDREN[strxi]])
+        self.OPEN.check_remove(strxi)
+        if self.g[strxi] != self.v[strxi]:
+            self.OPEN.put(strxi,self.key(strxi))
+    
+    def ComputePath(self):
+        print('computing path ...')
+        while self.key(self.xt) > self.OPEN.top_key() or self.v[self.xt] != self.g[self.xt]:
+            strxi = self.OPEN.get()
+            xi = dehash(strxi)
+            # if g > rhs, overconsistent
+            if self.g[strxi] > self.v[strxi]: 
+                self.g[strxi] = self.v[strxi]
+                # add xi to expanded node set
+                if strxi not in self.CLOSED:
+                    self.V.append(xi)
+                self.CLOSED.add(strxi)
+            else: # underconsistent and consistent
+                self.g[strxi] = np.inf
+                self.UpdateMembership(strxi, xi)
+            for xj in self.CHILDREN[strxi]:
+                strxj = hash3D(xj)
+                self.UpdateMembership(strxj, xj)
+
+            # visualization(self)
+            self.ind += 1
+        self.Path = self.path()
+        self.done = True
+        visualization(self)
+        plt.pause(2)
+
+    def change_env(self):
+        self.env.change()
+        self.done = False
+        self.Path = []
+        self.CLOSED = set()
+        N = self.costset()
+        for strxi in N:
+            xi = dehash(strxi)
+            self.UpdateMembership(strxi,xi)
+
+if __name__ == '__main__':
+    sta = time.time()
+    Astar = Lifelong_Astar(1)
+    Astar.ComputePath()
+    Astar.change_env()
+    Astar.ComputePath()
+    plt.show()
+    print(time.time() - sta)

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -13,7 +13,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 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, \
+from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
@@ -79,5 +79,5 @@ class LRT_A_star2:
 
 
 if __name__ == '__main__':
-    T = LRT_A_star2(resolution=0.5, N=1)
+    T = LRT_A_star2(resolution=0.5, N=150)
     T.run()

Search-based Planning/Search_3D/RTA_Astar3D.py

@@ -13,7 +13,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 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, \
+from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
@@ -74,5 +74,5 @@ class RTA_A_star:
 
 
 if __name__ == '__main__':
-    T = RTA_A_star(resolution=0.5, N=500)
+    T = RTA_A_star(resolution=0.5, N=100)
     T.run()

Search-based Planning/Search_3D/env3D.py

@@ -22,6 +22,13 @@ def getblocks():
         Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
+def getAABB(blocks):
+    # used for Pyrr package for detecting collision
+    AABB = []
+    for i in blocks:
+        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
+    return AABB
+
 def getballs():
     spheres = [[16,2.5,4,2],[10,2.5,1,1]]
     Obstacles = []
@@ -29,19 +36,23 @@ def getballs():
         Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
+def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):
+    return block
+
 class env():
     def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
         self.resolution = resolution
         self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
         self.blocks = getblocks()
+        self.AABB = getAABB(self.blocks)
         self.balls = getballs()
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
 
-    def visualize(self):
-        # fig = plt.figure()
-        # TODO: do visualizations
-        return
+    def change(self):
+        newblock = add_block()
+        self.blocks = np.vstack([self.blocks,newblock])
+        self.AABB = getAABB(self.blocks)
 
 
 if __name__ == '__main__':

Search-based Planning/Search_3D/queue.py

@@ -60,3 +60,48 @@ class QueuePrior:
 
     def enumerate(self):
         return self.queue
+
+    def check_remove(self, item):
+        for (p, x) in self.queue:
+            if item == x:
+                self.queue.remove((p, x))
+
+    def top_key(self):
+        return self.queue[0][0]
+
+
+# class QueuePrior:
+#     """
+#     Class: QueuePrior
+#     Description: QueuePrior reorders elements using value [priority]
+#     """
+
+#     def __init__(self):
+#         self.queue = []
+
+#     def empty(self):
+#         return len(self.queue) == 0
+
+#     def put(self, item, priority):
+#         count = 0
+#         for (p, x) in self.queue:
+#             if x == item:
+#                 self.queue[count] = (priority, item)
+#                 break
+#             count += 1
+#         if count == len(self.queue):
+#             heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+
+#     def get(self):
+#         return heapq.heappop(self.queue)[1]  # pop out the smallest item
+
+#     def enumerate(self):
+#         return self.queue
+
+#     def check_remove(self, item):
+#         for (p, x) in self.queue:
+#             if item == x:
+#                 self.queue.remove((p, x))
+
+#     def top_key(self):
+#         return self.queue[0][0]

Search-based Planning/Search_3D/utils3D.py

@@ -5,12 +5,6 @@ def getRay(x, y):
     direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
     return np.array([x, direc])
 
-def getAABB(blocks):
-    AABB = []
-    for i in blocks:
-        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
-    return AABB
-
 def getDist(pos1, pos2):
     return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
 
@@ -75,7 +69,7 @@ def isCollide(initparams, x, direc):
     ray , dist = getRay(x, child) ,  getDist(x, child)
     if not isinbound(initparams.env.boundary,child):
         return True, child
-    for i in initparams.AABB:
+    for i in initparams.env.AABB:
         shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
         if shot is not None:
             dist_wall = getDist(x, shot)