From ad80dd02e26dfa4bdbedfafd99a354c86b788ddb Mon Sep 17 00:00:00 2001 From: zhm-real Date: Thu, 2 Jul 2020 17:01:34 -0700 Subject: [PATCH] update queue --- Search-based Planning/.idea/workspace.xml | 80 +++++----- Search-based Planning/Search_2D/ARAstar.py | 102 ++++++------ .../Search_2D/D_star_Lite.py | 102 ++++++------ Search-based Planning/Search_2D/IDAstar.py | 96 ----------- Search-based Planning/Search_2D/LPAstar.py | 87 ++++++---- Search-based Planning/Search_2D/LRTAstar.py | 120 +++++++------- Search-based Planning/Search_2D/RTAAstar.py | 151 ++++++++++-------- .../__pycache__/queue.cpython-37.pyc | Bin 3198 -> 2673 bytes Search-based Planning/Search_2D/queue.py | 16 -- Search-based Planning/Search_2D/test.py | 20 --- Search-based Planning/Search_3D/Astar3D.py | 2 +- 11 files changed, 342 insertions(+), 434 deletions(-) delete mode 100644 Search-based Planning/Search_2D/IDAstar.py delete mode 100644 Search-based Planning/Search_2D/test.py diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 48f135c..a7fa2c4 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,20 +20,15 @@ - - - - - + + - - - - - - + + + + @@ -83,7 +79,7 @@ - + - + + + + - - - - - - + + + - - - - + + + + diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search-based Planning/Search_2D/ARAstar.py index 654afef..68e11bf 100644 --- a/Search-based Planning/Search_2D/ARAstar.py +++ b/Search-based Planning/Search_2D/ARAstar.py @@ -10,7 +10,6 @@ import math 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 @@ -20,37 +19,33 @@ class AraStar: self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type - self.Env = env.Env() # class Env + self.Env = env.Env() # class Env - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles - self.e = e # initial weight - self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + self.e = e # initial weight + self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come - self.OPEN = queue.QueuePrior() # priority queue / U - self.CLOSED = set() # closed set - self.INCONS = [] # incons set - self.PARENT = {self.s_start: self.s_start} # relations - self.path = [] # planning path - self.visited = [] # order of visited nodes + self.OPEN = {self.s_start: self.fvalue(self.s_start)} # priority queue / OPEN set + self.CLOSED = set() # CLOSED set + self.INCONS = {} # INCONS set + self.PARENT = {self.s_start: self.s_start} # relations + self.path = [] # planning path + self.visited = [] # order of visited nodes def searching(self): - self.OPEN.put(self.s_start, self.fvalue(self.s_start)) self.ImprovePath() self.path.append(self.extract_path()) - while self.update_e() > 1: # continue condition - self.e -= 0.5 # increase weight - OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS # combine two sets - self.OPEN = queue.QueuePrior() - self.OPEN.put(self.s_start, self.fvalue(self.s_start)) + while self.update_e() > 1: # continue condition + self.e -= 0.5 # increase weight + self.OPEN.update(self.INCONS) + for s in self.OPEN: + self.OPEN[s] = self.fvalue(s) - for x in OPEN_mid: - self.OPEN.put(x, self.fvalue(x)) # update priority - - self.INCONS = [] + self.INCONS = {} self.CLOSED = set() - self.ImprovePath() # improve path + self.ImprovePath() # improve path self.path.append(self.extract_path()) return self.path, self.visited @@ -62,12 +57,11 @@ class AraStar: visited_each = [] - while (self.fvalue(self.s_goal) > - min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])): - s = self.OPEN.get() - - if s not in self.CLOSED: - self.CLOSED.add(s) + while True: + s, f_small = self.get_smallest_f() + if self.fvalue(self.s_goal) <= f_small: + break + self.CLOSED.add(s) for s_n in self.get_neighbor(s): new_cost = self.g[s] + self.cost(s, s_n) @@ -77,12 +71,26 @@ class AraStar: visited_each.append(s_n) if s_n not in self.CLOSED: - self.OPEN.put(s_n, self.fvalue(s_n)) + self.OPEN[s_n] = self.fvalue(s_n) else: - self.INCONS.append(s_n) + self.INCONS[s_n] = 0 self.visited.append(visited_each) + def get_smallest_f(self): + """ + :return: node with smallest f_value in OPEN set. + """ + s_list = {} + + for s in self.OPEN: + s_list[s] = self.fvalue(s) + s_small = min(s_list, key=s_list.get) + + self.OPEN.pop(s_small) + + return s_small, s_list[s_small] + def get_neighbor(self, s): """ find neighbors of state s that not in obstacles. @@ -100,21 +108,17 @@ class AraStar: return s_list def update_e(self): - c_OPEN, c_INCONS = float("inf"), float("inf") + v = float("inf") if self.OPEN: - c_OPEN = min(self.g[x] + - self.Heuristic(x) for (p, x) in self.OPEN.enumerate()) + v = min(self.g[s] + self.h(s) for s in self.OPEN) if self.INCONS: - c_INCONS = min(self.g[x] + - self.Heuristic(x) for x in self.INCONS) - if min(c_OPEN, c_INCONS) == float("inf"): - return 1 + v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS)) - return min(self.e, self.g[self.s_goal] / min(c_OPEN, c_INCONS)) + return min(self.e, self.g[self.s_goal] / v) def fvalue(self, x): - return self.g[x] + self.e * self.Heuristic(x) + return self.g[x] + self.e * self.h(x) def extract_path(self): """ @@ -134,15 +138,15 @@ class AraStar: return list(path) - def Heuristic(self, s): + def h(self, s): """ Calculate heuristic. :param s: current node (state) :return: heuristic function value """ - heuristic_type = self.heuristic_type # heuristic type - goal = self.s_goal # goal node + heuristic_type = self.heuristic_type # heuristic type + goal = self.s_goal # goal node if heuristic_type == "manhattan": return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) @@ -163,16 +167,14 @@ class AraStar: def main(): - x_start = (5, 5) # Starting node - x_goal = (45, 25) # Goal node + s_start = (5, 5) + s_goal = (45, 25) - arastar = AraStar(x_start, x_goal, 2.5, "manhattan") - plot = plotting.Plotting(x_start, x_goal) + arastar = AraStar(s_start, s_goal, 2.5, "euclidean") + plot = plotting.Plotting(s_start, s_goal) - fig_name = "Anytime Repairing A* (ARA*)" path, visited = arastar.searching() - - plot.animation_ara_star(path, visited, fig_name) + plot.animation_ara_star(path, visited, "Anytime Repairing A* (ARA*)") if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search-based Planning/Search_2D/D_star_Lite.py index df67935..abce0fb 100644 --- a/Search-based Planning/Search_2D/D_star_Lite.py +++ b/Search-based Planning/Search_2D/D_star_Lite.py @@ -11,25 +11,24 @@ import matplotlib.pyplot as plt 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 class DStar: - def __init__(self, x_start, x_goal, heuristic_type): - self.xI, self.xG = x_start, x_goal + def __init__(self, s_start, s_goal, heuristic_type): + self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type self.Env = env.Env() # class Env - self.Plot = plotting.Plotting(x_start, x_goal) + self.Plot = plotting.Plotting(s_start, s_goal) self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles self.x = self.Env.x_range self.y = self.Env.y_range - self.U = queue.QueuePrior() # priority queue / U set + self.U = {} self.g, self.rhs = {}, {} self.km = 0 @@ -38,8 +37,8 @@ class DStar: self.rhs[(i, j)] = float("inf") self.g[(i, j)] = float("inf") - self.rhs[self.xG] = 0 - self.U.put(self.xG, self.Key(self.xG)) + self.rhs[self.s_goal] = 0 + self.U[self.s_goal] = self.CalculateKey(self.s_goal) self.fig = plt.figure() def run(self): @@ -57,15 +56,15 @@ class DStar: x, y = int(x), int(y) print("Change position: x =", x, ",", "y =", y) - s_curr = self.xI - s_last = self.xI + s_curr = self.s_start + s_last = self.s_start i = 0 path = [] - while s_curr != self.xG: + while s_curr != self.s_goal: s_list = {} for s in self.get_neighbor(s_curr): - s_list[s] = self.g[s] + self.get_cost(s_curr, s) + s_list[s] = self.g[s] + self.cost(s_curr, s) s_curr = min(s_list, key=s_list.get) path.append(s_curr) @@ -88,19 +87,18 @@ class DStar: self.plot_path(path) self.fig.canvas.draw_idle() - @staticmethod - def plot_path(path): - px = [x[0] for x in path] - py = [x[1] for x in path] - plt.plot(px, py, marker='o') - def ComputePath(self): - while self.U.top_key() < self.Key(self.xI) or \ - self.rhs[self.xI] != self.g[self.xI]: - k_old = self.U.top_key() - s = self.U.get() - if k_old < self.Key(s): - self.U.put(s, self.Key(s)) + while True: + s, v = self.TopKey() + if v >= self.CalculateKey(self.s_start) and \ + self.rhs[self.s_start] == self.g[self.s_start]: + break + + k_old = v + self.U.pop(s) + + if k_old < self.CalculateKey(s): + self.U[s] = self.CalculateKey(s) elif self.g[s] > self.rhs[s]: self.g[s] = self.rhs[s] for x in self.get_neighbor(s): @@ -112,29 +110,40 @@ class DStar: self.UpdateVertex(x) def UpdateVertex(self, s): - if s != self.xG: + if s != self.s_goal: self.rhs[s] = float("inf") for x in self.get_neighbor(s): - self.rhs[s] = min(self.rhs[s], self.g[x] + self.get_cost(s, x)) - self.U.remove(s) - if self.g[s] != self.rhs[s]: - self.U.put(s, self.Key(s)) + self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x)) + if s in self.U: + self.U.pop(s) - def Key(self, s): - return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km, + if self.g[s] != self.rhs[s]: + self.U[s] = self.CalculateKey(s) + + def CalculateKey(self, s): + return [min(self.g[s], self.rhs[s]) + self.h(self.s_start, s) + self.km, min(self.g[s], self.rhs[s])] + def TopKey(self): + """ + :return: return the min key and its value. + """ + + s = min(self.U, key=self.U.get) + return s, self.U[s] + def h(self, s_start, s_goal): - heuristic_type = self.heuristic_type # heuristic type + heuristic_type = self.heuristic_type # heuristic type if heuristic_type == "manhattan": return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1]) else: return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) - def get_cost(self, s_start, s_end): + def cost(self, s_start, s_end): if s_start in self.obs or s_end in self.obs: return float("inf") + return 1 def get_neighbor(self, s): @@ -148,7 +157,7 @@ class DStar: def extract_path(self): path = [] - s = self.xI + s = self.s_start count = 0 while True: count += 1 @@ -156,31 +165,22 @@ class DStar: for x in self.get_neighbor(s): g_list[x] = self.g[x] s = min(g_list, key=g_list.get) - if s == self.xG or count > 100: + if s == self.s_goal or count > 100: return list(reversed(path)) path.append(s) - def print_g(self): - print("he") - for k in range(self.Env.y_range): - j = self.Env.y_range - k - 1 - string = "" - for i in range(self.Env.x_range): - if self.g[(i, j)] == float("inf"): - string += ("00" + ', ') - else: - if self.g[(i, j)] // 10 == 0: - string += ("0" + str(self.g[(i, j)]) + ', ') - else: - string += (str(self.g[(i, j)]) + ', ') - print(string) + @staticmethod + def plot_path(path): + px = [x[0] for x in path] + py = [x[1] for x in path] + plt.plot(px, py, marker='o') def main(): - x_start = (5, 5) - x_goal = (45, 25) + s_start = (5, 5) + s_goal = (45, 25) - dstar = DStar(x_start, x_goal, "euclidean") + dstar = DStar(s_start, s_goal, "euclidean") dstar.run() diff --git a/Search-based Planning/Search_2D/IDAstar.py b/Search-based Planning/Search_2D/IDAstar.py deleted file mode 100644 index 6fd06e2..0000000 --- a/Search-based Planning/Search_2D/IDAstar.py +++ /dev/null @@ -1,96 +0,0 @@ -""" -IDA_Star 2D (Iteratively Deepening A*) -@author: huiming zhou -""" - -import os -import sys -import matplotlib.pyplot as plt - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") - -from Search_2D import plotting -from Search_2D import env - - -class IdaStar: - 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.visited = [] - - def ida_star(self): - bound = self.h(self.xI) - path = [self.xI] - - while True: - t = self.searching(path, 0, bound) - if t == self.xG: - return path, self.visited - if t == float("inf"): - return [], self.visited - bound = t - - def searching(self, path, g, bound): - s = path[-1] - self.visited.append(s) - f = g + self.h(s) - - if f > bound: - return f - if s == self.xG: - return s - - res_min = float("inf") - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(len(s))]) - if s_next not in self.obs and s_next not in path: - path.append(s_next) - t = self.searching(path, g + 1, bound) - if t == self.xG: - return self.xG - if t < res_min: - res_min = t - path.pop() - - return res_min - - def h(self, s): - heuristic_type = self.heuristic_type - goal = self.xG - - 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!") - - -def main(): - x_start = (5, 5) - x_goal = (15, 20) - - ida_star = IdaStar(x_start, x_goal, "manhattan") - plot = plotting.Plotting(x_start, x_goal) - - path, visited = ida_star.ida_star() - - if path: - plot.plot_grid("Iteratively Deepening A*") - plot.plot_path(visited, 'gray', True) - plot.plot_path(path) - plt.show() - else: - print("Path not found!") - - -if __name__ == '__main__': - main() diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py index 8bd2af3..8553347 100644 --- a/Search-based Planning/Search_2D/LPAstar.py +++ b/Search-based Planning/Search_2D/LPAstar.py @@ -11,25 +11,24 @@ import matplotlib.pyplot as plt 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 class LpaStar: - def __init__(self, x_start, x_goal, heuristic_type): - self.s_start, self.s_goal = x_start, x_goal + def __init__(self, s_start, s_goal, heuristic_type): + self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type self.Env = env.Env() - self.Plot = plotting.Plotting(x_start, x_goal) + self.Plot = plotting.Plotting(self.s_start, self.s_goal) self.u_set = self.Env.motions self.obs = self.Env.obs self.x = self.Env.x_range self.y = self.Env.y_range - self.U = queue.QueuePrior() + self.U = {} self.g, self.rhs = {}, {} for i in range(self.Env.x_range): @@ -38,7 +37,8 @@ class LpaStar: self.g[(i, j)] = float("inf") self.rhs[self.s_start] = 0 - self.U.put(self.s_start, self.Key(self.s_start)) + self.U[self.s_start] = self.CalculateKey(self.s_start) + self.fig = plt.figure() def run(self): @@ -73,13 +73,16 @@ class LpaStar: self.fig.canvas.draw_idle() def ComputePath(self): - while self.U.top_key() < self.Key(self.s_goal) or \ - self.rhs[self.s_goal] != self.g[self.s_goal]: - s = self.U.get() + while True: + s, v = self.TopKey() + if v >= self.CalculateKey(self.s_goal) and \ + self.rhs[self.s_goal] == self.g[self.s_goal]: + break + self.U.pop(s) - if self.g[s] > self.rhs[s]: # over-consistent: deleted obstacles + if self.g[s] > self.rhs[s]: # over-consistent: deleted obstacles self.g[s] = self.rhs[s] - else: # under-consistent: added obstacles + else: # under-consistent: added obstacles self.g[s] = float("inf") self.UpdateVertex(s) for s_n in self.get_neighbor(s): @@ -87,11 +90,25 @@ class LpaStar: def UpdateVertex(self, s): if s != self.s_start: - self.rhs[s] = min([self.g[s_n] + self.cost(s_n, s) - for s_n in self.get_neighbor(s)]) - self.U.remove(s) + self.rhs[s] = min(self.g[s_n] + self.cost(s_n, s) + for s_n in self.get_neighbor(s)) + if s in self.U: + self.U.pop(s) + if self.g[s] != self.rhs[s]: - self.U.put(s, self.Key(s)) + self.U[s] = self.CalculateKey(s) + + def TopKey(self): + """ + :return: return the min key and its value. + """ + + s = min(self.U, key=self.U.get) + return s, self.U[s] + + def CalculateKey(self, s): + return [min(self.g[s], self.rhs[s]) + self.h(s), + min(self.g[s], self.rhs[s])] def get_neighbor(self, s): """ @@ -109,11 +126,13 @@ class LpaStar: return s_list - def Key(self, s): - return [min(self.g[s], self.rhs[s]) + self.h(s), - min(self.g[s], self.rhs[s])] - def h(self, s): + """ + Calculate heuristic. + :param s: current node (state) + :return: heuristic function value + """ + heuristic_type = self.heuristic_type # heuristic type goal = self.s_goal # goal node @@ -123,11 +142,25 @@ class LpaStar: return math.hypot(goal[0] - s[0], goal[1] - s[1]) def cost(self, s_start, s_end): + """ + calculate edge cost: (s_start, s_end) + :param s_start: start node + :param s_end: end node + :return: cost + """ + + # if one of the vertex in obstacles: return infinity. if s_start in self.obs or s_end in self.obs: return float("inf") + return 1 def extract_path(self): + """ + Extract the path based on the PARENT set. + :return: The planning path + """ + path = [] s = self.s_goal @@ -139,6 +172,7 @@ class LpaStar: if s == self.s_start: break path.append(s) + return list(reversed(path)) @staticmethod @@ -147,21 +181,6 @@ class LpaStar: py = [x[1] for x in path] plt.plot(px, py, marker='o') - def print_g(self): - print("he") - for k in range(self.Env.y_range): - j = self.Env.y_range - k - 1 - string = "" - for i in range(self.Env.x_range): - if self.g[(i, j)] == float("inf"): - string += ("00" + ', ') - else: - if self.g[(i, j)] // 10 == 0: - string += ("0" + str(self.g[(i, j)]) + ', ') - else: - string += (str(self.g[(i, j)]) + ', ') - print(string) - def main(): x_start = (5, 5) diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search-based Planning/Search_2D/LRTAstar.py index 9e3d6d6..34bcd1e 100644 --- a/Search-based Planning/Search_2D/LRTAstar.py +++ b/Search-based Planning/Search_2D/LRTAstar.py @@ -6,6 +6,7 @@ LRTA_star 2D (Learning Real-time A*) import os import sys import copy +import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -16,8 +17,8 @@ from Search_2D import env class LrtAstarN: - def __init__(self, x_start, x_goal, N, heuristic_type): - self.xI, self.xG = x_start, x_goal + def __init__(self, s_start, s_goal, N, heuristic_type): + self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type self.Env = env.Env() @@ -35,7 +36,7 @@ class LrtAstarN: self.h_table[(i, j)] = self.h((i, j)) # initialize h_value def searching(self): - s_start = self.xI # initialize start node + s_start = self.s_start # initialize start node while True: OPEN, CLOSED = self.Astar(s_start, self.N) # U, CLOSED sets in each iteration @@ -58,13 +59,11 @@ class LrtAstarN: while True: h_list = {} - 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: - if s_next in h_value: - h_list[s_next] = h_value[s_next] - else: - h_list[s_next] = self.h_table[s_next] + for s_n in self.get_neighbor(s): + if s_n in h_value: + h_list[s_n] = h_value[s_n] + else: + h_list[s_n] = self.h_table[s_n] 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 @@ -82,13 +81,11 @@ class LrtAstarN: h_value_rec = copy.deepcopy(h_value) for s in CLOSED: h_list = [] - 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: - if s_next not in CLOSED: - 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]) + for s_n in self.get_neighbor(s): + if s_n not in CLOSED: + h_list.append(self.cost(s, s_n) + self.h_table[s_n]) + else: + h_list.append(self.cost(s, s_n) + h_value[s_n]) h_value[s] = min(h_list) # update h_value of current node if h_value == h_value_rec: # h_value table converged @@ -97,40 +94,53 @@ class LrtAstarN: def Astar(self, x_start, N): OPEN = queue.QueuePrior() # U set OPEN.put(x_start, self.h(x_start)) - CLOSED = set() # CLOSED set - g_table = {x_start: 0, self.xG: float("inf")} # cost to come + CLOSED = [] # CLOSED set + g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come PARENT = {x_start: x_start} # relations - visited = [] # order of visited nodes count = 0 # counter while not OPEN.empty(): count += 1 s = OPEN.get() - CLOSED.add(s) - visited.append(s) + CLOSED.append(s) - if s == self.xG: # reach the goal node - self.visited.append(visited) + if s == self.s_goal: # reach the goal node + self.visited.append(CLOSED) return "FOUND", self.extract_path(x_start, PARENT) - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(len(s))]) - if s_next not in self.obs and s_next not in CLOSED: - new_cost = g_table[s] + self.get_cost(s, u) - if s_next not in g_table: - g_table[s_next] = float("inf") - 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_table[s_next]) + for s_n in self.get_neighbor(s): + if s_n not in CLOSED: + new_cost = g_table[s] + self.cost(s, s_n) + if s_n not in g_table: + g_table[s_n] = float("inf") + if new_cost < g_table[s_n]: # conditions for updating cost + g_table[s_n] = new_cost + PARENT[s_n] = s + OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) - if count == N: # expand needed CLOSED nodes + if count == N: # expand needed CLOSED nodes break - self.visited.append(visited) # visited nodes in each iteration + self.visited.append(CLOSED) # visited nodes in each iteration return OPEN, CLOSED + def get_neighbor(self, s): + """ + find neighbors of state s that not in obstacles. + :param s: state + :return: neighbors + """ + + s_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: + s_list.add(s_next) + + return s_list + def extract_path(self, x_start, parent): """ Extract the path based on the relationship of nodes. @@ -138,8 +148,8 @@ class LrtAstarN: :return: The planning path """ - path_back = [self.xG] - x_current = self.xG + path_back = [self.s_goal] + x_current = self.s_goal while True: x_current = parent[x_current] @@ -151,23 +161,25 @@ class LrtAstarN: return list(reversed(path_back)) def h(self, s): - heuristic_type = self.heuristic_type - goal = self.xG + """ + Calculate heuristic. + :param s: current node (state) + :return: heuristic function value + """ + + heuristic_type = self.heuristic_type # heuristic type + goal = self.s_goal # 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!") + return math.hypot(goal[0] - s[0], goal[1] - s[1]) - @staticmethod - def get_cost(x, u): + def cost(self, s_start, s_end): """ Calculate cost for this motion - - :param x: current node - :param u: input + :param s_start: starting node + :param s_end: end node :return: cost for this motion :note: cost function could be more complicate! """ @@ -176,15 +188,15 @@ class LrtAstarN: def main(): - x_start = (10, 5) - x_goal = (45, 25) + s_start = (10, 5) + s_goal = (45, 25) - lrta = LrtAstarN(x_start, x_goal, 200, "euclidean") - plot = plotting.Plotting(x_start, x_goal) - fig_name = "Learning Real-time A* (LRTA*)" + lrta = LrtAstarN(s_start, s_goal, 250, "euclidean") + plot = plotting.Plotting(s_start, s_goal) lrta.searching() - plot.animation_lrta(lrta.path, lrta.visited, fig_name) + plot.animation_lrta(lrta.path, lrta.visited, + "Learning Real-time A* (LRTA*)") if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search-based Planning/Search_2D/RTAAstar.py index 233bfbd..97b2267 100644 --- a/Search-based Planning/Search_2D/RTAAstar.py +++ b/Search-based Planning/Search_2D/RTAAstar.py @@ -6,6 +6,7 @@ RTAAstar 2D (Real-time Adaptive A*) import os import sys import copy +import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -16,8 +17,8 @@ from Search_2D import env class RtaAstar: - def __init__(self, x_start, x_goal, N, heuristic_type): - self.xI, self.xG = x_start, x_goal + def __init__(self, s_start, s_goal, N, heuristic_type): + self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type self.Env = env.Env() @@ -35,7 +36,7 @@ class RtaAstar: self.h_table[(i, j)] = self.h((i, j)) # initialize h_value def searching(self): - s_start = self.xI # initialize start node + s_start = self.s_start # initialize start node while True: OPEN, CLOSED, g_table, PARENT = \ @@ -59,29 +60,12 @@ class RtaAstar: for (_, x) in OPEN.enumerate(): 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()) + f_min = v_open[s_open] for x in CLOSED: h_value[x] = f_min - g_table[x] return s_open, h_value - def extract_path_in_CLOSE(self, s_end, s_start, h_value): - path = [s_start] - s = s_start - - while True: - h_list = {} - 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 and s_next in h_value: - h_list[s_next] = h_value[s_next] - s_key = max(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 - - if s_key == s_end: # reach the expected node in U set - return s_start, list(reversed(path)) - def iteration(self, CLOSED): h_value = {} @@ -92,13 +76,11 @@ class RtaAstar: h_value_rec = copy.deepcopy(h_value) for s in CLOSED: h_list = [] - 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: - if s_next not in CLOSED: - 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]) + for s_n in self.get_neighbor(s): + if s_n not in CLOSED: + h_list.append(self.cost(s, s_n) + self.h_table[s_n]) + else: + h_list.append(self.cost(s, s_n) + h_value[s_n]) h_value[s] = min(h_list) # update h_value of current node if h_value == h_value_rec: # h_value table converged @@ -107,77 +89,106 @@ class RtaAstar: def Astar(self, x_start, N): OPEN = queue.QueuePrior() # U set OPEN.put(x_start, self.h_table[x_start]) - CLOSED = set() # CLOSED set - g_table = {x_start: 0, self.xG: float("inf")} # cost to come + CLOSED = [] # CLOSED set + g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come PARENT = {x_start: x_start} # relations - visited = [] # order of visited nodes count = 0 # counter while not OPEN.empty(): count += 1 s = OPEN.get() - CLOSED.add(s) - visited.append(s) + CLOSED.append(s) - if s == self.xG: # reach the goal node - self.visited.append(visited) + if s == self.s_goal: # reach the goal node + self.visited.append(CLOSED) return "FOUND", self.extract_path(x_start, PARENT), [], [] - for u in self.u_set: - s_next = tuple([s[i] + u[i] for i in range(len(s))]) - if s_next not in self.obs and s_next not in CLOSED: - new_cost = g_table[s] + self.get_cost(s, u) - if s_next not in g_table: - g_table[s_next] = float("inf") - 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_table[s_next]) + for s_n in self.get_neighbor(s): + if s_n not in CLOSED: + new_cost = g_table[s] + self.cost(s, s_n) + if s_n not in g_table: + g_table[s_n] = float("inf") + if new_cost < g_table[s_n]: # conditions for updating cost + g_table[s_n] = new_cost + PARENT[s_n] = s + OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) if count == N: # expand needed CLOSED nodes break - self.visited.append(visited) # visited nodes in each iteration + self.visited.append(CLOSED) # visited nodes in each iteration return OPEN, CLOSED, g_table, PARENT + def get_neighbor(self, s): + """ + find neighbors of state s that not in obstacles. + :param s: state + :return: neighbors + """ + + s_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: + s_list.add(s_next) + + return s_list + + def extract_path_in_CLOSE(self, s_end, s_start, h_value): + path = [s_start] + s = s_start + + while True: + h_list = {} + for s_n in self.get_neighbor(s): + if s_n in h_value: + h_list[s_n] = h_value[s_n] + s_key = max(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 + + if s_key == s_end: # reach the expected node in U set + return s_start, list(reversed(path)) + def extract_path(self, x_start, parent): """ Extract the path based on the relationship of nodes. - :return: The planning path """ - path_back = [self.xG] - x_current = self.xG + path = [self.s_goal] + s = self.s_goal while True: - x_current = parent[x_current] - path_back.append(x_current) - - if x_current == x_start: + s = parent[s] + path.append(s) + if s == x_start: break - return list(reversed(path_back)) + return list(reversed(path)) def h(self, s): - heuristic_type = self.heuristic_type - goal = self.xG + """ + Calculate heuristic. + :param s: current node (state) + :return: heuristic function value + """ + + heuristic_type = self.heuristic_type # heuristic type + goal = self.s_goal # 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!") + return math.hypot(goal[0] - s[0], goal[1] - s[1]) - @staticmethod - def get_cost(x, u): + def cost(self, s_start, s_end): """ Calculate cost for this motion - - :param x: current node - :param u: input + :param s_start: starting node + :param s_end: end node :return: cost for this motion :note: cost function could be more complicate! """ @@ -186,15 +197,15 @@ class RtaAstar: def main(): - x_start = (10, 5) - x_goal = (45, 25) + s_start = (10, 5) + s_goal = (45, 25) - rtaa = RtaAstar(x_start, x_goal, 200, "euclidean") - plot = plotting.Plotting(x_start, x_goal) - fig_name = "Real-time Adaptive A* (RTAA*)" + rtaa = RtaAstar(s_start, s_goal, 220, "euclidean") + plot = plotting.Plotting(s_start, s_goal) rtaa.searching() - plot.animation_lrta(rtaa.path, rtaa.visited, fig_name) + plot.animation_lrta(rtaa.path, rtaa.visited, + "Real-time Adaptive A* (RTAA*)") if __name__ == '__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 ad642f84476c015adc48585c358843f83359a140..a4592425fee42f1cdfe952de2e8095143300c66d 100644 GIT binary patch delta 146 zcmew-@lk}&iI=b6h{hAFoP!VW^wk#jEqr} zcXL=y-p#4Q^pbJ%OHN}(tI4ulsVcTW-Nj5GGZ?Ex0!vd%Qv-@J^NaLS^Gb75ixNvx q{WLiz@8?#TynvaNQFih|W{JrHJe&eNKn)x$EKD5C5I9+%#~%PH`y;sk delta 588 zcmY*V-AWrl6rMBtli9dQV^XcP3)&chThc=9g(#s?tJoW3L`tF57I9Xvp-r45lFBaL zcf;YZ^dd?<@p5gn>%$&dZ=JMgyM0PooNi($9r&ph?)Sv8` z4Rw8Cye!F)6ZTO^S7y*VGAkdTPs^MfML#0*Tg)$5u*WGD8dX+DmVBN?#)12RNroegngZ1E*Jy7%Hip$2Pm zf(Fr00)88g82p%C;@-1Vr`h__2!5