diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index bc974b6..b97c0f9 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,9 +20,10 @@ - - + + + - + - + - + - - + + + + - - diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py index 8e7b0a2..ee37b0c 100644 --- a/Search-based Planning/Search_2D/LPAstar.py +++ b/Search-based Planning/Search_2D/LPAstar.py @@ -15,3 +15,4 @@ from Search_2D import env class LpaStar: def __init__(self): + return diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 305051b..75b0a57 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -12,46 +12,51 @@ 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 getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost +from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ + cost 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.env = env(resolution = resolution) - self.Space = StateSpace(self) # key is the point, store g value - self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal) + 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.env = env(resolution=resolution) + self.Space = StateSpace(self) # key is the point, store g value + self.start, self.goal = getNearest(self.Space, self.env.start), getNearest(self.Space, self.env.goal) self.AABB = getAABB(self.env.blocks) - self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # set g(x0) = 0 - self.OPEN = queue.QueuePrior() # store [point,priority] - self.h = Heuristic(self.Space,self.goal) + self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0 + + self.h = Heuristic(self.Space, self.goal) self.Parent = {} self.CLOSED = set() self.V = [] self.done = False self.Path = [] self.ind = 0 + self.x0, self.xt = hash3D(self.start), hash3D(self.goal) + self.OPEN = queue.QueuePrior() # store [point,priority] + self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h - def children(self,x): + def children(self, x): allchild = [] for j in self.Alldirec: - collide,child = isCollide(self,x,j) + collide, child = isCollide(self, x, j) if not collide: allchild.append(child) return allchild def run(self, N=None): - x0, xt = hash3D(self.start), hash3D(self.goal) - self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # item, priority = g + h - while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty - strxi = self.OPEN.get() + xt = self.xt + while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty + strxi = self.OPEN.get() xi = dehash(strxi) - self.CLOSED.add(strxi) # add the point in CLOSED set + self.CLOSED.add(strxi) # add the point in CLOSED set self.V.append(xi) visualization(self) allchild = self.children(xi) @@ -59,22 +64,23 @@ class Weighted_A_star(object): strxj = hash3D(xj) if strxj not in self.CLOSED: gi, gj = self.Space[strxi], self.Space[strxj] - a = gi + cost(xi,xj) + a = gi + cost(xi, xj) if a < gj: self.Space[strxj] = a self.Parent[strxj] = xi if (a, strxj) in self.OPEN.enumerate(): # update priority of xj - self.OPEN.put(strxj, a+1*self.h[strxj]) + self.OPEN.put(strxj, a + 1 * self.h[strxj]) else: # add xj in to OPEN set - self.OPEN.put(strxj, a+1*self.h[strxj]) + self.OPEN.put(strxj, a + 1 * self.h[strxj]) # For specified expanded nodes, used primarily in LRTA* - if N is not None: - if len(self.V) % N == 0: + if N: + if len(self.CLOSED) % N == 0: break - if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) + if self.ind % 100 == 0: print('iteration number = ' + str(self.ind)) self.ind += 1 + # if the path finding is finished if xt in self.CLOSED: self.done = True @@ -87,11 +93,12 @@ class Weighted_A_star(object): strx = hash3D(self.goal) strstart = hash3D(self.start) while strx != strstart: - path.append([dehash(strx),self.Parent[strx]]) + path.append([dehash(strx), self.Parent[strx]]) strx = hash3D(self.Parent[strx]) - path = np.flip(path,axis=0) + path = np.flip(path, axis=0) return path + if __name__ == '__main__': Astar = Weighted_A_star(1) - Astar.run() \ No newline at end of file + Astar.run() diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index a0f854c..9c56ffd 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -12,8 +12,9 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") from Search_3D.env3D import env -from Search_3D.Astar3D import Weighted_A_star -from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost +from Search_3D import Astar3D +from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ + cost from Search_3D.plot_util3D import visualization import queue @@ -91,31 +92,22 @@ import queue # return path class LRT_A_star2(): - def __init__(self,resolution=0.5, N=7): + def __init__(self, resolution=0.5, N=7): self.lookahead = N - self.Astar = Weighted_A_star() - self.Astar.env.resolution = resolution - - def expand(self): - self.Astar.run(self.lookahead) + self.Astar = Astar3D.Weighted_A_star() + + while True: + self.Astar.run(self.lookahead) def updateHeuristic(self): for strxi in self.Astar.CLOSED: self.Astar.h[strxi] = np.inf xi = dehash(strxi) - self.Astar.h[strxi] = min([cost(xi,xj) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)]) - + self.Astar.h[strxi] = min([cost(xi, xj) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)]) + def move(self): print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()])) - - - def run(self): - xt = hash3D(self.Astar.goal) - while xt not in self.Astar.CLOSED: - self.expand() - #self.updateHeuristic() if __name__ == '__main__': - T = LRT_A_star2(resolution = 1, N = 2) - T.run() + T = LRT_A_star2(resolution=1, N=50) \ No newline at end of file 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 b607012..49718a2 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__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index 8c180c5..bd5a61d 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 61a7bf0..9d17d49 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 d1f5eb2..7e1431a 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 f1face4..6b0b616 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