'D*Lite_and_qp'

Search-based Planning/Search_3D/Astar3D.py

@@ -40,7 +40,7 @@ class Weighted_A_star(object):
         self.Path = []
         self.ind = 0
         self.x0, self.xt = self.start, self.goal
-        self.OPEN = queue.QueuePrior()  # store [point,priority]
+        self.OPEN = queue.MinheapPQ()  # store [point,priority]
         self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0))  # item, priority = g + h
         self.lastpoint = self.x0
 
@@ -52,7 +52,7 @@ class Weighted_A_star(object):
             if xi not in self.CLOSED:
                 self.V.append(np.array(xi))
             self.CLOSED.add(xi)  # add the point in CLOSED set
-            if xi == xt:
+            if getDist(xi,xt) < self.env.resolution:
                 break
             # visualization(self)
             for xj in children(self,xi):
@@ -80,7 +80,7 @@ class Weighted_A_star(object):
 
         self.lastpoint = xi
         # if the path finding is finished
-        if xt in self.CLOSED:
+        if self.lastpoint in self.CLOSED:
             self.done = True
             self.Path = self.path()
             if N is None:
@@ -113,7 +113,8 @@ class Weighted_A_star(object):
 
 
 if __name__ == '__main__':
+    
+    Astar = Weighted_A_star(1)
     sta = time.time()
-    Astar = Weighted_A_star(0.5)
     Astar.run()
     print(time.time() - sta)

Search-based Planning/Search_3D/Dstar3D.py

@@ -27,44 +27,23 @@ class D_star(object):
         self.env = env(resolution=resolution)
         self.X = StateSpace(self.env)
         self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
+        # self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
         self.b = defaultdict(lambda: defaultdict(dict))  # back pointers every state has one except xt.
         self.OPEN = {}  # OPEN list, here use a hashmap implementation. hash is point, key is value
-        self.h = self.initH()  # estimate from a point to the end point
-        self.tag = self.initTag()  # set all states to new
+        self.h = {}  # estimate from a point to the end point
+        self.tag = {}  # set all states to new
         self.V = set()  # vertice in closed
-        # initialize cost set
-        # self.c = initcost(self)
         # for visualization
         self.ind = 0
         self.Path = []
         self.done = False
         self.Obstaclemap = {}
 
-    def update_obs(self):
-        for xi in self.X:
-            print('xi')
-            self.Obstaclemap[xi] = False
-            for aabb in self.env.blocks:
-                self.Obstaclemap[xi] = isinbound(aabb, xi)
-            if self.Obstaclemap[xi] == False:
-                for ball in self.env.balls:
-                    self.Obstaclemap[xi] = isinball(ball, xi)
-
-    def initH(self):
-        # h set, all initialzed h vals are 0 for all states.
-        h = {}
-        for xi in self.X:
-            h[xi] = 0
-        return h
-
-    def initTag(self):
-        # tag , New point (never been in the OPEN list)
-        #       Open point ( currently in OPEN )
-        #       Closed (currently in CLOSED)
-        t = {}
-        for xi in self.X:
-            t[xi] = 'New'
-        return t
+    def checkState(self, y):
+        if y not in self.h:
+            self.h[y] = 0
+        if y not in self.tag:
+            self.tag[y] = 'New'
 
     def get_kmin(self):
         # get the minimum of the k val in OPEN
@@ -97,18 +76,26 @@ class D_star(object):
         self.h[x], self.tag[x] = h_new, 'Open'
 
     def process_state(self):
+        # main function of the D star algorithm, perform the process state 
+        # around the old path when needed.
         x, kold = self.min_state()
         self.tag[x] = 'Closed'
         self.V.add(x)
         if x is None:
             return -1
+        # check if 1st timer x
+        self.checkState(x)
         if kold < self.h[x]:  # raised states
             for y in children(self, x):
+                # check y
+                self.checkState(y)
                 a = self.h[y] + cost(self, y, x)
                 if self.h[y] <= kold and self.h[x] > a:
                     self.b[x], self.h[x] = y, a
         if kold == self.h[x]:  # lower
             for y in children(self, x):
+                # check y
+                self.checkState(y)
                 bb = self.h[x] + cost(self, x, y)
                 if self.tag[y] == 'New' or \
                         (self.b[y] == x and self.h[y] != bb) or \
@@ -117,6 +104,8 @@ class D_star(object):
                     self.insert(y, bb)
         else:
             for y in children(self, x):
+                 # check y
+                self.checkState(y)
                 bb = self.h[x] + cost(self, x, y)
                 if self.tag[y] == 'New' or \
                         (self.b[y] == x and self.h[y] != bb):
@@ -135,6 +124,7 @@ class D_star(object):
         xparent = self.b[x]
         if self.tag[x] == 'Closed':
             self.insert(x, self.h[xparent] + cost(self, x, xparent))
+
     def modify(self, x):
         self.modify_cost(x)
         while True:
@@ -158,6 +148,7 @@ class D_star(object):
     def run(self):
         # put G (ending state) into the OPEN list
         self.OPEN[self.xt] = 0
+        self.tag[self.x0] = 'New'
         # first run
         while True:
             # TODO: self.x0 =
@@ -178,7 +169,7 @@ class D_star(object):
             self.env.move_block(a=[0, 0, -0.25], s=0.5, block_to_move=0, mode='translation')
             # travel from end to start
             s = tuple(self.env.start)
-            self.V = set()
+            # self.V = set()
             while s != self.xt:
                 if s == tuple(self.env.start):
                     sparent = self.b[self.x0]
@@ -196,5 +187,5 @@ class D_star(object):
 
 
 if __name__ == '__main__':
-    D = D_star(1)
+    D = D_star(0.75)
     D.run()

Search-based Planning/Search_3D/DstarLite3D.py

@@ -0,0 +1,151 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+import os
+import sys
+from collections import defaultdict
+
+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, g_Space, Heuristic, heuristic_fun, getNearest, isinbound, isinball, \
+    cost, obstacleFree, children, StateSpace
+from Search_3D.plot_util3D import visualization
+import queue
+import pyrr
+import time
+
+class D_star_Lite(object):
+    # Original version of the D*lite
+    def __init__(self, resolution = 1):
+        self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
+        self.env = env(resolution=resolution)
+        # self.X = StateSpace(self.env)
+        # self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
+        self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
+        self.OPEN = queue.QueuePrior()
+        self.km = 0
+        self.g = {} # all g initialized at inf
+        self.rhs = {self.xt:0} # rhs(x0) = 0
+        self.h = {}
+        self.OPEN.put(self.xt, self.CalculateKey(self.xt))
+
+        # init children set:
+        self.CHILDREN = {}
+        # init cost set
+        self.COST = defaultdict(lambda: defaultdict(dict))
+
+        # for visualization
+        self.V = set()  # vertice in closed
+        self.ind = 0
+        self.Path = []
+        self.done = False
+
+    def getcost(self, xi, xj):
+        # use a LUT for getting the costd
+        if xi not in self.COST:
+            for (xj,xjcost) in children(self, xi, settings=1):
+                self.COST[xi][xj] = cost(self, xi, xj, xjcost)
+        # this might happen when there is a node changed. 
+        if xj not in self.COST[xi]:
+            self.COST[xi][xj] = cost(self, xi, xj)
+        return self.COST[xi][xj]
+
+    def updatecost(self):
+        # TODO: update cost when the environment is changed
+        pass
+
+    def getchildren(self, xi):
+        if xi not in self.CHILDREN:
+            allchild = children(self, xi)
+            self.CHILDREN[xi] = set(allchild)
+        return self.CHILDREN[xi]
+
+    def updatechildren(self):
+        # TODO: update children set when the environment is changed
+        pass
+
+    def geth(self, xi):
+        # when the heurisitic is first calculated
+        if xi not in self.h:
+            self.h[xi] = heuristic_fun(self, xi, self.x0)
+        return self.h[xi]
+
+    def getg(self, xi):
+        if xi not in self.g:
+            self.g[xi] = np.inf
+        return self.g[xi]
+
+    def getrhs(self, xi):
+        if xi not in self.rhs:
+            self.rhs[xi] = np.inf
+        return self.rhs[xi]
+#-------------main functions for D*Lite-------------
+
+    def CalculateKey(self, s, epsilion = 1):
+        return [min(self.getg(s), self.getrhs(s)) + epsilion * self.geth(s) + self.km, min(self.getg(s), self.getrhs(s))]
+
+    def UpdateVertex(self, u):
+        # if still in the hunt
+        if not getDist(self.xt, u) <= self.env.resolution: # originally: u != s_goal
+            self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)])
+        # if u is in OPEN, remove it
+        self.OPEN.check_remove(u)
+        # if rhs(u) not equal to g(u)
+        if self.getg(u) != self.getrhs(u):
+            self.OPEN.put(u, self.CalculateKey(u))
+        
+    def ComputeShortestPath(self):
+        while self.OPEN.top_key() < self.CalculateKey(self.x0) or self.getrhs(self.x0) != self.getg(self.x0) :
+            kold = self.OPEN.top_key()
+            u = self.OPEN.get()
+            self.V.add(u)
+            if getDist(self.x0, u) <= self.env.resolution:
+                break
+            # visualization(self)
+            if kold < self.CalculateKey(u):
+                self.OPEN.put(u, self.CalculateKey(u))
+            if self.getg(u) > self.getrhs(u):
+                self.g[u] = self.rhs[u]
+            else:
+                self.g[u] = np.inf
+                self.UpdateVertex(u)
+            for s in self.getchildren(u):
+                self.UpdateVertex(s)
+
+            self.ind += 1
+
+    def main(self):
+        s_last = self.x0
+        s_start = self.x0
+        self.ComputeShortestPath()
+        # while s_start != self.xt:
+        # while getDist(s_start, self.xt) > self.env.resolution:
+        #     newcost, allchild = [], []
+        #     for i in children(self, s_start):
+        #         newcost.append(cost(self, i, s_start) + self.g[s_start])
+        #         allchild.append(i)
+        #     s_start = allchild[np.argmin(newcost)]
+        #     #TODO: move to s_start
+        #     #TODO: scan graph or costs changes 
+        #     # self.km = self.km + heuristic_fun(self, s_start, s_last)
+        #     # for all directed edges (u,v) with changed edge costs
+        #     #   update edge cost c(u,v)
+        #     #   updatevertex(u)
+        #     self.ComputeShortestPath()
+
+if __name__ == '__main__':
+    a = time.time()
+    D_lite = D_star_Lite(1)
+    # D_lite.UpdateVertex(D_lite.x0)
+    D_lite.main()
+    print('used time (s) is ' + str(time.time() - a))
+            

Search-based Planning/Search_3D/LP_Astar3D.py

@@ -30,11 +30,11 @@ class Lifelong_Astar(object):
         self.g = g_Space(self)
         self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
         self.x0, self.xt = self.start, self.goal
-        self.v = g_Space(self) # rhs(.) = g(.) = inf
-        self.v[self.start] = 0 # rhs(x0) = 0
+        self.rhs = g_Space(self) # rhs(.) = g(.) = inf
+        self.rhs[self.start] = 0 # rhs(x0) = 0
         self.h = Heuristic(self.g, self.goal)
         
-        self.OPEN = queue.QueuePrior()  # store [point,priority]
+        self.OPEN = queue.MinheapPQ()  # store [point,priority]
         self.OPEN.put(self.x0, [self.h[self.x0],0])
         self.CLOSED = set()
 
@@ -115,7 +115,7 @@ class Lifelong_Astar(object):
         else: return dist
             
     def key(self,xi,epsilion = 1):
-        return [min(self.g[xi],self.v[xi]) + epsilion*self.h[xi],min(self.g[xi],self.v[xi])]
+        return [min(self.g[xi],self.rhs[xi]) + epsilion*self.h[xi],min(self.g[xi],self.rhs[xi])]
 
     def path(self):
         path = []
@@ -141,18 +141,18 @@ class Lifelong_Astar(object):
     #------------------Lifelong Plannning A* 
     def UpdateMembership(self, xi, xparent=None):
         if xi != self.x0:
-            self.v[xi] = min([self.g[j] + self.getCOSTset(xi,j) for j in self.CHILDREN[xi]])
+            self.rhs[xi] = min([self.g[j] + self.getCOSTset(xi,j) for j in self.CHILDREN[xi]])
         self.OPEN.check_remove(xi)
-        if self.g[xi] != self.v[xi]:
+        if self.g[xi] != self.rhs[xi]:
             self.OPEN.put(xi,self.key(xi))
     
     def ComputePath(self):
         print('computing path ...')
-        while self.key(self.xt) > self.OPEN.top_key() or self.v[self.xt] != self.g[self.xt]:
+        while self.key(self.xt) > self.OPEN.top_key() or self.rhs[self.xt] != self.g[self.xt]:
             xi = self.OPEN.get()
             # if g > rhs, overconsistent
-            if self.g[xi] > self.v[xi]: 
-                self.g[xi] = self.v[xi]
+            if self.g[xi] > self.rhs[xi]: 
+                self.g[xi] = self.rhs[xi]
                 # add xi to expanded node set
                 if xi not in self.CLOSED:
                     self.V.append(xi)
@@ -182,8 +182,10 @@ class Lifelong_Astar(object):
 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/RTA_Astar3D.py

@@ -30,7 +30,7 @@ class RTA_A_star:
         # Initialize hvalues at infinity
         self.localhvals = []
         nodeset, vals = [], []
-        for (_,xi) in self.Astar.OPEN.enumerate():
+        for (_,_,xi) in self.Astar.OPEN.enumerate():
             nodeset.append(xi)
             vals.append(self.Astar.g[xi] + self.Astar.h[xi])
         j, fj = nodeset[np.argmin(vals)], min(vals)

Search-based Planning/Search_3D/bidirectional_Astar3D.py

@@ -32,8 +32,8 @@ class Weighted_A_star(object):
         self.env = env(resolution = resolution)
         self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
         self.g = {self.start:0,self.goal:0}
-        self.OPEN1 = queue.QueuePrior() # store [point,priority]
-        self.OPEN2 = queue.QueuePrior()
+        self.OPEN1 = queue.MinheapPQ() # store [point,priority]
+        self.OPEN2 = queue.MinheapPQ()
         self.Parent1, self.Parent2 = {}, {}
         self.CLOSED1, self.CLOSED2 = set(), set()
         self.V = []
@@ -76,10 +76,7 @@ class Weighted_A_star(object):
                     if a < self.g[xj]:
                         self.g[xj] = a
                         self.Parent1[xj] = xi
-                        if (a, xj) in self.OPEN1.enumerate():
-                            self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))
-                        else:
-                            self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))
+                        self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))
             if conf == 2:
                 if xj not in self.CLOSED2:
                     if xj not in self.g:
@@ -91,10 +88,7 @@ class Weighted_A_star(object):
                     if a < self.g[xj]:
                         self.g[xj] = a
                         self.Parent2[xj] = xi
-                        if (a, xj) in self.OPEN2.enumerate():
-                            self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
-                        else:
-                            self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
+                        self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
             
     def path(self):
         # TODO: fix path

Search-based Planning/Search_3D/queue.py

@@ -1,6 +1,6 @@
 import collections
 import heapq
-
+import itertools
 
 class QueueFIFO:
     """
@@ -69,6 +69,46 @@ class QueuePrior:
     def top_key(self):
         return self.queue[0][0]
 
+class MinheapPQ:
+    """
+    A priority queue based on min heap, which takes O(logn) on element removal
+    https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes
+    """
+    def __init__(self):
+        self.pq = [] # lis of the entries arranged in a heap
+        self.entry_finder = {} # mapping of the item entries
+        self.counter = itertools.count() # unique sequence count
+        self.REMOVED = '<removed-item>'
+    
+    def put(self, item, priority):
+        '''add a new task or update the priority of an existing item'''
+        if item in self.entry_finder:
+            self.check_remove(item)
+        count = next(self.counter)
+        entry = [priority, count, item]
+        self.entry_finder[item] = entry
+        heapq.heappush(self.pq, entry)
+
+    def check_remove(self, item):
+        if item not in self.entry_finder:
+            return
+        entry = self.entry_finder.pop(item)
+        entry[-1] = self.REMOVED
+
+    def get(self):
+        """Remove and return the lowest priority task. Raise KeyError if empty."""
+        while self.pq:
+            priority, count, item = heapq.heappop(self.pq)
+            if item is not self.REMOVED:
+                del self.entry_finder[item]
+                return item
+        raise KeyError('pop from an empty priority queue')
+
+    def top_key(self):
+        return self.pq[0][0]
+        
+    def enumerate(self):
+        return self.pq
 
 # class QueuePrior:
 #     """

Search-based Planning/Search_3D/utils3D.py

@@ -141,9 +141,10 @@ def isCollide(initparams, x, child, dist):
     return False, dist
 
 
-def children(initparams, x):
+def children(initparams, x, settings = 0):
     # get the neighbor of a specific state
     allchild = []
+    allcost = []
     resolution = initparams.env.resolution
     for direc in initparams.Alldirec:
         child = tuple(map(np.add, x, np.multiply(direc, resolution)))
@@ -153,8 +154,11 @@ def children(initparams, x):
             continue
         if isinbound(initparams.env.boundary, child):
             allchild.append(child)
-            # initparams.Alldirec[direc]*resolution
-    return allchild
+            allcost.append((child,initparams.Alldirec[direc]*resolution))
+    if settings == 0:
+        return allchild
+    if settings == 1:
+        return allcost
 
 
 def obstacleFree(initparams, x):
@@ -167,15 +171,15 @@ def obstacleFree(initparams, x):
     return True
 
 
-def cost(initparams, i, j, dist=None, settings=0):
+def cost(initparams, i, j, dist=None, settings='Euclidean'):
     collide, dist = isCollide(initparams, i, j, dist)
     # collide, dist= False, getDist(i, j)
-    if settings == 0:
+    if settings == 'Euclidean':
         if collide:
             return np.inf
         else:
             return dist
-    if settings == 1:
+    if settings == 'Manhattan':
         if collide:
             return np.inf
         else: