diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 734f944..305051b 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -35,6 +35,7 @@ class Weighted_A_star(object): self.V = [] self.done = False self.Path = [] + self.ind = 0 def children(self,x): allchild = [] @@ -44,10 +45,9 @@ class Weighted_A_star(object): allchild.append(child) return allchild - def run(self): + 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 - self.ind = 0 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) @@ -69,12 +69,18 @@ class Weighted_A_star(object): else: # add xj in to OPEN set 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: + break if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) self.ind += 1 - self.done = True - self.Path = self.path() - visualization(self) - plt.show() + # if the path finding is finished + if xt in self.CLOSED: + self.done = True + self.Path = self.path() + visualization(self) + plt.show() def path(self): path = [] @@ -87,5 +93,5 @@ class Weighted_A_star(object): return path if __name__ == '__main__': - Astar = Weighted_A_star(0.5) + Astar = Weighted_A_star(1) Astar.run() \ No newline at end of file diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index 07e6c2a..925a608 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -1,4 +1,4 @@ -# this is the three dimensional LRTA* algo +# this is the three dimensional near-sighted 1 neighborhood LRTA* algo # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @@ -12,84 +12,110 @@ 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.plot_util3D import visualization import queue -class LRT_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) - 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 - self.OPEN = queue.QueuePrior() - self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0 - self.Child = {} - self.CLOSED = set() - self.V = [] - self.done = False - self.Path = [] +# class LRT_A_star1(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) +# 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 # this is g +# self.OPEN = queue.QueuePrior() +# self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0 +# self.Child = {} +# self.CLOSED = set() +# self.V = [] +# self.done = False +# self.Path = [] - 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 children(self,x): +# allchild = [] +# for j in self.Alldirec: +# collide,child = isCollide(self,x,j) +# if not collide: +# allchild.append(child) +# return allchild - def step(self, xi, strxi): - childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1} - fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs] - xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s')) - strxj = hash3D(xj) - # add the child of xi - self.Child[strxi] = xj - if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update h(s) = h'(s) - self.h[strxi] = fmin - # TODO: action to move to xj - self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s')) +# def step(self, xi, strxi): +# childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1} +# fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs] +# xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s')) +# strxj = hash3D(xj) +# # add the child of xi +# self.Child[strxi] = xj +# if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update h(s) = h'(s) +# self.h[strxi] = fmin +# # TODO: action to move to xj +# self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s')) + +# def run(self): +# x0 = hash3D(self.start) +# xt = hash3D(self.goal) +# self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state +# self.ind = 0 +# while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do +# strxi = self.OPEN.get() +# xi = dehash(strxi) +# self.CLOSED.add(strxi) +# self.V.append(xi) +# visualization(self) +# if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) +# self.ind += 1 +# self.done = True +# self.Path = self.path() +# visualization(self) +# plt.show() + +# def path(self): +# # this is a suboptimal path. +# path = [] +# strgoal = hash3D(self.goal) +# strx = hash3D(self.start) +# ind = 0 +# while strx != strgoal: +# path.append([dehash(strx),self.Child[strx]]) +# strx = hash3D(self.Child[strx]) +# ind += 1 +# if ind == 1000: +# return np.flip(path,axis=0) +# path = np.flip(path,axis=0) +# return path + +class LRT_A_star2(): + 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) + + 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)]) + + #def move(self): + # print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()])) + def run(self): - x0 = hash3D(self.start) - xt = hash3D(self.goal) - self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state - self.ind = 0 - while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do - strxi = self.OPEN.get() - xi = dehash(strxi) - self.CLOSED.add(strxi) - self.V.append(xi) - visualization(self) - self.step(xi , strxi) - if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) - self.ind += 1 - self.done = True - self.Path = self.path() - visualization(self) - plt.show() + xt = hash3D(self.Astar.goal) + while xt not in self.Astar.CLOSED: + self.expand() + #self.updateHeuristic() - def path(self): - # this is a suboptimal path. - path = [] - strgoal = hash3D(self.goal) - strx = hash3D(self.start) - ind = 0 - while strx != strgoal: - path.append([dehash(strx),self.Child[strx]]) - strx = hash3D(self.Child[strx]) - ind += 1 - if ind == 1000: - return np.flip(path,axis=0) - path = np.flip(path,axis=0) - return path if __name__ == '__main__': - Astar = LRT_A_star(0.5) - Astar.run() \ No newline at end of file + T = LRT_A_star2(resolution = 1, N = 2) + T.run() 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 cf15227..b607012 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