diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 861e213..27c3740 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -21,6 +21,12 @@ + + + + + + - + - - - - + + + + - - + + - + + + - - diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py index 7a2828a..b2c420a 100644 --- a/Search-based Planning/Search_2D/LPAstar.py +++ b/Search-based Planning/Search_2D/LPAstar.py @@ -34,12 +34,38 @@ class LpaStar: self.g[(i, j)] = float("inf") self.rhs[self.xI] = 0 - self.U.put(self.xI, [self.h(self.xI), 0]) + self.U.put(self.xI, self.CalculateKey(self.xI)) def searching(self): self.computePath() - path = self.extract_path() - return path + path = [self.extract_path()] + + obs_change = set() + for j in range(14, 15): + self.obs.add((30, j)) + obs_change.add((30, j)) + for s in obs_change: + self.rhs[s] = float("inf") + self.g[s] = float("inf") + for x in self.get_neighbor(s): + self.UpdateVertex(x) + # for x in obs_change: + # self.obs.remove(x) + # for x in obs_change: + # self.UpdateVertex(x) + print(self.g[(29, 15)]) + print(self.g[(29, 14)]) + print(self.g[(29, 13)]) + print(self.g[(30, 13)]) + print(self.g[(31, 13)]) + print(self.g[(32, 13)]) + print(self.g[(33, 13)]) + print(self.g[(34, 13)]) + + self.computePath() + path.append(self.extract_path_test()) + + return path, obs_change def computePath(self): while self.U.top_key() < self.CalculateKey(self.xG) \ @@ -47,13 +73,12 @@ class LpaStar: 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) + for x in self.get_neighbor(s): + self.UpdateVertex(x) + # return self.extract_path() def extract_path(self): path = [] @@ -68,6 +93,18 @@ class LpaStar: return list(reversed(path)) path.append(s) + def extract_path_test(self): + path = [] + s = self.xG + + for k in range(30): + g_list = {} + for x in self.get_neighbor(s): + g_list[x] = self.g[x] + s = min(g_list, key=g_list.get) + path.append(s) + return list(reversed(path)) + def get_neighbor(self, s): nei_list = set() for u in self.u_set: @@ -85,7 +122,7 @@ class LpaStar: if u != self.xI: u_min = float("inf") for x in self.get_neighbor(u): - u_min = min(u_min, self.g[x] + 1) + u_min = min(u_min, self.g[x] + self.get_cost(u, x)) self.rhs[u] = u_min self.U.check_remove(u) if self.g[u] != self.rhs[u]: @@ -102,32 +139,45 @@ class LpaStar: else: print("Please choose right heuristic type!") - @staticmethod - def get_cost(x, u): + def get_cost(self, s_start, s_end): """ Calculate cost for this motion - :param x: current node - :param u: current input + :param s_start: + :param s_end: :return: cost for this motion :note: cost function could be more complicate! """ - return 1 + if s_start not in self.obs: + if s_end not in self.obs: + return 1 + else: + return float("inf") + return float("inf") def main(): x_start = (5, 5) x_goal = (45, 25) - lpastar = LpaStar(x_start, x_goal, "manhattan") + lpastar = LpaStar(x_start, x_goal, "euclidean") 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') + path, obs = lpastar.searching() + + plot.plot_grid("Lifelong Planning A*") + p = path[0] + px = [x[0] for x in p] + py = [x[1] for x in p] + plt.plot(px, py, marker='o') + plt.pause(0.5) + + p = path[1] + px = [x[0] for x in p] + py = [x[1] for x in p] + plt.plot(px, py, marker='o') + plt.pause(0.01) plt.show() diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search-based Planning/Search_2D/LRTAstar.py index 6c97ead..5eb2e86 100644 --- a/Search-based Planning/Search_2D/LRTAstar.py +++ b/Search-based Planning/Search_2D/LRTAstar.py @@ -29,6 +29,11 @@ class LrtAstarN: self.N = N # number of expand nodes each iteration self.visited = [] # order of visited nodes in planning self.path = [] # path of each iteration + self.h_table = {} + + for i in range(self.Env.x_range): + for j in range(self.Env.y_range): + self.h_table[(i, j)] = self.h((i, j)) # initialize h_value def searching(self): s_start = self.xI # initialize start node @@ -41,6 +46,10 @@ class LrtAstarN: break h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes + + for x in h_value: + self.h_table[x] = h_value[x] + s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # s_start -> expected node in OPEN set self.path.append(path_k) @@ -56,7 +65,7 @@ class LrtAstarN: if s_next in h_value: h_list[s_next] = h_value[s_next] else: - h_list[s_next] = self.h(s_next) + h_list[s_next] = self.h_table[s_next] s_key = min(h_list, key=h_list.get) # move to the smallest node with min h_value path.append(s_key) # generate path s = s_key # use end of this iteration as the start of next @@ -78,7 +87,7 @@ class LrtAstarN: s_next = tuple([s[i] + u[i] for i in range(2)]) if s_next not in self.obs: if s_next not in CLOSED: - h_list.append(self.get_cost(s, s_next) + self.h(s_next)) + h_list.append(self.get_cost(s, s_next) + self.h_table[s_next]) else: h_list.append(self.get_cost(s, s_next) + h_value[s_next]) h_value[s] = min(h_list) # update h_value of current node @@ -114,7 +123,7 @@ class LrtAstarN: if new_cost < g_table[s_next]: # conditions for updating cost g_table[s_next] = new_cost PARENT[s_next] = s - OPEN.put(s_next, g_table[s_next] + self.h(s_next)) + OPEN.put(s_next, g_table[s_next] + self.h_table[s_next]) if count == N: # expand needed CLOSED nodes break @@ -171,7 +180,7 @@ def main(): x_start = (10, 5) x_goal = (45, 25) - lrta = LrtAstarN(x_start, x_goal, 220, "euclidean") + lrta = LrtAstarN(x_start, x_goal, 100, "euclidean") plot = plotting.Plotting(x_start, x_goal) fig_name = "Learning Real-time A* (LRTA*)" diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search-based Planning/Search_2D/RTAAstar.py index 5b60a84..0983c36 100644 --- a/Search-based Planning/Search_2D/RTAAstar.py +++ b/Search-based Planning/Search_2D/RTAAstar.py @@ -29,6 +29,11 @@ class RtaAstar: self.N = N # number of expand nodes each iteration self.visited = [] # order of visited nodes in planning self.path = [] # path of each iteration + self.h_table = {} + + for i in range(self.Env.x_range): + for j in range(self.Env.y_range): + self.h_table[(i, j)] = self.h((i, j)) # initialize h_value def searching(self): s_start = self.xI # initialize start node @@ -42,6 +47,10 @@ class RtaAstar: break s_next, h_value = self.cal_h_value(OPEN, CLOSED, g_table, PARENT) + + for x in h_value: + self.h_table[x] = h_value[x] + s_start, path_k = self.extract_path_in_CLOSE(s_start, s_next, h_value) self.path.append(path_k) @@ -49,7 +58,7 @@ class RtaAstar: v_open = {} h_value = {} for (_, x) in OPEN.enumerate(): - v_open[x] = g_table[PARENT[x]] + 1 + self.h(x) + v_open[x] = g_table[PARENT[x]] + 1 + self.h_table[x] s_open = min(v_open, key=v_open.get) f_min = min(v_open.values()) for x in CLOSED: @@ -88,7 +97,7 @@ class RtaAstar: s_next = tuple([s[i] + u[i] for i in range(2)]) if s_next not in self.obs: if s_next not in CLOSED: - h_list.append(self.get_cost(s, s_next) + self.h(s_next)) + h_list.append(self.get_cost(s, s_next) + self.h_table[s_next]) else: h_list.append(self.get_cost(s, s_next) + h_value[s_next]) h_value[s] = min(h_list) # update h_value of current node @@ -124,7 +133,7 @@ class RtaAstar: if new_cost < g_table[s_next]: # conditions for updating cost g_table[s_next] = new_cost PARENT[s_next] = s - OPEN.put(s_next, g_table[s_next] + self.h(s_next)) + OPEN.put(s_next, g_table[s_next] + self.h_table[s_next]) if count == N: # expand needed CLOSED nodes break diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc index a03b5a8..8283245 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc differ diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc index 178a3ab..91533df 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ 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 b628e1d..71d39d5 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/env.py b/Search-based Planning/Search_2D/env.py index 4cdcba2..ca17d1f 100644 --- a/Search-based Planning/Search_2D/env.py +++ b/Search-based Planning/Search_2D/env.py @@ -20,26 +20,26 @@ class Env: x = self.x_range y = self.y_range - obs = [] + obs = set() for i in range(x): - obs.append((i, 0)) + obs.add((i, 0)) for i in range(x): - obs.append((i, y - 1)) + obs.add((i, y - 1)) for i in range(y): - obs.append((0, i)) + obs.add((0, i)) for i in range(y): - obs.append((x - 1, i)) + obs.add((x - 1, i)) for i in range(10, 21): - obs.append((i, 15)) + obs.add((i, 15)) for i in range(15): - obs.append((20, i)) + obs.add((20, i)) for i in range(15, 30): - obs.append((30, i)) + obs.add((30, i)) for i in range(16): - obs.append((40, i)) + obs.add((40, i)) return obs diff --git a/Search-based Planning/Search_2D/plotting.py b/Search-based Planning/Search_2D/plotting.py index 492cf8f..1121d06 100644 --- a/Search-based Planning/Search_2D/plotting.py +++ b/Search-based Planning/Search_2D/plotting.py @@ -59,8 +59,8 @@ class Plotting: plt.show() def plot_grid(self, name): - obs_x = [self.obs[i][0] for i in range(len(self.obs))] - obs_y = [self.obs[i][1] for i in range(len(self.obs))] + obs_x = [x[0] for x in self.obs] + obs_y = [x[1] for x in self.obs] plt.plot(self.xI[0], self.xI[1], "bs") plt.plot(self.xG[0], self.xG[1], "gs") diff --git a/Search-based Planning/Search_2D/test.py b/Search-based Planning/Search_2D/test.py index 6097bc5..f5d910d 100644 --- a/Search-based Planning/Search_2D/test.py +++ b/Search-based Planning/Search_2D/test.py @@ -17,4 +17,4 @@ 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 +print(U.get())