diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 8a5b430..938a22d 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/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) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index a501429..fda8797 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/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() diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py new file mode 100644 index 0000000..c2dc364 --- /dev/null +++ b/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)) + \ No newline at end of file diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py index 33ed5c0..a4df422 100644 --- a/Search-based Planning/Search_3D/LP_Astar3D.py +++ b/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) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search-based Planning/Search_3D/RTA_Astar3D.py index 818dd24..5e93ebf 100644 --- a/Search-based Planning/Search_3D/RTA_Astar3D.py +++ b/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) diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index 3cd3c71..692c02f 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc index 6871e7f..eb24b07 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/queue.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 cf4416e..c9b85e7 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/bidirectional_Astar3D.py b/Search-based Planning/Search_3D/bidirectional_Astar3D.py index 439c28d..da41482 100644 --- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py +++ b/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 diff --git a/Search-based Planning/Search_3D/queue.py b/Search-based Planning/Search_3D/queue.py index 9531c36..b4e4c0b 100644 --- a/Search-based Planning/Search_3D/queue.py +++ b/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 = '' + + 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: # """ diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 636d756..90046f5 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/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: