From 89ecf8c8106073861d5cad68bcbf6ffec0b706d1 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Tue, 30 Jun 2020 14:12:07 -0700 Subject: [PATCH] update --- Search-based Planning/.idea/workspace.xml | 64 +++++++------ Search-based Planning/Search_2D/LPAstar.py | 88 ++++++++++++++---- Search-based Planning/Search_2D/LRTAstar.py | 17 +++- Search-based Planning/Search_2D/RTAAstar.py | 15 ++- .../Search_2D/__pycache__/env.cpython-37.pyc | Bin 1247 -> 1213 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 5272 -> 5216 bytes .../__pycache__/queue.cpython-37.pyc | Bin 3115 -> 3115 bytes Search-based Planning/Search_2D/env.py | 18 ++-- Search-based Planning/Search_2D/plotting.py | 4 +- Search-based Planning/Search_2D/test.py | 2 +- 10 files changed, 141 insertions(+), 67 deletions(-) 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 a03b5a817d509e7d76809a14917f112195fa08f2..82832455a65a8f922223c62697da1c9b6e904b77 100644 GIT binary patch delta 367 zcmcc5xtEjIiI@HGwsS9od8wc9;s4!WU2_9AG0vAihcAgejQ7SjaKihPg+YwTK-Ughd=c;udpp cYRN6uqQt!P)LYDnDU%;ETQagumS#Bu05H>9O#lD@ delta 371 zcmdnXd7qQliIo*oEIp2(P5uf)i}P{WYLP|FA; z8Ecu+8ETm;g!Q^Y0QkWzd zplYmuYCvW%A)ArHSi=I9kAs@Q)Laj;1ZD^$$OyQiI-nwj5~dX96qaTt6sNF4jadS* zCWRHn8aB8IN1!IKrm!QMkirgE@k|M-gad4Z2*fujoG=9w7z^1ZOEC9HvJ|lc1G0z% eNZevAO3X`7y~UPTP>`B8c|WryBg^EU%truM*IIi3 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 178a3ab97c73e93e875565884e26914e992f50af..91533dfa0ea087a1918e9271ff2c52f8913a3315 100644 GIT binary patch delta 746 zcmZuv&ubGw6rR~1+1)0aZqgL8{s0B7Mo39*wbm*^tyDyzSUpzQIO7_-+uhD?Xj4-P z>dA}9T*Q;TiWdoZ_7Cvj!GjDQyw;Og<22 z2=dW$ZNpeW4ZM$K1LjC*XzV!f36`zRK?>Kr?) zd<>q3l_Xr0Co4-p*u&o7wlkyQCVyJIWU8iv55+dt1@7=^_Zhy*TdvQ)xTQl=(!iDg zPD^O$Y#R6sz!`3on+Kaf>!1~w4ZH$yK!O-tFGqMvb!b8I-wse!zMHW1dr`7USv%@d zc9!pt&dtw)0oIqBp94Mz9JBMl;YI#fZ(fFvAay~)_9=d(IL?`O(^I1@fB=64TjZa- zkG3#3@6#X`{7t1kA)G7jp#AUj!3X{3@C z>BP!+4TKSrkZ`!bG9-*N>c(O;rlix2)g{VR`jtq#ERAI2&sA!}y>iJVer4=^O;q9_ SPopG{I_=4{2TX9kw)+#G!L}*@ delta 823 zcma)2O=}ZD7@nC;vb#;T%|}tA(JHmoMH^eK6%V#(i}tLZ1}bcv={9!K-Og^Pfz*N# z6g=sG6!Fl4f1u{($sgdsbMT-?{Ry6&cM}ECg9G!-`##V6IbY81y3P&98AAGukN>#$ z_P{BcJBIe6ml~d;Rm?CWPplZl_!)XYI=F+EPzP_5L^J-W8L_dK($Q&_Dx>60ml4bC z8yF3g%s|O7P|ROSo3ssP6$h@_#``!)vDB`4V!i}p9o68YHd-le!$7EwbpcdHFUV%< z4sx-?IzT@rJfuge(>+|*zNNmKE7$+4#J4|6$O&CZPWG!>>XCJ3jFMVJiLY<8PSbp| z=?T}Cham2-2KC-Lg{k^%ogRb6K*I;ioQ7-qM#CBquHZ`dxIIg-t8Ql(j2?d2eU+_Y zA}7^zr>qK23xhRLIj^JTBEXae*N|1PF91xdkIvLF7tAA|W%}(3K)_GY?&z?l4pv%TT_f!tUX!4{V_(d2SQ9gJL^y9f!%Ul4f->{$(Zqsw61zaiOePP>nRx}r|z zMLwAbc9ru03+he&(@mP*TH{_!bE59A#nE~Tdi87>v^)VOwIkuh+?QTU!muaE zG|M;-2k}(BE4;pY3uHTrIqzqhtlmiSmOvO{0rLl|ECZ1WMYBOi8l2UejigD^FV`o+ nrVItAYOlyG=<5=>sGQP=QCettaVUaV)N9qS1r}9bOFKUSY!|(d 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 b628e1d3e145af1824a6316de94e8698510a8693..71d39d5ebdbd8b2e1f6ad4d3137ca8fd1c280b2e 100644 GIT binary patch delta 58 zcmZ22v08%LiI