diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 26b4425..957da69 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,9 +20,12 @@ + - - + + + + - - + + - - - - - - + + + + + + - - - + + + - - + + + - - + diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py index ee37b0c..7a2828a 100644 --- a/Search-based Planning/Search_2D/LPAstar.py +++ b/Search-based Planning/Search_2D/LPAstar.py @@ -5,6 +5,7 @@ LPA_star 2D import os import sys +import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -13,6 +14,122 @@ from Search_2D import queue from Search_2D import plotting from Search_2D import env + class LpaStar: - def __init__(self): - return + def __init__(self, x_start, x_goal, heuristic_type): + self.xI, self.xG = x_start, x_goal + self.heuristic_type = heuristic_type + + self.Env = env.Env() # class Env + + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + + self.U = queue.QueuePrior() # priority queue / OPEN set + self.g, self.rhs = {}, {} + + for i in range(self.Env.x_range): + for j in range(self.Env.y_range): + self.rhs[(i, j)] = float("inf") + self.g[(i, j)] = float("inf") + + self.rhs[self.xI] = 0 + self.U.put(self.xI, [self.h(self.xI), 0]) + + def searching(self): + self.computePath() + path = self.extract_path() + return path + + def computePath(self): + while self.U.top_key() < self.CalculateKey(self.xG) \ + or self.rhs[self.xG] != self.g[self.xG]: + s = self.U.get() + if self.g[s] > self.rhs[s]: + self.g[s] = self.rhs[s] + for x in self.get_neighbor(s): + self.UpdateVertex(x) + else: + self.g[s] = float("inf") + self.UpdateVertex(s) + for x in self.get_neighbor(s): + self.UpdateVertex(x) + + def extract_path(self): + path = [] + s = self.xG + + while True: + g_list = {} + for x in self.get_neighbor(s): + g_list[x] = self.g[x] + s = min(g_list, key=g_list.get) + if s == self.xI: + return list(reversed(path)) + path.append(s) + + def get_neighbor(self, s): + nei_list = set() + for u in self.u_set: + s_next = tuple([s[i] + u[i] for i in range(2)]) + if s_next not in self.obs: + nei_list.add(s_next) + + return nei_list + + def CalculateKey(self, s): + return [min(self.g[s], self.rhs[s]) + self.h(s), + min(self.g[s], self.rhs[s])] + + def UpdateVertex(self, u): + if u != self.xI: + u_min = float("inf") + for x in self.get_neighbor(u): + u_min = min(u_min, self.g[x] + 1) + self.rhs[u] = u_min + self.U.check_remove(u) + if self.g[u] != self.rhs[u]: + self.U.put(u, self.CalculateKey(u)) + + def h(self, s): + heuristic_type = self.heuristic_type # heuristic type + goal = self.xG # goal node + + if heuristic_type == "manhattan": + return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) + elif heuristic_type == "euclidean": + return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2) + else: + print("Please choose right heuristic type!") + + @staticmethod + def get_cost(x, u): + """ + Calculate cost for this motion + + :param x: current node + :param u: current input + :return: cost for this motion + :note: cost function could be more complicate! + """ + + return 1 + + +def main(): + x_start = (5, 5) + x_goal = (45, 25) + + lpastar = LpaStar(x_start, x_goal, "manhattan") + plot = plotting.Plotting(x_start, x_goal) + + path = lpastar.searching() + plot.plot_grid("test") + px = [x[0] for x in path] + py = [x[1] for x in path] + plt.plot(px, py, color='red', marker='o') + plt.show() + + +if __name__ == '__main__': + main() diff --git a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc index d42d249..b628e1d 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc differ diff --git a/Search-based Planning/Search_2D/bfs.py b/Search-based Planning/Search_2D/bfs.py index 70b8675..75bd06a 100644 --- a/Search-based Planning/Search_2D/bfs.py +++ b/Search-based Planning/Search_2D/bfs.py @@ -68,7 +68,7 @@ class BFS: def main(): x_start = (5, 5) # Starting node - x_goal = (49, 25) # Goal node + x_goal = (45, 25) # Goal node bfs = BFS(x_start, x_goal) plot = plotting.Plotting(x_start, x_goal) diff --git a/Search-based Planning/Search_2D/queue.py b/Search-based Planning/Search_2D/queue.py index 0bdacb2..c434bee 100644 --- a/Search-based Planning/Search_2D/queue.py +++ b/Search-based Planning/Search_2D/queue.py @@ -67,3 +67,11 @@ class QueuePrior: def enumerate(self): return self.queue + + def check_remove(self, item): + for (p, x) in self.queue: + if item == x: + self.queue.remove((p, x)) + + def top_key(self): + return self.queue[0][0] diff --git a/Search-based Planning/Search_2D/test.py b/Search-based Planning/Search_2D/test.py new file mode 100644 index 0000000..6097bc5 --- /dev/null +++ b/Search-based Planning/Search_2D/test.py @@ -0,0 +1,20 @@ +""" +A_star 2D +@author: huiming zhou +""" + +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Search-based Planning/") + +from Search_2D import queue +from Search_2D import plotting +from Search_2D import env + + +U = queue.QueuePrior() +U.put((1, 2), [2, 3]) +U.put((2, 3), [1, 5]) +print(U.get()) \ No newline at end of file diff --git a/Search-based Planning/gif/BFS.gif b/Search-based Planning/gif/BFS.gif index ddccff8..1f885f5 100644 Binary files a/Search-based Planning/gif/BFS.gif and b/Search-based Planning/gif/BFS.gif differ