diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc index 1ae15c3..de55fab 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc differ diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc index bff683c..b11ed3b 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ diff --git a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc index a459242..183bce0 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 0331cd2..8a5b430 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -13,27 +13,26 @@ import sys 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, getRay, g_Space, Heuristic, getNearest, isCollide, \ - cost, children, StateSpace + cost, children, StateSpace, heuristic_fun from Search_3D.plot_util3D import visualization import queue import time class Weighted_A_star(object): def __init__(self, resolution=0.5): - self.Alldirec = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1], [1, 1, 1], - [-1, 0, 0], [0, -1, 0], [0, 0, -1], [-1, -1, 0], [-1, 0, -1], [0, -1, -1], - [-1, -1, -1], - [1, -1, 0], [-1, 1, 0], [1, 0, -1], [-1, 0, 1], [0, 1, -1], [0, -1, 1], - [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 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.g = g_Space(self) # key is the point, store g value - self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) - # self.AABB = getAABB(self.env.blocks) - self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0 - - self.h = Heuristic(self.g, self.goal) + self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) + self.g = {self.start:0,self.goal:np.inf} self.Parent = {} self.CLOSED = set() self.V = [] @@ -42,40 +41,36 @@ class Weighted_A_star(object): self.ind = 0 self.x0, self.xt = self.start, self.goal self.OPEN = queue.QueuePrior() # store [point,priority] - self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0]) # item, priority = g + h + self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h self.lastpoint = self.x0 - # def children(self, x): - # allchild = [] - # for j in self.Alldirec: - # collide, child = isCollide(self, x, j) - # if not collide: - # allchild.append(child) - # return allchild - def run(self, N=None): xt = self.xt xi = self.x0 - while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty + while self.OPEN: # while xt not reached and open is not empty xi = self.OPEN.get() if xi not in self.CLOSED: self.V.append(np.array(xi)) self.CLOSED.add(xi) # add the point in CLOSED set - visualization(self) - allchild = children(self,xi) - for xj in allchild: - if xj not in self.CLOSED: - gi, gj = self.g[xi], self.g[xj] - a = gi + cost(self, xi, xj) - if a < gj: - self.g[xj] = a - self.Parent[xj] = xi - if (a, xj) in self.OPEN.enumerate(): - # update priority of xj - self.OPEN.put(xj, a + 1 * self.h[xj]) - else: - # add xj in to OPEN set - self.OPEN.put(xj, a + 1 * self.h[xj]) + if xi == xt: + break + # visualization(self) + for xj in children(self,xi): + # if xj not in self.CLOSED: + if xj not in self.g: + self.g[xj] = np.inf + else: + pass + a = self.g[xi] + cost(self, xi, xj) + if a < self.g[xj]: + self.g[xj] = a + self.Parent[xj] = xi + # if (a, xj) in self.OPEN.enumerate(): + # update priority of xj + self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) + # else: + # add xj in to OPEN set + # self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) # For specified expanded nodes, used primarily in LRTA* if N: if len(self.CLOSED) % N == 0: @@ -111,7 +106,7 @@ class Weighted_A_star(object): self.start = xj self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0 self.x0 = xj - self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0]) # item, priority = g + h + self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h self.CLOSED = set() # self.h = h(self.Space, self.goal) diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index e618237..a501429 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -15,11 +15,15 @@ from Search_3D.plot_util3D import visualization class D_star(object): def __init__(self, resolution=1): - self.Alldirec = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1], [1, 1, 1], - [-1, 0, 0], [0, -1, 0], [0, 0, -1], [-1, -1, 0], [-1, 0, -1], [0, -1, -1], - [-1, -1, -1], - [1, -1, 0], [-1, 1, 0], [1, 0, -1], [-1, 0, 1], [0, 1, -1], [0, -1, 1], - [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 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) @@ -133,7 +137,6 @@ class D_star(object): self.insert(x, self.h[xparent] + cost(self, x, xparent)) def modify(self, x): self.modify_cost(x) - self.V = set() while True: kmin = self.process_state() # visualization(self) @@ -175,6 +178,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() while s != self.xt: if s == tuple(self.env.start): sparent = self.b[self.x0] diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py index de4632e..33ed5c0 100644 --- a/Search-based Planning/Search_3D/LP_Astar3D.py +++ b/Search-based Planning/Search_3D/LP_Astar3D.py @@ -17,11 +17,15 @@ import time class Lifelong_Astar(object): def __init__(self,resolution = 1): - self.Alldirec = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1], [1, 1, 1], - [-1, 0, 0], [0, -1, 0], [0, 0, -1], [-1, -1, 0], [-1, 0, -1], [0, -1, -1], - [-1, -1, -1], - [1, -1, 0], [-1, 1, 0], [1, 0, -1], [-1, 0, 1], [0, 1, -1], [0, -1, 1], - [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 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.g = g_Space(self) self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) @@ -177,7 +181,7 @@ class Lifelong_Astar(object): if __name__ == '__main__': sta = time.time() - Astar = Lifelong_Astar(0.5) + Astar = Lifelong_Astar(1) Astar.ComputePath() Astar.change_env() Astar.ComputePath() 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 a140348..3cd3c71 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__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index 69a2b14..cf4416e 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 e93ee6a..439c28d 100644 --- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py +++ b/Search-based Planning/Search_3D/bidirectional_Astar3D.py @@ -13,26 +13,27 @@ import sys 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, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children +from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun from Search_3D.plot_util3D import visualization import queue class Weighted_A_star(object): def __init__(self,resolution=0.5): - self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\ - [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\ - [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\ - [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,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.g = g_Space(self) # key is the point, store g value - self.start, self.goal = getNearest(self.g,self.env.start), getNearest(self.g,self.env.goal) - self.g[self.start] = 0 # set g(x0) = 0 - self.g[self.goal] = 0 # set g(x0) = 0 + 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.h1 = Heuristic(self.g,self.goal) # tree NO.1 - self.h2 = Heuristic(self.g,self.start) # tree NO.2 self.Parent1, self.Parent2 = {}, {} self.CLOSED1, self.CLOSED2 = set(), set() self.V = [] @@ -41,8 +42,8 @@ class Weighted_A_star(object): def run(self): x0, xt = self.start, self.goal - self.OPEN1.put(x0, self.g[x0] + self.h1[x0]) # item, priority = g + h - self.OPEN2.put(xt, self.g[xt] + self.h2[xt]) # item, priority = g + h + self.OPEN1.put(x0, self.g[x0] + heuristic_fun(self,x0,xt)) # item, priority = g + h + self.OPEN2.put(xt, self.g[xt] + heuristic_fun(self,xt,x0)) # item, priority = g + h self.ind = 0 while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() @@ -50,7 +51,7 @@ class Weighted_A_star(object): self.CLOSED2.add(xi2) self.V.append(xi1) self.V.append(xi2) - visualization(self) + # visualization(self) allchild1, allchild2 = children(self,xi1), children(self,xi2) self.evaluation(allchild1,xi1,conf=1) self.evaluation(allchild2,xi2,conf=2) @@ -66,26 +67,34 @@ class Weighted_A_star(object): for xj in allchild: if conf == 1: if xj not in self.CLOSED1: - gi, gj = self.g[xi], self.g[xj] + if xj not in self.g: + self.g[xj] = np.inf + else: + pass + gi = self.g[xi] a = gi + cost(self,xi,xj) - if a < gj: + 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*self.h1[xj]) + self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal)) else: - self.OPEN1.put(xj, a+1*self.h1[xj]) + self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal)) if conf == 2: if xj not in self.CLOSED2: - gi, gj = self.g[xi], self.g[xj] + if xj not in self.g: + self.g[xj] = np.inf + else: + pass + gi = self.g[xi] a = gi + cost(self,xi,xj) - if a < gj: + 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*self.h2[xj]) + self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start)) else: - self.OPEN2.put(xj, a+1*self.h2[xj]) + 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/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index c6f7c44..636d756 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -30,9 +30,13 @@ def Heuristic(Space, t): '''Max norm distance''' h = {} for k in Space.keys(): - h[k] = max(abs(np.array([t[0] - k[0], t[1] - k[1], t[2] - k[2]]))) + h[k] = max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])]) return h +def heuristic_fun(initparams, k, t=None): + if t is None: + t = initparams.goal + return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])]) def isinbound(i, x): if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: @@ -117,9 +121,11 @@ def g_Space(initparams): return g -def isCollide(initparams, x, child): +def isCollide(initparams, x, child, dist): '''see if line intersects obstacle''' - dist = getDist(x, child) + '''specified for expansion in A* 3D lookup table''' + if dist==None: + dist = getDist(x, child) if not isinbound(initparams.env.boundary, child): return True, dist for i in range(len(initparams.env.AABB)): @@ -147,6 +153,7 @@ def children(initparams, x): continue if isinbound(initparams.env.boundary, child): allchild.append(child) + # initparams.Alldirec[direc]*resolution return allchild @@ -160,8 +167,9 @@ def obstacleFree(initparams, x): return True -def cost(initparams, i, j, settings=0): - collide, dist = isCollide(initparams, i, j) +def cost(initparams, i, j, dist=None, settings=0): + collide, dist = isCollide(initparams, i, j, dist) + # collide, dist= False, getDist(i, j) if settings == 0: if collide: return np.inf