diff --git a/README.md b/README.md index 975de25..5d0dcb7 100644 --- a/README.md +++ b/README.md @@ -11,10 +11,14 @@ Directory Structure ├── ARAstar.py # Anytime Reparing A* ├── IDAstar.py # Iteratively Deepening A* ├── LRTAstar.py # Learning Real-time A* - └── RTAAstar.py # Real-time Adaptive A* + ├── RTAAstar.py # Real-time Adaptive A* + ├── LPAstar.py # Lifelong Planning A* + ├── D_star.py # D* (Dynamic A*) + └── D_star_Lite.py # D* Lite └── Search_3D ├── Astar3D.py # A*_3D ├── bidirectional_Astar3D.py # Bidirectional A*_3D + ├── RTA_Astar3D.py # Real-time Adaptive A*_3D └── LRT_Astar3D.py # Learning Real-time A*_3D └── gif # Animations └── Sampling-based Planning diff --git a/Sampling-based Planning/.idea/Sampling-based Planning.iml b/Sampling-based Planning/.idea/Sampling-based Planning.iml index 5965bde..c444878 100644 --- a/Sampling-based Planning/.idea/Sampling-based Planning.iml +++ b/Sampling-based Planning/.idea/Sampling-based Planning.iml @@ -2,7 +2,7 @@ - + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/misc.xml b/Sampling-based Planning/.idea/misc.xml index 0e7ac62..a2e120d 100644 --- a/Sampling-based Planning/.idea/misc.xml +++ b/Sampling-based Planning/.idea/misc.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc index 12d1bb9..47fc5c8 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc index 8eee08a..bbadd8b 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc index 3083612..68d12de 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc index c7c08f6..d3f1a64 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py index 70db37e..af36877 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star.py +++ b/Sampling-based Planning/rrt_2D/rrt_star.py @@ -161,7 +161,7 @@ def main(): x_start = (2, 2) # Starting node x_goal = (49, 24) # Goal node - rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 20000) + rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 10000) path = rrt_star.planning() if path: diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index fdcfda4..48f135c 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,13 +20,21 @@ + + + + + - - - - + + + + + + + + - + + + + + - - + + - + - - - - + + + + + + + + - - - - - - - - + + + + @@ -226,6 +239,14 @@ @@ -241,39 +262,43 @@ + + + - + - + - + - - + + - - + + - - + + - + - + - + \ No newline at end of file diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search-based Planning/Search_2D/ARAstar.py index b9b305b..654afef 100644 --- a/Search-based Planning/Search_2D/ARAstar.py +++ b/Search-based Planning/Search_2D/ARAstar.py @@ -5,6 +5,7 @@ ARA_star 2D (Anytime Repairing A*) import os import sys +import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -15,8 +16,8 @@ from Search_2D import env class AraStar: - def __init__(self, x_start, x_goal, e, heuristic_type): - self.xI, self.xG = x_start, x_goal + def __init__(self, s_start, s_goal, e, heuristic_type): + self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type self.Env = env.Env() # class Env @@ -24,17 +25,17 @@ class AraStar: 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.xI: 0, self.xG: float("inf")} # cost to come + self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come - self.OPEN = queue.QueuePrior() # priority queue / OPEN + self.OPEN = queue.QueuePrior() # priority queue / U self.CLOSED = set() # closed set self.INCONS = [] # incons set - self.PARENT = {self.xI: self.xI} # relations + 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.xI, self.fvalue(self.xI)) + self.OPEN.put(self.s_start, self.fvalue(self.s_start)) self.ImprovePath() self.path.append(self.extract_path()) @@ -42,7 +43,7 @@ class AraStar: 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.xI, self.fvalue(self.xI)) + self.OPEN.put(self.s_start, self.fvalue(self.s_start)) for x in OPEN_mid: self.OPEN.put(x, self.fvalue(x)) # update priority @@ -61,94 +62,105 @@ class AraStar: visited_each = [] - while (self.fvalue(self.xG) > + 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) - for u_next in self.u_set: - s_next = tuple([s[i] + u_next[i] for i in range(len(s))]) - if s_next not in self.obs: - new_cost = self.g[s] + self.get_cost(s, u_next) - if s_next not in self.g or new_cost < self.g[s_next]: - self.g[s_next] = new_cost - self.PARENT[s_next] = s - visited_each.append(s_next) + for s_n in self.get_neighbor(s): + new_cost = self.g[s] + self.cost(s, s_n) + if s_n not in self.g or new_cost < self.g[s_n]: + self.g[s_n] = new_cost + self.PARENT[s_n] = s + visited_each.append(s_n) - if s_next not in self.CLOSED: - self.OPEN.put(s_next, self.fvalue(s_next)) - else: - self.INCONS.append(s_next) + if s_n not in self.CLOSED: + self.OPEN.put(s_n, self.fvalue(s_n)) + else: + self.INCONS.append(s_n) self.visited.append(visited_each) + 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 update_e(self): c_OPEN, c_INCONS = float("inf"), float("inf") if self.OPEN: - c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate()) - + c_OPEN = min(self.g[x] + + self.Heuristic(x) for (p, x) in self.OPEN.enumerate()) if self.INCONS: - c_INCONS = min(self.g[x] + self.Heuristic(x) for x in 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 - return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS)) + return min(self.e, self.g[self.s_goal] / min(c_OPEN, c_INCONS)) def fvalue(self, x): return self.g[x] + self.e * self.Heuristic(x) def extract_path(self): """ - Extract the path based on the relationship of nodes. - + Extract the path based on the PARENT set. :return: The planning path """ - path_back = [self.xG] - x_current = self.xG + path = [self.s_goal] + s = self.s_goal while True: - x_current = self.PARENT[x_current] - path_back.append(x_current) + s = self.PARENT[s] + path.append(s) - if x_current == self.xI: + if s == self.s_start: break - return list(path_back) + return list(path) + + def Heuristic(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 + + if heuristic_type == "manhattan": + return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) + else: + return math.hypot(goal[0] - s[0], goal[1] - s[1]) @staticmethod - def get_cost(x, u): + def cost(s_start, s_goal): """ Calculate cost for this motion - :param x: current node - :param u: input + :param s_start: starting node + :param s_goal: end node :return: cost for this motion :note: cost function could be more complicate! """ return 1 - def Heuristic(self, state): - """ - Calculate heuristic. - :param state: current node (state) - :return: heuristic - """ - - heuristic_type = self.heuristic_type - goal = self.xG - - if heuristic_type == "manhattan": - return abs(goal[0] - state[0]) + abs(goal[1] - state[1]) - elif heuristic_type == "euclidean": - return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2) - else: - print("Please choose right heuristic type!") - def main(): x_start = (5, 5) # Starting node diff --git a/Search-based Planning/Search_2D/D_star.py b/Search-based Planning/Search_2D/D_star.py new file mode 100644 index 0000000..c8ddbcd --- /dev/null +++ b/Search-based Planning/Search_2D/D_star.py @@ -0,0 +1,186 @@ +""" +D_star 2D +@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 Dstar: + def __init__(self, x_start, x_goal): + self.xI, self.xG = x_start, x_goal + + self.Env = env.Env() + self.Plot = plotting.Plotting(self.xI, self.xG) + + self.u_set = self.Env.motions + self.obs = self.Env.obs + self.x = self.Env.x_range + self.y = self.Env.y_range + + self.fig = plt.figure() + self.OPEN = set() + self.t = {} + self.PARENT = {} + self.h = {self.xG: 0} + self.k = {} + self.path = [] + + for i in range(self.Env.x_range): + for j in range(self.Env.y_range): + self.t[(i, j)] = 'NEW' + self.k[(i, j)] = 0 + self.PARENT[(i, j)] = None + + def run(self, s_start, s_end): + self.insert(s_end, 0) + while True: + self.process_state() + if self.t[s_start] == 'CLOSED': + break + self.path = self.extract_path(s_start, s_end) + self.Plot.plot_grid("Dynamic A* (D*)") + self.plot_path(self.path) + self.fig.canvas.mpl_connect('button_press_event', self.on_press) + plt.show() + + def on_press(self, event): + x, y = event.xdata, event.ydata + if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1: + print("Please choose right area!") + else: + x, y = int(x), int(y) + print("Add obstacle at: x =", x, ",", "y =", y) + self.obs.add((x, y)) + plt.plot(x, y, 'sk') + if (x, y) in self.path: + s = self.xI + while s != self.xG: + if self.PARENT[s] in self.obs: + self.modify(s) + continue + s = self.PARENT[s] + self.path = self.extract_path(self.xI, self.xG) + self.plot_path(self.path) + self.fig.canvas.draw_idle() + + def extract_path(self, s_start, s_end): + path = [] + s = s_start + while True: + s = self.PARENT[s] + if s == s_end: + return path + path.append(s) + + def process_state(self): + s = self.min_state() + if s is None: + return -1 + k_old = self.get_k_min() + self.delete(s) + + if k_old < self.h[s]: + for s_n in self.get_neighbor(s): + if self.h[s_n] <= k_old and self.h[s] > self.h[s_n] + self.cost(s_n, s): + self.PARENT[s] = s_n + self.h[s] = self.h[s_n] + self.cost(s_n, s) + if k_old == self.h[s]: + for s_n in self.get_neighbor(s): + if self.t[s_n] == 'NEW' or \ + (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \ + (self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)): + self.PARENT[s_n] = s + self.insert(s_n, self.h[s] + self.cost(s, s_n)) + else: + for s_n in self.get_neighbor(s): + if self.t[s_n] == 'NEW' or \ + (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)): + self.PARENT[s_n] = s + self.insert(s_n, self.h[s] + self.cost(s, s_n)) + else: + if self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n): + self.insert(s, self.h[s]) + else: + if self.PARENT[s_n] != s and \ + self.h[s] > self.h[s_n] + self.cost(s_n, s) and \ + self.t[s_n] == 'CLOSED' and \ + self.h[s_n] > k_old: + self.insert(s_n, self.h[s_n]) + return self.get_k_min() + + def min_state(self): + if not self.OPEN: + return None + return min(self.OPEN, key=lambda x: self.k[x]) + + def get_k_min(self): + if not self.OPEN: + return -1 + return min([self.k[x] for x in self.OPEN]) + + def insert(self, s, h_new): + if self.t[s] == 'NEW': + self.k[s] = h_new + elif self.t[s] == 'OPEN': + self.k[s] = min(self.k[s], h_new) + elif self.t[s] == 'CLOSED': + self.k[s] = min(self.h[s], h_new) + self.h[s] = h_new + self.t[s] = 'OPEN' + self.OPEN.add(s) + + def delete(self, s): + if self.t[s] == 'OPEN': + self.t[s] = 'CLOSED' + self.OPEN.remove(s) + + def modify(self, s): + self.modify_cost(s) + while True: + k_min = self.process_state() + if k_min >= self.h[s]: + break + + def modify_cost(self, s): + if self.t[s] == 'CLOSED': + self.insert(s, self.h[self.PARENT[s]] + self.cost(s, self.PARENT[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 cost(self, s_start, s_end): + if s_start in self.obs or s_end in self.obs: + return float("inf") + return 1 + + @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(): + s_start = (5, 5) + s_goal = (45, 25) + dstar = Dstar(s_start, s_goal) + dstar.run(s_start, s_goal) + + +if __name__ == '__main__': + main() diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search-based Planning/Search_2D/D_star_Lite.py new file mode 100644 index 0000000..df67935 --- /dev/null +++ b/Search-based Planning/Search_2D/D_star_Lite.py @@ -0,0 +1,188 @@ +""" +D_star_Lite 2D +@author: huiming zhou +""" + +import os +import sys +import math +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 + self.heuristic_type = heuristic_type + + self.Env = env.Env() # class Env + self.Plot = plotting.Plotting(x_start, x_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.g, self.rhs = {}, {} + self.km = 0 + + 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.xG] = 0 + self.U.put(self.xG, self.Key(self.xG)) + self.fig = plt.figure() + + def run(self): + self.Plot.plot_grid("Dynamic A* (D*)") + self.ComputePath() + self.plot_path(self.extract_path()) + self.fig.canvas.mpl_connect('button_press_event', self.on_press) + plt.show() + + def on_press(self, event): + x, y = event.xdata, event.ydata + if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1: + print("Please choose right area!") + else: + x, y = int(x), int(y) + print("Change position: x =", x, ",", "y =", y) + + s_curr = self.xI + s_last = self.xI + i = 0 + path = [] + + while s_curr != self.xG: + s_list = {} + for s in self.get_neighbor(s_curr): + s_list[s] = self.g[s] + self.get_cost(s_curr, s) + s_curr = min(s_list, key=s_list.get) + path.append(s_curr) + + if i < 1: + self.km += self.h(s_last, s_curr) + s_last = s_curr + if (x, y) not in self.obs: + self.obs.add((x, y)) + plt.plot(x, y, 'sk') + self.g[(x, y)] = float("inf") + self.rhs[(x, y)] = float("inf") + else: + self.obs.remove((x, y)) + plt.plot(x, y, marker='s', color='white') + self.UpdateVertex((x, y)) + for s in self.get_neighbor((x, y)): + self.UpdateVertex(s) + i += 1 + self.ComputePath() + 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)) + elif 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 UpdateVertex(self, s): + if s != self.xG: + 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)) + + def Key(self, s): + return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km, + min(self.g[s], self.rhs[s])] + + def h(self, s_start, s_goal): + 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): + if s_start in self.obs or s_end in self.obs: + return float("inf") + return 1 + + 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 extract_path(self): + path = [] + s = self.xI + count = 0 + while True: + count += 1 + 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.xG 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) + + +def main(): + x_start = (5, 5) + x_goal = (45, 25) + + dstar = DStar(x_start, x_goal, "euclidean") + dstar.run() + + +if __name__ == '__main__': + main() diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py index b2c420a..8bd2af3 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 math import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + @@ -17,15 +18,18 @@ from Search_2D import env class LpaStar: def __init__(self, x_start, x_goal, heuristic_type): - self.xI, self.xG = x_start, x_goal + self.s_start, self.s_goal = x_start, x_goal self.heuristic_type = heuristic_type - self.Env = env.Env() # class Env + self.Env = env.Env() + self.Plot = plotting.Plotting(x_start, x_goal) - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + 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() # priority queue / OPEN set + self.U = queue.QueuePrior() self.g, self.rhs = {}, {} for i in range(self.Env.x_range): @@ -33,152 +37,138 @@ class LpaStar: self.rhs[(i, j)] = float("inf") self.g[(i, j)] = float("inf") - self.rhs[self.xI] = 0 - self.U.put(self.xI, self.CalculateKey(self.xI)) + self.rhs[self.s_start] = 0 + self.U.put(self.s_start, self.Key(self.s_start)) + self.fig = plt.figure() - def searching(self): - self.computePath() - path = [self.extract_path()] + def run(self): + self.Plot.plot_grid("Lifelong Planning A*") - 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() + self.plot_path(self.extract_path()) + self.fig.canvas.mpl_connect('button_press_event', self.on_press) - self.computePath() - path.append(self.extract_path_test()) + plt.show() - return path, obs_change - - 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] + def on_press(self, event): + x, y = event.xdata, event.ydata + if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1: + print("Please choose right area!") + else: + x, y = int(x), int(y) + print("Change position: x =", x, ",", "y =", y) + if (x, y) not in self.obs: + self.obs.add((x, y)) + plt.plot(x, y, 'sk') else: + self.obs.remove((x, y)) + plt.plot(x, y, marker='s', color='white') + self.UpdateVertex((x, y)) + + for s_n in self.get_neighbor((x, y)): + self.UpdateVertex(s_n) + + self.ComputePath() + self.plot_path(self.extract_path()) + 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() + + if self.g[s] > self.rhs[s]: # over-consistent: deleted obstacles + self.g[s] = self.rhs[s] + else: # under-consistent: added obstacles self.g[s] = float("inf") self.UpdateVertex(s) - for x in self.get_neighbor(s): - self.UpdateVertex(x) - # return self.extract_path() + for s_n in self.get_neighbor(s): + self.UpdateVertex(s_n) - 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 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 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) + if self.g[s] != self.rhs[s]: + self.U.put(s, self.Key(s)) def get_neighbor(self, s): - nei_list = set() + """ + 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: - nei_list.add(s_next) + s_list.add(s_next) - return nei_list + return s_list - def CalculateKey(self, s): + def Key(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] + self.get_cost(u, x)) - 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 + 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]) - def get_cost(self, s_start, s_end): - """ - Calculate cost for this motion + def cost(self, s_start, s_end): + if s_start in self.obs or s_end in self.obs: + return float("inf") + return 1 - :param s_start: - :param s_end: - :return: cost for this motion - :note: cost function could be more complicate! - """ + def extract_path(self): + path = [] + s = self.s_goal - if s_start not in self.obs: - if s_end not in self.obs: - return 1 - else: - return float("inf") - return float("inf") + for k in range(100): + 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.s_start: + break + path.append(s) + return list(reversed(path)) + + @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 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) x_goal = (45, 25) - lpastar = LpaStar(x_start, x_goal, "euclidean") - plot = plotting.Plotting(x_start, x_goal) - - 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() + lpastar = LpaStar(x_start, x_goal, "manhattan") + lpastar.run() if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search-based Planning/Search_2D/LRTAstar.py index fc0b401..9e3d6d6 100644 --- a/Search-based Planning/Search_2D/LRTAstar.py +++ b/Search-based Planning/Search_2D/LRTAstar.py @@ -6,7 +6,6 @@ LRTA_star 2D (Learning Real-time A*) import os import sys import copy -import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -39,7 +38,7 @@ class LrtAstarN: s_start = self.xI # initialize start node while True: - OPEN, CLOSED = self.Astar(s_start, self.N) # OPEN, CLOSED sets in each iteration + OPEN, CLOSED = self.Astar(s_start, self.N) # U, CLOSED sets in each iteration if OPEN == "FOUND": # reach the goal node self.path.append(CLOSED) @@ -50,7 +49,7 @@ class LrtAstarN: 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 + s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # s_start -> expected node in U set self.path.append(path_k) def extract_path_in_CLOSE(self, s_start, h_value): @@ -70,7 +69,7 @@ class LrtAstarN: path.append(s_key) # generate path s = s_key # use end of this iteration as the start of next - if s_key not in h_value: # reach the expected node in OPEN set + if s_key not in h_value: # reach the expected node in U set return s_key, path def iteration(self, CLOSED): @@ -96,7 +95,7 @@ class LrtAstarN: return h_value def Astar(self, x_start, N): - OPEN = queue.QueuePrior() # OPEN set + 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 @@ -180,7 +179,7 @@ def main(): x_start = (10, 5) x_goal = (45, 25) - lrta = LrtAstarN(x_start, x_goal, 150, "euclidean") + lrta = LrtAstarN(x_start, x_goal, 200, "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 d437849..233bfbd 100644 --- a/Search-based Planning/Search_2D/RTAAstar.py +++ b/Search-based Planning/Search_2D/RTAAstar.py @@ -6,7 +6,6 @@ RTAAstar 2D (Real-time Adaptive A*) import os import sys import copy -import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -80,7 +79,7 @@ class RtaAstar: 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 OPEN set + if s_key == s_end: # reach the expected node in U set return s_start, list(reversed(path)) def iteration(self, CLOSED): @@ -106,7 +105,7 @@ class RtaAstar: return h_value def Astar(self, x_start, N): - OPEN = queue.QueuePrior() # OPEN set + 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 @@ -190,7 +189,7 @@ def main(): x_start = (10, 5) x_goal = (45, 25) - rtaa = RtaAstar(x_start, x_goal, 150, "euclidean") + rtaa = RtaAstar(x_start, x_goal, 200, "euclidean") plot = plotting.Plotting(x_start, x_goal) fig_name = "Real-time Adaptive A* (RTAA*)" 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 8283245..0561250 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 91533df..33df9b2 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 71d39d5..ad642f8 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/astar.py b/Search-based Planning/Search_2D/astar.py index a2a4ab5..5bda0f8 100644 --- a/Search-based Planning/Search_2D/astar.py +++ b/Search-based Planning/Search_2D/astar.py @@ -5,6 +5,7 @@ A_star 2D import os import sys +import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -15,165 +16,179 @@ from Search_2D import env class Astar: - def __init__(self, x_start, x_goal, e, heuristic_type): - self.xI, self.xG = x_start, x_goal + def __init__(self, start, goal, heuristic_type): + self.s_start, self.s_goal = start, goal self.heuristic_type = heuristic_type - self.Env = env.Env() # class Env + self.Env = env.Env() # class Env - self.e = e # weighted A*: e >= 1 - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles - self.g = {self.xI: 0, self.xG: float("inf")} # cost to come - self.OPEN = queue.QueuePrior() # priority queue / OPEN set - self.OPEN.put(self.xI, self.fvalue(self.xI)) - self.CLOSED = set() # closed set & visited - self.VISITED = [] - self.PARENT = {self.xI: self.xI} # relations + self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come + self.OPEN = queue.QueuePrior() # priority queue / OPEN set + self.OPEN.put(self.s_start, self.fvalue(self.s_start)) + self.CLOSED = [] # CLOSED set / VISITED order + self.PARENT = {self.s_start: self.s_start} def searching(self): """ - Searching using A_star. - - :return: path, order of visited nodes in the planning + A_star Searching. + :return: path, order of visited nodes """ - while not self.OPEN.empty(): + while self.OPEN: s = self.OPEN.get() - self.CLOSED.add(s) - self.VISITED.append(s) + self.CLOSED.append(s) - if s == self.xG: # stop condition + if s == self.s_goal: # stop condition break - for u in self.u_set: # explore neighborhoods of current node - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs and s_next not in self.CLOSED: - new_cost = self.g[s] + self.get_cost(s, u) - if s_next not in self.g: - self.g[s_next] = float("inf") - if new_cost < self.g[s_next]: # conditions for updating cost - self.g[s_next] = new_cost - self.PARENT[s_next] = s - self.OPEN.put(s_next, self.fvalue(s_next)) + for s_n in self.get_neighbor(s): + new_cost = self.g[s] + self.cost(s, s_n) + if s_n not in self.g: + self.g[s_n] = float("inf") + if new_cost < self.g[s_n]: # conditions for updating cost + self.g[s_n] = new_cost + self.PARENT[s_n] = s + self.OPEN.put(s_n, self.fvalue(s_n)) - return self.extract_path(self.PARENT), self.VISITED + return self.extract_path(self.PARENT), self.CLOSED + + def repeated_astar(self, e): + """ + repeated a*. + :param e: weight of a* + :return: path and visited order + """ - def repeated_Searching(self, xI, xG, e): path, visited = [], [] while e >= 1: - p_k, v_k = self.repeated_Astar(xI, xG, e) + p_k, v_k = self.repeated_searching(self.s_start, self.s_goal, e) path.append(p_k) visited.append(v_k) e -= 0.5 return path, visited - def repeated_Astar(self, xI, xG, e): - g = {xI: 0, xG: float("inf")} + def repeated_searching(self, s_start, s_goal, e): + """ + run a* with weight e. + :param s_start: starting state + :param s_goal: goal state + :param e: weight of a* + :return: path and visited order. + """ + + g = {s_start: 0, s_goal: float("inf")} OPEN = queue.QueuePrior() - OPEN.put(xI, g[xI] + e * self.Heuristic(xI)) - CLOSED = set() - PARENT = {xI: xI} - VISITED = [] + OPEN.put(s_start, g[s_start] + e * self.Heuristic(s_start)) + CLOSED = [] + PARENT = {s_start: s_start} while OPEN: s = OPEN.get() - CLOSED.add(s) - VISITED.append(s) + CLOSED.append(s) - if s == xG: + if s == s_goal: break - for u in self.u_set: # explore neighborhoods of current node - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs and s_next not in CLOSED: - new_cost = g[s] + self.get_cost(s, u) - if s_next not in g: - g[s_next] = float("inf") - if new_cost < g[s_next]: # conditions for updating cost - g[s_next] = new_cost - PARENT[s_next] = s - OPEN.put(s_next, g[s_next] + e * self.Heuristic(s_next)) + for s_n in self.get_neighbor(s): + if s_n not in CLOSED: + new_cost = g[s] + self.cost(s, s_n) + if s_n not in g: + g[s_n] = float("inf") + if new_cost < g[s_n]: # conditions for updating cost + g[s_n] = new_cost + PARENT[s_n] = s + OPEN.put(s_n, g[s_n] + e * self.Heuristic(s_n)) - return self.extract_path(PARENT), VISITED + return self.extract_path(PARENT), CLOSED - def fvalue(self, x, e=1): + 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 fvalue(self, x): """ f = g + h. (g: cost to come, h: heuristic function) :param x: current state :return: f """ - return self.g[x] + e * self.Heuristic(x) + return self.g[x] + self.Heuristic(x) def extract_path(self, PARENT): """ - Extract the path based on the relationship of nodes. - + Extract the path based on the PARENT set. :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) + s = PARENT[s] + path.append(s) - if x_current == self.xI: + if s == self.s_start: break - return list(path_back) + return list(path) + + def Heuristic(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 + + if heuristic_type == "manhattan": + return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) + else: + return math.hypot(goal[0] - s[0], goal[1] - s[1]) @staticmethod - def get_cost(x, u): + def cost(s_start, s_goal): """ Calculate cost for this motion - - :param x: current node - :param u: current input + :param s_start: starting node + :param s_goal: end node :return: cost for this motion :note: cost function could be more complicate! """ return 1 - def Heuristic(self, state): - """ - Calculate heuristic. - - :param state: current node (state) - :return: heuristic function value - """ - - heuristic_type = self.heuristic_type # heuristic type - goal = self.xG # goal node - - if heuristic_type == "manhattan": - return abs(goal[0] - state[0]) + abs(goal[1] - state[1]) - elif heuristic_type == "euclidean": - return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2) - else: - print("Please choose right heuristic type!") - def main(): - x_start = (5, 5) - x_goal = (45, 25) + s_start = (5, 5) + s_goal = (45, 25) - astar = Astar(x_start, x_goal, 1, "euclidean") # weight e = 1 - plot = plotting.Plotting(x_start, x_goal) # class Plotting + astar = Astar(s_start, s_goal, "euclidean") + plot = plotting.Plotting(s_start, s_goal) - fig_name = "A*" path, visited = astar.searching() - plot.animation(path, visited, fig_name) # animation generate + plot.animation(path, visited, "A*") # animation - # fig_name = "Repeated A*" - # path, visited = astar.repeated_Searching(x_start, x_goal, 2.5) - # plot.animation_ara_star(path, visited, fig_name) + # path, visited = astar.repeated_astar(2.5) # initial weight e = 2.5 + # plot.animation_ara_star(path, visited, "Repeated A*") if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/bfs.py b/Search-based Planning/Search_2D/bfs.py index 75bd06a..a8031a2 100644 --- a/Search-based Planning/Search_2D/bfs.py +++ b/Search-based Planning/Search_2D/bfs.py @@ -15,67 +15,83 @@ from Search_2D import env class BFS: - def __init__(self, x_start, x_goal): - self.xI, self.xG = x_start, x_goal + def __init__(self, s_start, s_goal): + self.s_start, self.s_goal = s_start, s_goal self.Env = env.Env() - self.plotting = plotting.Plotting(self.xI, self.xG) + self.plotting = plotting.Plotting(self.s_start, self.s_goal) self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles - self.OPEN = queue.QueueFIFO() # OPEN set: visited nodes - self.OPEN.put(self.xI) + self.OPEN = queue.QueueFIFO() # U set: visited nodes + self.OPEN.put(self.s_start) self.CLOSED = [] # CLOSED set: explored nodes - self.PARENT = {self.xI: self.xI} # relations + self.PARENT = {self.s_start: self.s_start} def searching(self): """ - :return: path, order of visited nodes in the planning + Breadth-first Searching. + :return: path, visited order """ - while not self.OPEN.empty(): + while self.OPEN: s = self.OPEN.get() - if s == self.xG: + + if s == self.s_goal: break self.CLOSED.append(s) - for u_next in self.u_set: # explore neighborhoods - s_next = tuple([s[i] + u_next[i] for i in range(2)]) - if s_next not in self.PARENT and s_next not in self.obs: # node not visited and not in obstacles - self.OPEN.put(s_next) - self.PARENT[s_next] = s + for s_n in self.get_neighbor(s): + if s_n not in self.PARENT: # node not explored + self.OPEN.put(s_n) + self.PARENT[s_n] = s return self.extract_path(), self.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): """ - Extract the path based on the relationship of nodes. + Extract the path based on the PARENT set. :return: The planning path """ - path = [self.xG] - s = self.xG + path = [self.s_goal] + s = self.s_goal while True: s = self.PARENT[s] path.append(s) - if s == self.xI: + if s == self.s_start: break return list(path) def main(): - x_start = (5, 5) # Starting node - x_goal = (45, 25) # Goal node + s_start = (5, 5) + s_goal = (45, 25) - bfs = BFS(x_start, x_goal) - plot = plotting.Plotting(x_start, x_goal) - fig_name = "Breadth-first Searching (BFS)" + bfs = BFS(s_start, s_goal) + plot = plotting.Plotting(s_start, s_goal) path, visited = bfs.searching() - plot.animation(path, visited, fig_name) # animation + plot.animation(path, visited, "Breadth-first Searching (BFS)") if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/bidirectional_a_star.py b/Search-based Planning/Search_2D/bidirectional_a_star.py index 65d256f..4f8035f 100644 --- a/Search-based Planning/Search_2D/bidirectional_a_star.py +++ b/Search-based Planning/Search_2D/bidirectional_a_star.py @@ -5,6 +5,7 @@ Bidirectional_a_star 2D import os import sys +import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") @@ -15,50 +16,51 @@ from Search_2D import env class BidirectionalAstar: - 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.Env = env.Env() # class Env - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles - self.g_fore = {self.xI: 0, self.xG: float("inf")} # cost to come: from x_start - self.g_back = {self.xG: 0, self.xI: float("inf")} # cost to come: form x_goal + self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # cost to come: from s_start + self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # cost to come: form s_goal - self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching - self.OPEN_fore.put(self.xI, self.g_fore[self.xI] + self.h(self.xI, self.xG)) - self.OPEN_back = queue.QueuePrior() # OPEN set for backward searching - self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI)) + self.OPEN_fore = queue.QueuePrior() # U set for foreward searching + self.OPEN_fore.put(self.s_start, + self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal)) + self.OPEN_back = queue.QueuePrior() # U set for backward searching + self.OPEN_back.put(self.s_goal, + self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start)) - self.CLOSED_fore = [] # CLOSED set for foreward - self.CLOSED_back = [] # CLOSED set for backward + self.CLOSED_fore = [] # CLOSED set for foreward + self.CLOSED_back = [] # CLOSED set for backward - self.PARENT_fore = {self.xI: self.xI} - self.PARENT_back = {self.xG: self.xG} + self.PARENT_fore = {self.s_start: self.s_start} + self.PARENT_back = {self.s_goal: self.s_goal} def searching(self): - s_meet = self.xI + s_meet = self.s_start - while not self.OPEN_fore.empty() and not self.OPEN_back.empty(): + while self.OPEN_fore and self.OPEN_back: # solve foreward-search s_fore = self.OPEN_fore.get() + if s_fore in self.PARENT_back: s_meet = s_fore break self.CLOSED_fore.append(s_fore) - for u in self.u_set: - s_next = tuple([s_fore[i] + u[i] for i in range(2)]) - if s_next not in self.obs: - new_cost = self.g_fore[s_fore] + self.get_cost(s_fore, u) - if s_next not in self.g_fore: - self.g_fore[s_next] = float("inf") - if new_cost < self.g_fore[s_next]: - self.g_fore[s_next] = new_cost - self.PARENT_fore[s_next] = s_fore - self.OPEN_fore.put(s_next, new_cost + self.h(s_next, self.xG)) + for s_n in self.get_neighbor(s_fore): + new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n) + if s_n not in self.g_fore: + self.g_fore[s_n] = float("inf") + if new_cost < self.g_fore[s_n]: + self.g_fore[s_n] = new_cost + self.PARENT_fore[s_n] = s_fore + self.OPEN_fore.put(s_n, new_cost + self.h(s_n, self.s_goal)) # solve backward-search s_back = self.OPEN_back.get() @@ -67,20 +69,40 @@ class BidirectionalAstar: break self.CLOSED_back.append(s_back) - for u in self.u_set: - s_next = tuple([s_back[i] + u[i] for i in range(len(s_back))]) - if s_next not in self.obs: - new_cost = self.g_back[s_back] + self.get_cost(s_back, u) - if s_next not in self.g_back: - self.g_back[s_next] = float("inf") - if new_cost < self.g_back[s_next]: - self.g_back[s_next] = new_cost - self.PARENT_back[s_next] = s_back - self.OPEN_back.put(s_next, new_cost + self.h(s_next, self.xI)) + for s_n in self.get_neighbor(s_back): + new_cost = self.g_back[s_back] + self.cost(s_back, s_n) + if s_n not in self.g_back: + self.g_back[s_n] = float("inf") + if new_cost < self.g_back[s_n]: + self.g_back[s_n] = new_cost + self.PARENT_back[s_n] = s_back + self.OPEN_back.put(s_n, new_cost + self.h(s_n, self.s_start)) return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back + 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, s_meet): + """ + extract path from start and goal + :param s_meet: meet point of bi-direction a* + :return: path + """ + # extract path for foreward part path_fore = [s_meet] s = s_meet @@ -88,7 +110,7 @@ class BidirectionalAstar: while True: s = self.PARENT_fore[s] path_fore.append(s) - if s == self.xI: + if s == self.s_start: break # extract path for backward part @@ -98,7 +120,7 @@ class BidirectionalAstar: while True: s = self.PARENT_back[s] path_back.append(s) - if s == self.xG: + if s == self.s_goal: break return list(reversed(path_fore)) + list(path_back) @@ -115,17 +137,15 @@ class BidirectionalAstar: 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(s_start, s_goal): """ Calculate cost for this motion - :param x: current node - :param u: input + :param s_start: starting node + :param s_goal: end node :return: cost for this motion :note: cost function could be more complicate! """ @@ -139,10 +159,9 @@ def main(): bastar = BidirectionalAstar(x_start, x_goal, "euclidean") plot = plotting.Plotting(x_start, x_goal) - fig_name = "Bidirectional-A*" path, visited_fore, visited_back = bastar.searching() - plot.animation_bi_astar(path, visited_fore, visited_back, fig_name) # animation + plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*") # animation if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/dfs.py b/Search-based Planning/Search_2D/dfs.py index 1b0350f..c168be1 100644 --- a/Search-based Planning/Search_2D/dfs.py +++ b/Search-based Planning/Search_2D/dfs.py @@ -1,5 +1,5 @@ """ -DFS 2D +Depth-first Searching_2D (DFS) @author: huiming zhou """ @@ -15,69 +15,83 @@ from Search_2D import env class DFS: - def __init__(self, x_start, x_goal): - self.xI, self.xG = x_start, x_goal + def __init__(self, s_start, s_goal): + self.s_start, self.s_goal = s_start, s_goal self.Env = env.Env() - self.plotting = plotting.Plotting(self.xI, self.xG) + self.plotting = plotting.Plotting(self.s_start, self.s_goal) self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles - self.OPEN = queue.QueueLIFO() # OPEN set: visited nodes - self.OPEN.put(self.xI) - self.CLOSED = [] # CLOSED set: explored nodes - self.PARENT = {self.xI: self.xI} # relations + self.OPEN = queue.QueueLIFO() # OPEN set + self.OPEN.put(self.s_start) + self.CLOSED = [] # CLOSED set / visited order + self.PARENT = {self.s_start: self.s_start} def searching(self): """ - Searching using DFS. - - :return: planning path, action in each node, visited nodes in the planning process + Depth-first Searching + :return: planning path, visited order """ - while not self.OPEN.empty(): + while self.OPEN: s = self.OPEN.get() - if s == self.xG: + + if s == self.s_goal: break self.CLOSED.append(s) - for u in self.u_set: # explore neighborhoods - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.PARENT and s_next not in self.obs: # node not visited and not in obstacles - self.OPEN.put(s_next) - self.PARENT[s_next] = s + for s_n in self.get_neighbor(s): + if s_n not in self.PARENT: # node not explored + self.OPEN.put(s_n) + self.PARENT[s_n] = s return self.extract_path(), self.CLOSED + def get_neighbor(self, s): + """ + find neighbors of state s that not in obstacles. + :param s: state + :return: neighbors + """ + + s_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: + s_list.append(s_next) + + return s_list + def extract_path(self): """ Extract the path based on the relationship of nodes. :return: The planning path """ - path = [self.xG] - s = self.xG + path = [self.s_goal] + s = self.s_goal while True: s = self.PARENT[s] path.append(s) - if s == self.xI: + if s == self.s_start: break return list(path) def main(): - x_start = (5, 5) - x_goal = (45, 25) + s_start = (5, 5) + s_goal = (45, 25) - dfs = DFS(x_start, x_goal) - plot = plotting.Plotting(x_start, x_goal) - fig_name = "Depth-first Searching (DFS)" + dfs = DFS(s_start, s_goal) + plot = plotting.Plotting(s_start, s_goal) path, visited = dfs.searching() - plot.animation(path, visited, fig_name) # animation + plot.animation(path, visited, "Depth-first Searching (DFS)") # animation if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/dijkstra.py b/Search-based Planning/Search_2D/dijkstra.py index 20f3532..e8667de 100644 --- a/Search-based Planning/Search_2D/dijkstra.py +++ b/Search-based Planning/Search_2D/dijkstra.py @@ -15,72 +15,85 @@ from Search_2D import env class Dijkstra: - def __init__(self, x_start, x_goal): - self.xI, self.xG = x_start, x_goal + def __init__(self, s_start, s_goal): + self.s_start, self.s_goal = s_start, s_goal self.Env = env.Env() - self.plotting = plotting.Plotting(self.xI, self.xG) + self.plotting = plotting.Plotting(self.s_start, self.s_goal) self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles - self.g = {self.xI: 0, self.xG: float("inf")} # cost to come - self.OPEN = queue.QueuePrior() # priority queue / OPEN set - self.OPEN.put(self.xI, 0) + self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come + self.OPEN = queue.QueuePrior() # priority queue / U set + self.OPEN.put(self.s_start, 0) self.CLOSED = [] # closed set & visited - self.PARENT = {self.xI: self.xI} # relations + self.PARENT = {self.s_start: self.s_start} def searching(self): """ - Searching using Dijkstra. + Dijkstra Searching. :return: path, order of visited nodes in the planning """ - while not self.OPEN.empty(): + while self.OPEN: s = self.OPEN.get() - if s == self.xG: # stop condition + + if s == self.s_goal: # stop condition break self.CLOSED.append(s) - for u in self.u_set: # explore neighborhoods - s_next = tuple([s[i] + u[i] for i in range(2)]) - if s_next not in self.obs: # node not visited and not in obstacles - new_cost = self.g[s] + self.get_cost(s, u) - if s_next not in self.g: - self.g[s_next] = float("inf") - if new_cost < self.g[s_next]: - self.g[s_next] = new_cost - self.OPEN.put(s_next, new_cost) - self.PARENT[s_next] = s + for s_n in self.get_neighbor(s): + new_cost = self.g[s] + self.cost(s, s_n) + if s_n not in self.g: + self.g[s_n] = float("inf") + if new_cost < self.g[s_n]: + self.g[s_n] = new_cost + self.OPEN.put(s_n, new_cost) + self.PARENT[s_n] = s return self.extract_path(), self.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): """ - Extract the path based on the relationship of nodes. - + Extract the path based on PARENT set. :return: The planning path """ - path_back = [self.xG] - x_current = self.xG + path = [self.s_goal] + s = self.s_goal while True: - x_current = self.PARENT[x_current] - path_back.append(x_current) + s = self.PARENT[s] + path.append(s) - if x_current == self.xI: + if s == self.s_start: break - return list(path_back) + return list(path) @staticmethod - def get_cost(x, u): + def cost(s_start, s_goal): """ Calculate cost for this motion - - :param x: current node - :param u: input + :param s_start: starting node + :param s_goal: end node :return: cost for this motion :note: cost function could be more complicate! """ @@ -89,15 +102,14 @@ class Dijkstra: def main(): - x_start = (5, 5) - x_goal = (45, 25) + s_start = (5, 5) + s_goal = (45, 25) - dijkstra = Dijkstra(x_start, x_goal) - plot = plotting.Plotting(x_start, x_goal) # class Plotting + dijkstra = Dijkstra(s_start, s_goal) + plot = plotting.Plotting(s_start, s_goal) - fig_name = "Dijkstra's" path, visited = dijkstra.searching() - plot.animation(path, visited, fig_name) # animation generate + plot.animation(path, visited, "Dijkstra's") # animation generate if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/env.py b/Search-based Planning/Search_2D/env.py index ca17d1f..e08da2f 100644 --- a/Search-based Planning/Search_2D/env.py +++ b/Search-based Planning/Search_2D/env.py @@ -14,7 +14,6 @@ class Env: def obs_map(self): """ Initialize obstacles' positions - :return: map of obstacles """ diff --git a/Search-based Planning/Search_2D/plotting.py b/Search-based Planning/Search_2D/plotting.py index 1121d06..d95da9c 100644 --- a/Search-based Planning/Search_2D/plotting.py +++ b/Search-based Planning/Search_2D/plotting.py @@ -88,7 +88,7 @@ class Plotting: elif count < len(visited) * 2 / 3: length = 25 else: - length = 35 + length = 30 if count % length == 0: plt.pause(0.001) diff --git a/Search-based Planning/Search_2D/queue.py b/Search-based Planning/Search_2D/queue.py index c434bee..2a9fb3b 100644 --- a/Search-based Planning/Search_2D/queue.py +++ b/Search-based Planning/Search_2D/queue.py @@ -53,14 +53,15 @@ class QueuePrior: return len(self.queue) == 0 def put(self, item, priority): + heapq.heappush(self.queue, (priority, item)) # reorder x using priority + + def update(self, item, priority): count = 0 for (p, x) in self.queue: if x == item: self.queue[count] = (priority, item) break count += 1 - if count == len(self.queue): - heapq.heappush(self.queue, (priority, item)) # reorder x using priority def get(self): return heapq.heappop(self.queue)[1] # pop out the smallest item @@ -68,10 +69,10 @@ 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 remove(self, item): + for x in self.queue: + if item == x[1]: + self.queue.remove(x) def top_key(self): return self.queue[0][0] diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 85c5f06..d8fa4d8 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -75,7 +75,7 @@ class Weighted_A_star(object): # update priority of xj self.OPEN.put(strxj, a + 1 * self.h[strxj]) else: - # add xj in to OPEN set + # add xj in to U set self.OPEN.put(strxj, a + 1 * self.h[strxj]) # For specified expanded nodes, used primarily in LRTA* if N: diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index d2dd224..e03c91b 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -48,7 +48,7 @@ class LRT_A_star2: st = self.Astar.start ind = 0 # find the lowest path down hill - 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 U, stop # strChildren = self.children(st) strChildren = [hash3D(i) for i in self.Astar.children(st)] minh, minchild = np.inf, None diff --git a/Search-based Planning/gif/LPA_star.gif b/Search-based Planning/gif/LPA_star.gif new file mode 100644 index 0000000..69a0a55 Binary files /dev/null and b/Search-based Planning/gif/LPA_star.gif differ