From 8dd8a6e220d8e6351888c51f78e2aaf3b8ec5cdd Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sun, 19 Jul 2020 17:17:06 -0700 Subject: [PATCH] 'AnyDstar' --- .../Search_3D/Anytime_Dstar3D.py | 114 ++++++++++++++++++ Search-based Planning/Search_3D/Astar3D.py | 2 +- Search-based Planning/Search_3D/Dstar3D.py | 2 +- .../Search_3D/DstarLite3D.py | 35 +++--- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3499 -> 3539 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 10371 -> 10429 bytes Search-based Planning/Search_3D/utils3D.py | 10 +- 7 files changed, 138 insertions(+), 25 deletions(-) create mode 100644 Search-based Planning/Search_3D/Anytime_Dstar3D.py diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py new file mode 100644 index 0000000..dfd6380 --- /dev/null +++ b/Search-based Planning/Search_3D/Anytime_Dstar3D.py @@ -0,0 +1,114 @@ +# check paper of +# [Likhachev2005] +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.utils3D import getDist, heuristic_fun, getNearest, isinbound, \ + cost, children, StateSpace +from Search_3D.plot_util3D import visualization +from Search_3D import queue +import time + +class Anytime_Dstar(object): + + 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.settings = 'CollisionChecking' # for collision checking + self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) + self.OPEN = queue.MinheapPQ() + 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.key(self.xt)) + self.INCONS = set() + self.CLOSED = set() + + # 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 getchildren(self, xi): + if xi not in self.CHILDREN: + allchild = children(self, xi) + self.CHILDREN[xi] = set(allchild) + return self.CHILDREN[xi] + + 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 Anytime D star + + def key(self, s, epsilon=1): + if self.getg(s) > self.getrhs(s): + return [self.rhs[s] + epsilon * heuristic_fun(self, s, self.x0), self.rhs[s]] + else: + return [self.getg(s) + heuristic_fun(self, s, self.x0), self.getg(s)] + + def UpdateState(self, s): + if s not in self.CLOSED: + # TODO if s is not visited before + self.g[s] = np.inf + if getDist(s, self.xt) <= self.env.resolution: + self.rhs[s] = min([self.getcost(s, s_p) + self.getg(s_p) for s_p in self.getchildren(s)]) + self.OPEN.check_remove(s) + if self.getg(s) != self.getrhs(s): + if s not in self.CLOSED: + self.OPEN.put(s, self.key(s)) + else: + self.INCONS.add(s) + + def ComputeorImprovePath(self): + pass + + def Main(self): + pass + +if __name__ == '__main__': + AD = Anytime_Dstar(resolution = 1) + AD.Main() \ No newline at end of file diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index fa27d0f..8d36332 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -29,7 +29,7 @@ class Weighted_A_star(object): (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.settings = 'NonCollisionChecking' self.env = env(resolution=resolution) self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) self.g = {self.start:0,self.goal:np.inf} diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index bbc4e19..09d4cea 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -165,7 +165,7 @@ class D_star(object): # when the environemnt changes over time for i in range(5): - self.env.move_block(a=[0.25, 0, 0], s=0.5, block_to_move=1, mode='translation') + self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation') 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) diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index 7742dc6..30ed925 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search-based Planning/Search_3D/DstarLite3D.py @@ -7,12 +7,10 @@ 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, \ - isCollide, cost, obstacleFree, children, StateSpace +from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \ + cost, children, StateSpace from Search_3D.plot_util3D import visualization from Search_3D import queue -import pyrr import time class D_star_Lite(object): @@ -23,13 +21,14 @@ class D_star_Lite(object): (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)} + (-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.settings = 'CollisionChecking' # for collision checking self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # self.OPEN = queue.QueuePrior() self.OPEN = queue.MinheapPQ() @@ -51,16 +50,6 @@ class D_star_Lite(object): 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,range_changed=None, new=None, old=None, mode=False): # scan graph for changed cost, if cost is changed update it CHANGED = set() @@ -86,6 +75,16 @@ class D_star_Lite(object): self.COST[xi][xj] = cost(self, xi, xj) return CHANGED + 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 getchildren(self, xi): if xi not in self.CHILDREN: allchild = children(self, xi) 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 12c44398200e40eb3bcce81c62897fe4411e1bf4..e11383bec79f751c1c08c716b0b9d562f6df76ff 100644 GIT binary patch delta 775 zcmZXQKX21O7{-0JbBWU=(2}H-2nw`N3?U#@P{M$qsXBJ)#uA0vN^C+Ly}pVfMe2}+ zg#m5=sY{E*fEW-e65oJ{1$993Ct&3p@IDTrNLcdk-g}<+&)t2f9o0&!#bVyz@90Ol zU7z_>S}{ea*Uc}xUS?#pEoF~{CtnDTZN*AxhjJ|bMA@-04R$2|S@xs{W|a}<0+Tf- zfSXrSW2}WPSoCttQW~E?e)}XUQJ40bW@Sd5+HaN*kX~Vi2^<;GpMkJq`w4S0s7z>*z6c~xs_Xl(dQD#WNKqXeJi0becpxVQZ>d_+h zMeMC%UnO?ji=MiYu0s(UgmWe4PQZVdhz>Oj9>rwb&~KfuchY|8!IvT846dD)Qxmmf zhiWj=l1Ux5O}(H0yp~=dwX+ca-v9U(bRGs?!Zjyd??L#dn|?FZUAzhLK7gUL3LNK5 z#_EN_nyBhtVZMBq{ltu7ev)VLq-0s|6<)Wlz_3;D#7|`ODD<}!mC&ojc$j$*s}A8A PJCj)}u>KpfUA+Apvx2V@ delta 772 zcmZXQ&ubGw6vuaFvzs5?#H1!|O4L?c#jUMSMB9Ui(b9|H#hV_&+Fjx4<1BlkK#$HcohT@_2xmCKSJ<7@O?MM7TjS!^X7fukD2$g^|n>HQ7)GZ{(e1~ z?EJ31tE@`lryKI)jxvYBx1Nci(J@tSXhddYjl_XvkAyT>vG})HvwYB2MwAaF`wAh{ zqAHEBPjo@Y&oj%JT|s>37&4KU)vB_mrY3if?;-k9orU^tgULcvz&>NBsUeF|btI>a zJt;DqgQtDtj^P&eKbSwnqAPTXf@IKptinN`1V3)WZED$%buM{CN*CI_o*z=MnSQsP z7V50OF=#2hXTNGLVHCpV)jU{L-sj|gHUCHKoMn*~fb$G(b^}GTIL_f{dvP4}eQMxf zc|_i}Ha*`b6Cn~nFT%L#1keJCz)8Rb9ELV~L7(PfSuururOiYU)fN$O8tz=|sRwi> zeNmj6zW|50O}L$wTOt3E79Qa(V=SlNieJ{U?Cka{P|(uN5wdBOp~V)>;BX`_)6%b{ z_v_hxGdU0Ezabo6nodE$I9xfddKHc%)zVKB-Gz&AUIWmSt^Kmsd?jGmm&Sv=5_hX EU%sZOO8@`> 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 d55e468bf1280c77826d6404da0c73eb9d847a7d..df0f89fb254c8e2db7339f0b273c9b7db4f87304 100644 GIT binary patch delta 1266 zcmZ8g&u<%55Z>7z{uN>=ZrV7(N!xX8XX7NK4W*E_5IarOrX>)JfLf8Pjh}-Z+iPjO z1ea_YRSx2s@NPXI%71`-LeLvm4jfsDGgr7H!OW%vsZV;}oA=H4X6Egi?QcHcj2DN8 zV+xJVZ=;p$wUc-fdM7y%z9kak+!34{FV5W-m&D~GD31zLq>q&Hd2vN#XuTlb7Fm%a z&zP7N7BNZj&dHzS;K~>rjz9QVspV>^Z&S~d+Lffz2T%l*6l;0#9o#c6=$w=WuNuu2 zn%!?Cgj0uFQt?2j}biW@pG~r z84XQw*1Y^G@(gAMR`fZ*y!<1UPF^FK54I15vbx%%`H1EhI3FCmK9q+W1A8c?!Sdj7 zyr;g&a!fGH6IfAvi|r)>Ul)z8i?6e(wHv~Rd%jw4;VcVo1>p)KH(^yCj68)|nM|a{ zf?%uUx5EF&!GgS>xC3_w$BEy=L!1Qz-JxDwGh+LO% zY}>6?7ncLyd&J%)%QEm1EvN5mRO^kFz{23~ixCy_(lIk~#$1uQnU&H^hJpko`Mr4& zK9RqgnV}$UP}rP=^cUr|eQcG*vV`O(?XcT=2kC z`|5!v)C0pwxYQ%ecv@fIBIZEzbfJ-Q-7|2_)pn_0Jn9=_bz*mj-8yAm<1`ZWloIrm zKf2LzRywU#qub~>lPmRFwb^h!UBez#QjWfBJ9XQ2ZO00sE49ol6y!qY>O>485QC7K zP$Sf8Ayub6uedMwGk*>5)8YR<+)eo?TZAX_r))K@t_3|z&C9JUFXwK+jw2k^k*R|2bbs3$005SQ|^xVIefED`y delta 1163 zcmYjQ&2Jl35P!2Dwu6&Ybt!eyBn=zen{{F;H>C<6X%Z)GNR@eA8>mYd2{AS+o{dQ*d?d!?X_t8p7C#XqU)vTO_^?=T50Ulrcim1^6aVv5mqM%7442y>mGt?{7rNz&YCt$VS z>o^9;h*)$eo+XJ7whz;J<=PI}XJosW5sT5QaHaKSv`>Y})?xRKGA^YUlaL{>I&fUZ zZxHwzFV{UBlc8EGb06k?rBuZcDR@iVI=uw5;-}N!UJeLd$~uc%)jKt}Q1|R&1w&F8 zDR zRDjm=$?Ld7KHsruyCvK6Y?r>uN&DI4C`^gl$t-k={bU?=#bNTVu3eJoI=6Z symHO)b;sQvo5crGApZiqAz@sCoJE`>@O7Fi^l)Cx3@<=L>