diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py index 3bb9d5d..026a1c0 100644 --- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search-based Planning/Search_3D/Anytime_Dstar3D.py @@ -10,39 +10,40 @@ 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 + 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)} + (-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.settings = 'CollisionChecking' # for collision checking self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.OPEN = queue.MinheapPQ() - self.g = {} # all g initialized at inf + self.g = {} # all g initialized at inf self.h = {} - self.rhs = {self.xt:0} # rhs(x0) = 0 + self.rhs = {self.xt: 0} # rhs(x0) = 0 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 @@ -57,7 +58,7 @@ class Anytime_Dstar(object): 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): + 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]: @@ -86,31 +87,33 @@ class Anytime_Dstar(object): self.rhs[xi] = np.inf return self.rhs[xi] - def updatecost(self,range_changed=None, new=None, old=None, mode=False): + 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() for xi in self.CLOSED: if xi in self.CHILDREN: - oldchildren = self.CHILDREN[xi]# A + oldchildren = self.CHILDREN[xi] # A if isinbound(old, xi, mode) or isinbound(new, xi, mode): - newchildren = set(children(self,xi))# B + newchildren = set(children(self, xi)) # B removed = oldchildren.difference(newchildren) intersection = oldchildren.intersection(newchildren) added = newchildren.difference(oldchildren) + self.CHILDREN[xi] = newchildren for xj in removed: self.COST[xi][xj] = cost(self, xi, xj) for xj in intersection.union(added): self.COST[xi][xj] = cost(self, xi, xj) CHANGED.add(xi) - else: + else: if isinbound(old, xi, mode) or isinbound(new, xi, mode): CHANGED.add(xi) - children_added = set(children(self,xi)) + children_added = set(children(self, xi)) self.CHILDREN[xi] = children_added for xj in children_added: self.COST[xi][xj] = cost(self, xi, xj) return CHANGED -#--------------main functions for Anytime D star + + # --------------main functions for Anytime D star def key(self, s, epsilon=1): if self.getg(s) > self.getrhs(s): @@ -120,10 +123,10 @@ class Anytime_Dstar(object): def UpdateState(self, s): if s not in self.CLOSED: - # TODO if s is not visited before + # TODO if s is not visited before self.g[s] = np.inf if s != self.xt: - self.rhs[s] = min([self.getcost(s, s_p) + self.getg(s_p) for s_p in self.getchildren(s)]) + 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: @@ -132,7 +135,7 @@ class Anytime_Dstar(object): self.INCONS.add(s) def ComputeorImprovePath(self): - while self.OPEN.top_key() < self.key(self.x0,self.epsilon) or self.rhs[self.x0] != self.g[self.x0]: + while self.OPEN.top_key() < self.key(self.x0, self.epsilon) or self.rhs[self.x0] != self.g[self.x0]: s = self.OPEN.get() if getDist(s, tuple(self.env.start)) < self.env.resolution: @@ -156,7 +159,7 @@ class Anytime_Dstar(object): islargelychanged = False t = 0 self.ComputeorImprovePath() - #TODO publish current epsilon sub-optimal solution + # TODO publish current epsilon sub-optimal solution self.done = True self.ind = 0 self.Path = self.path() @@ -167,7 +170,7 @@ class Anytime_Dstar(object): break # change environment # new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation') - new2,old2 = self.env.move_block(a = [0,0,-0.2], mode='translation') + new2, old2 = self.env.move_block(a=[0, 0, -0.2], mode='translation') ischanged = True # islargelychanged = True self.Path = [] @@ -181,23 +184,23 @@ class Anytime_Dstar(object): self.ComputeorImprovePath() ischanged = False - if islargelychanged: - self.epsilon += self.increment # or replan from scratch + if islargelychanged: + self.epsilon += self.increment # or replan from scratch elif self.epsilon > 1: self.epsilon -= self.decrement - + # move states from the INCONS to OPEN # update priorities in OPEN Allnodes = self.INCONS.union(self.OPEN.allnodes()) for node in Allnodes: - self.OPEN.put(node, self.key(node, self.epsilon)) + self.OPEN.put(node, self.key(node, self.epsilon)) self.INCONS = set() self.CLOSED = set() self.ComputeorImprovePath() - #TODO publish current epsilon sub optimal solution + # TODO publish current epsilon sub optimal solution self.Path = self.path() # if epsilon == 1: - # wait for change to occur + # wait for change to occur t += 1 def path(self, s_start=None): @@ -212,10 +215,10 @@ class Anytime_Dstar(object): ind = 0 while getDist(s, s_goal) > self.env.resolution: if s == self.x0: - children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution*np.sqrt(3)] - else: + children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution * np.sqrt(3)] + else: children = list(self.CHILDREN[s]) - snext = children[np.argmin([self.getcost(s,s_p) + self.getg(s_p) for s_p in children])] + snext = children[np.argmin([self.getcost(s, s_p) + self.getg(s_p) for s_p in children])] path.append([s, snext]) s = snext if ind > 100: @@ -223,6 +226,7 @@ class Anytime_Dstar(object): ind += 1 return path + if __name__ == '__main__': - AD = Anytime_Dstar(resolution = 1) - AD.Main() \ No newline at end of file + AD = Anytime_Dstar(resolution=0.5) + AD.Main() diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index 682db30..727f29d 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index 847155f..8478a15 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.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 aaa219f..4306b48 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 a43f948..2ad0952 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