diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 27c3740..7da76db 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -21,12 +21,8 @@ - - - - - - + + - + + + + + + + - - - - - - - - + + + + - - diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 702c8f7..c0a8df6 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -54,6 +54,7 @@ class Weighted_A_star(object): def run(self, N=None): xt = self.xt + strxi = self.x0 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) @@ -85,25 +86,29 @@ class Weighted_A_star(object): self.lastpoint = strxi # if the path finding is finished - if xt in self.CLOSED and N is None: + if xt in self.CLOSED: self.done = True self.Path = self.path() - visualization(self) - plt.show() + if N is None: + visualization(self) + plt.show() + return True + + return False def path(self): path = [] strx = self.lastpoint - #strstart = hash3D(getNearest(self.Space, self.env.start)) + # 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) + # path = np.flip(path, axis=0) return path # utility used in LRTA* - def reset(self,xj): + 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 @@ -113,6 +118,7 @@ class Weighted_A_star(object): # self.h = Heuristic(self.Space, self.goal) + if __name__ == '__main__': Astar = Weighted_A_star(1) Astar.run() diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index d414d57..a6b3ca3 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -101,43 +101,43 @@ class LRT_A_star2: allchild = [] resolution = self.Astar.env.resolution for direc in self.Astar.Alldirec: - child = np.array(list(map(np.add,x,np.multiply(direc,resolution)))) + child = np.array(list(map(np.add, x, np.multiply(direc, resolution)))) allchild.append(hash3D(child)) return allchild - + def updateHeuristic(self): # Initialize at infinity for strxi in self.Astar.CLOSED: self.Astar.h[strxi] = np.inf # initialize difference Diff = True - while Diff: # repeat until converge + while Diff: # repeat until converge hvals, lasthvals = [], [] for strxi in self.Astar.CLOSED: xi = dehash(strxi) - lasthvals.append(self.Astar.h[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)]) if self.Astar.h[strxi] >= minfval: self.Astar.h[strxi] = minfval - hvals.append(self.Astar.h[strxi]) + hvals.append(self.Astar.h[strxi]) if lasthvals == hvals: Diff = False def move(self): 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 + 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 + 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(): + 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 @@ -146,21 +146,17 @@ class LRT_A_star2: 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) + while True: + if self.Astar.run(N=self.N): + self.Astar.Path = self.Astar.Path + self.path + self.Astar.done = True + visualization(self.Astar) + plt.show() + break self.updateHeuristic() self.move() - print(hash3D(self.Astar.goal) in self.Astar.CLOSED) - self.updateHeuristic() - self.move() - self.Astar.Path = self.path # previous path (determined from DP) + last path (determined from A*) - self.Astar.done = True - visualization(self.Astar) - plt.show() if __name__ == '__main__': - T = LRT_A_star2(resolution=0.5, N=1) + T = LRT_A_star2(resolution=1, N=30) 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 faf4486..72db8ba 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 43d7047..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 3fca6ab..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 d267c52..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 60291af..a8c1d46 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