From 892c3a0e4643a69c92e98a28df0ec6a94ad87d20 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Mon, 13 Jul 2020 23:29:54 -0700 Subject: [PATCH] 'D*Lite_and_qp' --- Search-based Planning/Search_3D/Astar3D.py | 9 +- Search-based Planning/Search_3D/Dstar3D.py | 53 +++--- .../Search_3D/DstarLite3D.py | 151 ++++++++++++++++++ Search-based Planning/Search_3D/LP_Astar3D.py | 20 +-- .../Search_3D/RTA_Astar3D.py | 2 +- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3296 -> 3495 bytes .../__pycache__/queue.cpython-37.pyc | Bin 2999 -> 4585 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 6306 -> 6418 bytes .../Search_3D/bidirectional_Astar3D.py | 14 +- Search-based Planning/Search_3D/queue.py | 42 ++++- Search-based Planning/Search_3D/utils3D.py | 16 +- 11 files changed, 245 insertions(+), 62 deletions(-) create mode 100644 Search-based Planning/Search_3D/DstarLite3D.py 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 3cd3c71112e5835c1d819a72d1e7c6f44713b8d8..692c02f5f496cf91e11751fdc2869e8246eda14f 100644 GIT binary patch delta 1499 zcmbVL&2Jk;6rVRUyIz0d#*Q63vD172aa-sC5xzqC<^&-(G}t>ccTQX;Lf z#3If`a|ERI1q2rk{0VS}3lb~`xK@H|&%C#e-IU(g(Y$$Izu%jWeSg)ySuMPt&u18Z z?f3HT-{(Fr{JOF3WkP6g_>HJBW$jIWBifoWVE*>Z<8S^T;d#icA?OX9BhS3ekM=c- zz!OvWzpE#|tOv~HS_I%(=11NF+x)|~@+tJ@uQQYd&o&KlwdCyI11x-xlMpCcbrXnp zswGqvMOq?RW};;N6feA$#LgxxbVb@6u}DKlk?0m_=qQ;(S$9s;OJ*YLlSi-wnqw1! zQ^$gMUhV<=oRG>byi{&c|CA#Mp3Nv${+p6uisoy%18wuO{4m>2zuO($@H@kUtL9~E z+bJXBTc$3UuJuLp5*iF_94n`zi0gTtgC>;0tl7I08#B^4lb6t;Vh6_mcnNIZ{BR!CNY(PYXK-8EnQhFdmQDtK*ux1%GhIGBTwMQAO zcaD&_@@NTJEe~`Fu`=XkR%KHT>~`;c7J$01%a|oY2%s`y9`AE?|A2tYo~R@?o_9@G zJgKWmsue;ARTd`alavR~G0xoO+eIYA((VePY0AEh*rA8pbZY^N>Qm%(SMiT$eFq~q z^;(ed=tk6}rZaz}b|>p<8(%!9)B8MUtH~oRyB+VMC?m)+4q=iyD=2K_8YpWJTU_aSU z%#XV`dogs>1tLhq#e_xO8C*tX^eT$k50--rjwQ(9x?IG*{ delta 1270 zcmY*XO>Y}T7@l`#zr3-HyLI9uZIY%jkZqc(htl*TP$(@D2vzL~5mBu+?8%l@>#+My7r)o>lE9uK@#Ja|GYhe~Hf<9#To7FY==R zPdAObk8|6b4rg2vJB>JVL!n0L&fHM&^qMj6oVa^>)0nTEi`9v_3YmUsZkIFn*+F~| zKb7sC)FrFwT*PF*Gr6YUvwoU=2Y)cIaLwlHidi-mMP2BBt=peEXe;5)ujN}DN3k6? zQ_&4 ziKFpbSjdhbtznok;&s+w6Ao;D`wON3TQ|}R7{EuMV1`A!!%2Hejd+nbQ?8{KBO|Tn zzUBx<&|o7RN-=}{V1scMc>6WXiRFXolPj{0eUK_LQJPU~+?ZfMFpE!%bsQ`+Nas|Z zCY>K&(my-*?p&5lym`4out@MOLMB>qDwl{^VZR@DBT3ITGearE!%T!xq!&x`u&VEt zmcZ9vmYPS7yhbkYBu|*}K^ynZ7H`pRXAsIYv=?xlJcT^yet;x-h;aHgTyVjIGOuwL z%6QW68aRlqxMEsR2M#qh10J_{8DA6G9bAU~uk>Qm$3(>_H;G+F@Qu7Lc^i2K1362E z^PS|jUHz;3_k9{(t`d|976|B8d|qU^Pr~`=%#?8wr{CybroK8_C;R&ZR8KxQ@16(J z?6Qh5jXR;IkJGKXH=LP-q%5pncaiE{$cTNT-XcK z{%$YbZExS`9}-*0tb)i5jLdj1$wYFP6wTb!KbHSk`;ZJGNyE(i2vHW@-a_?_xr#>h Mer3J1N4HDse;70Vn*aa+ 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 6871e7f84465862e00e9ed3414b5452d84a6cae1..eb24b07f97254f69d81c04316e0c8e9cf2f7ddf3 100644 GIT binary patch delta 1496 zcmYjR&2Aev5GMCWTFI82#*P1q1Vogi-old5^wh#F;?zb17~8b?mXop>NR#(7BfYeTQ_0>sTQ%vmA0b9Dd($|6YAxDL*ZhiUiv0 z7p3OEOK;0-jbFgxgwq~rWMA5#+BO=-PlOxXd_%Z-X*5i*EN+8kgJpr0<9V?1VA)_f zyZ}}KtQ@Du#4Y~y>X}1l@7_5BW8og0BJZQBq*HS(-Cwx>Hd>)i(*Mk_Cw$=zX*hh5 zm*89AW!StlD;)SiM|jbTpGSWd8MGZ1Nk5dn8nUy27zlRa#e%aiV0}Me=(@$uJASLf zl-Ct8d%oEV+kuM?q9^(yP)v$`_`BEB;W|o1@ptt)5BFPfEgGs$7}P@9u76YS`6tqo z!#c)1t94YrSNV8!RUcjT`%xCsQ+^m!gHVZBM@^}{GGTDfHc3d6_^7EeuFNTpEKWh& zoWKo@!%=x#CnR{)SEB!eo0}QY*=!LqmBZ$#A8;XO1z3_Q414j+ZiRzD&GPyKOZlU{ zgXh2Q?YdOv5ZU~-O8+V5Cb9t50Z8nCA~XlRNJqDCP^~qae&DNSvt^J_% zs&!NkLAiE@zy?lyfoie|dS+`VZ-5~Zv5;j@#~|?{C}I==m#mLUb9ADCY8ki=4FN*Y zz+m8OVDhqzRO00`c*Y2hkr5jcT!PoH; zf67EZQp4JaMPbBFW!TrW;@o_zeny9GPSdB^+>&=8dkv1pYUX?l=~@|9 znplKiZXjTrw_tbza?PVdZv&4G^iO$g3k7d-Bb^r4>BDr#Sg*(m?CIh@%`Y8&HiuY+ zQL`(CX{Ycg^WK#B=L$aYUY>i4V9*y5>iEztrXPwMcd)XuzZ~YK50dxrfA{nO*v~Ku yx;U0aot#t9VoF?tI!6|>y8aSJn4G+WY8BN32(z00v9NRts}^tR80BT?cmD@bc4Kt_ delta 168 zcmaEXPM36EB`6A+c{mm;nF64=llcPQMbf@x)TqQ!tl3xLt9WXsS2_g7 zX0^IqZ~N8iW2NXgP;IvYx$NWQ$GX3_z4M6c@{#yuZ%o%H;&L#Ka_flYEXJ&q LX;}%=k{bR2DMFCo delta 573 zcmX9*O>fgc5ZzhZiQ_tHnmD!-477ZeN@z>nyOAHfe`Hr>^{d44;y^Q=9(b$qjUuUNDgt#_-1c79T< z!=K6W16ZEyH8-xKhyEDCd8}h&%)&CB!+`Pvp2rK6D|itXDOa(HOL&PY9lVTJNU23G zUVYBRZ@dwah^QJVG! zJ4u-D?8b2qBcYz<4CaWP>Yd#r{X#6M5HP546LiSUOsFijC9^ z-Z#}^s}^pPTZ?u8qI={AE?*bQMze8h3|_w2dnMFcgz}ERQJ!6}NuOd%L02C8Xq#oz KvgS=o4gL=jfOJU! 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: