diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 75b0a57..8555add 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -42,6 +42,7 @@ class Weighted_A_star(object): 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 + self.lastpoint = self.x0 def children(self, x): allchild = [] @@ -56,8 +57,9 @@ class Weighted_A_star(object): 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) + if strxi not in self.CLOSED: + self.V.append(xi) self.CLOSED.add(strxi) # add the point in CLOSED set - self.V.append(xi) visualization(self) allchild = self.children(xi) for xj in allchild: @@ -78,11 +80,12 @@ class Weighted_A_star(object): 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('number node expanded = ' + str(len(self.V))) self.ind += 1 + self.lastpoint = strxi # if the path finding is finished - if xt in self.CLOSED: + if xt in self.CLOSED and N is None: self.done = True self.Path = self.path() visualization(self) @@ -90,15 +93,24 @@ class Weighted_A_star(object): def path(self): path = [] - strx = hash3D(self.goal) - strstart = hash3D(self.start) + strx = self.lastpoint + #strstart = hash3D(getNearest(self.Space, self.env.start)) + strstart = self.x0 while strx != strstart: path.append([dehash(strx), self.Parent[strx]]) strx = hash3D(self.Parent[strx]) path = np.flip(path, axis=0) return path + # utility used in LRTA* + def reset(self,xj): + self.Space = StateSpace(self) # key is the point, store g value + self.start = xj + self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0 + self.x0 = hash3D(xj) + self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h + self.CLOSED = set() if __name__ == '__main__': - Astar = Weighted_A_star(1) + Astar = Weighted_A_star(0.5) Astar.run() diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index 9c56ffd..94a2edf 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \ - cost + cost, obstacleFree from Search_3D.plot_util3D import visualization import queue @@ -93,21 +93,62 @@ import queue class LRT_A_star2(): def __init__(self, resolution=0.5, N=7): - self.lookahead = N - self.Astar = Astar3D.Weighted_A_star() - - while True: - self.Astar.run(self.lookahead) + self.N = N + self.Astar = Astar3D.Weighted_A_star(resolution=resolution) + self.path = [] + def children(self, x): + allchild = [] + resolution = self.Astar.env.resolution + for direc in self.Astar.Alldirec: + child = np.array(list(map(np.add,x,np.multiply(direc,resolution)))) + allchild.append(hash3D(child)) + return allchild + 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)]) + minfval = min([cost(xi, xj, settings=1) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)]) + if self.Astar.h[strxi] >= minfval: + self.Astar.h[strxi] = minfval def move(self): - print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()])) + #self.Astar.Parent[xj] = dehash(self.Astar.lastpoint) + strst = self.Astar.x0 + st = self.Astar.start + ind = 0 + while strst in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop + # strChildren = self.children(st) + strChildren = [hash3D(i) for i in self.Astar.children(st)] + minh , minchild = np.inf , None + for child in strChildren: + h = self.Astar.h[child] + if h <= minh: + minh , minchild = h , dehash(child) + self.path.append([st,minchild]) + strst, st = hash3D(minchild), minchild + for (_,strp) in self.Astar.OPEN.enumerate(): + if strp == strst: + break + ind += 1 + if ind > 1000: + break + self.Astar.reset(st) + + def run(self): + while self.Astar.lastpoint != hash3D(self.Astar.goal): + self.Astar.run(N=self.N) + #print(path) + visualization(self.Astar) + self.updateHeuristic() + self.move() + self.Astar.Path = self.path + self.Astar.done = True + visualization(self.Astar) + plt.show() if __name__ == '__main__': - T = LRT_A_star2(resolution=1, N=50) \ No newline at end of file + T = LRT_A_star2(resolution=1, N=100) + T.run() \ 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 49718a2..339ed0a 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..43d7047 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..3fca6ab 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..d267c52 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 6b0b616..60291af 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/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index e0b208b..2ac5aa8 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -14,6 +14,9 @@ def getAABB(blocks): def getDist(pos1, pos2): return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) +def getManDist(pos1, pos2): + return sum([abs(pos1[0] - pos2[0]),abs(pos1[1] - pos2[1]),abs(pos1[2] - pos2[2])]) + def getNearest(Space,pt): '''get the nearest point on the grid''' mindis,minpt = 1000,None @@ -88,8 +91,20 @@ def isCollide(initparams, x, direc): return True, child return False, child -def cost(i,j): - return getDist(i,j) +def obstacleFree(initparams,x): + for i in initparams.env.blocks: + if isinbound(i,x): + return False + for i in initparams.env.balls: + if isinball(i,x): + return False + return True + +def cost(i,j,settings=0): + if settings == 0: + return getDist(i,j) + if settings == 1: + return getManDist(i,j) if __name__ == "__main__": from env3D import env \ No newline at end of file