diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index a6b3ca3..265ce5c 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -18,79 +18,6 @@ from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, g from Search_3D.plot_util3D import visualization import queue - -# 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 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.N = N @@ -117,7 +44,7 @@ class LRT_A_star2: xi = dehash(strxi) lasthvals.append(self.Astar.h[strxi]) # update h values if they are smaller - minfval = min([cost(xi, xj, settings=1) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)]) + minfval = min([cost(xi, xj, settings=0) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)]) if self.Astar.h[strxi] >= minfval: self.Astar.h[strxi] = minfval hvals.append(self.Astar.h[strxi]) @@ -158,5 +85,5 @@ class LRT_A_star2: if __name__ == '__main__': - T = LRT_A_star2(resolution=1, N=30) + T = LRT_A_star2(resolution=0.5, N=1500) 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 72db8ba..32a2674 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 bd5a61d..7b46412 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 9d17d49..3be010f 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 7e1431a..e2fddd0 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 a8c1d46..f20c254 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