diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 4a44399..7e11f97 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -1,7 +1,16 @@ - + + + + + + + + + + - + @@ -148,9 +157,9 @@ - + diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc index 21dfed4..6e4ab82 100644 Binary files a/Search-based Planning/__pycache__/env.cpython-37.pyc and b/Search-based Planning/__pycache__/env.cpython-37.pyc differ diff --git a/Search-based Planning/__pycache__/motion_model.cpython-37.pyc b/Search-based Planning/__pycache__/motion_model.cpython-37.pyc deleted file mode 100644 index 9049963..0000000 Binary files a/Search-based Planning/__pycache__/motion_model.cpython-37.pyc and /dev/null differ diff --git a/Search-based Planning/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/__pycache__/plotting.cpython-37.pyc new file mode 100644 index 0000000..7e5f0a1 Binary files /dev/null and b/Search-based Planning/__pycache__/plotting.cpython-37.pyc differ diff --git a/Search-based Planning/__pycache__/tools.cpython-37.pyc b/Search-based Planning/__pycache__/tools.cpython-37.pyc deleted file mode 100644 index 1febd5e..0000000 Binary files a/Search-based Planning/__pycache__/tools.cpython-37.pyc and /dev/null differ diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index a6d3af0..fad3386 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -5,22 +5,26 @@ """ import queue -import tools +import plotting import env -import motion_model class Astar: def __init__(self, x_start, x_goal, heuristic_type): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - self.heuristic_type = heuristic_type - tools.show_map(self.xI, self.xG, self.obs, "a_star searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG, heuristic_type) + + self.fig_name = "A* Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG, heuristic_type): """ Searching using A_star. @@ -28,30 +32,53 @@ class Astar: """ q_astar = queue.QueuePrior() # priority queue - q_astar.put(self.xI, 0) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes - cost = {self.xI: 0} + q_astar.put(xI, 0) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] + cost = {xI: 0} while not q_astar.empty(): x_current = q_astar.get() - if x_current == self.xG: # stop condition + if x_current == xG: # stop condition break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) + visited.append(x_current) for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) if x_next not in self.obs: new_cost = cost[x_current] + self.get_cost(x_current, u_next) if x_next not in cost or new_cost < cost[x_next]: # conditions for updating cost cost[x_next] = new_cost - priority = new_cost + self.Heuristic(x_next, self.xG, self.heuristic_type) + priority = new_cost + self.Heuristic(x_next, xG, heuristic_type) q_astar.put(x_next, priority) # put node into queue using priority "f+h" - parent[x_next] = x_current - action[x_next] = u_next - [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, action) + parent[x_next], action[x_next] = x_current, u_next - return path_astar, actions_astar + [path, policy] = self.extract_path(xI, xG, parent, action) + + return path, policy, visited + + + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) def get_cost(self, x, u): @@ -88,6 +115,4 @@ class Astar: if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node - astar = Astar(x_Start, x_Goal, "manhattan") - [path_astar, actions_astar] = astar.searching() - tools.showPath(x_Start, x_Goal, path_astar) \ No newline at end of file + astar = Astar(x_Start, x_Goal, "manhattan") \ No newline at end of file diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index 38483fa..0638105 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -5,21 +5,25 @@ """ import queue -import tools +import plotting import env -import motion_model class BFS: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - tools.show_map(self.xI, self.xG, self.obs, "breadth-first searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + + self.fig_name = "Dijkstra's Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG): """ Searching using BFS. @@ -27,30 +31,51 @@ class BFS: """ q_bfs = queue.QueueFIFO() # first-in-first-out queue - q_bfs.put(self.xI) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes + q_bfs.put(xI) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] while not q_bfs.empty(): x_current = q_bfs.get() - if x_current == self.xG: + if x_current == xG: break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) + visited.append(x_current) for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles q_bfs.put(x_next) - parent[x_next] = x_current - action[x_next] = u_next - [path_bfs, action_bfs] = tools.extract_path(self.xI, self.xG, parent, action) # extract path + parent[x_next], action[x_next] = x_current, u_next - return path_bfs, action_bfs + [path, policy] = self.extract_path(xI, xG, parent, action) # extract path + + return path, policy, visited + + + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node bfs = BFS(x_Start, x_Goal) - [path_bf, actions_bf] = bfs.searching() - tools.showPath(x_Start, x_Goal, path_bf) diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py index 1ae5ea5..e37678a 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -5,21 +5,25 @@ """ import queue -import tools +import plotting import env -import motion_model class DFS: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - tools.show_map(self.xI, self.xG, self.obs, "depth-first searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + + self.fig_name = "Dijkstra's Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG): """ Searching using DFS. @@ -27,30 +31,51 @@ class DFS: """ q_dfs = queue.QueueLIFO() # last-in-first-out queue - q_dfs.put(self.xI) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes + q_dfs.put(xI) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] while not q_dfs.empty(): x_current = q_dfs.get() - if x_current == self.xG: + if x_current == xG: break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) + visited.append(x_current) for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles q_dfs.put(x_next) - parent[x_next] = x_current - action[x_next] = u_next - [path_dfs, action_dfs] = tools.extract_path(self.xI, self.xG, parent, action) + parent[x_next], action[x_next] = x_current, u_next - return path_dfs, action_dfs + [path, policy] = self.extract_path(xI, xG, parent, action) + + return path, policy, visited + + + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node dfs = DFS(x_Start, x_Goal) - [path_dfs, action_dfs] = dfs.searching() - tools.showPath(x_Start, x_Goal, path_dfs) diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 78f00b8..1d50afa 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -6,51 +6,55 @@ import queue import env -import tools -import motion_model +import plotting class Dijkstra: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - tools.show_map(self.xI, self.xG, self.obs, "dijkstra searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + + self.fig_name = "Dijkstra's Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG): """ Searching using Dijkstra. :return: planning path, action in each node, visited nodes in the planning process """ - q_dijk = queue.QueuePrior() # priority queue - q_dijk.put(self.xI, 0) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes - cost = {self.xI: 0} + q_dijk = queue.QueuePrior() # priority queue + q_dijk.put(xI, 0) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] # record visited nodes + cost = {xI: 0} while not q_dijk.empty(): x_current = q_dijk.get() - if x_current == self.xG: # stop condition + if x_current == xG: # stop condition break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) - for u_next in self.u_set: # explore neighborhoods of current node + visited.append(x_current) + for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in self.obs: # node not visited and not in obstacles + if x_next not in self.obs: # node not visited and not in obstacles new_cost = cost[x_current] + self.get_cost(x_current, u_next) if x_next not in cost or new_cost < cost[x_next]: cost[x_next] = new_cost priority = new_cost - q_dijk.put(x_next, priority) # put node into queue using cost to come as priority - parent[x_next] = x_current - action[x_next] = u_next - [path_dijk, action_dijk] = tools.extract_path(self.xI, self.xG, parent, action) + q_dijk.put(x_next, priority) # put node into queue using cost to come as priority + parent[x_next], action[x_next] = x_current, u_next - return path_dijk, action_dijk + [path, policy] = self.extract_path(xI, xG, parent, action) + + return path, policy, visited def get_cost(self, x, u): @@ -66,9 +70,30 @@ class Dijkstra: return 1 + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) + + if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node dijkstra = Dijkstra(x_Start, x_Goal) - [path_dijk, actions_dijk] = dijkstra.searching() - tools.showPath(x_Start, x_Goal, path_dijk) diff --git a/Search-based Planning/env.py b/Search-based Planning/env.py index a128ecf..c8e517d 100644 --- a/Search-based Planning/env.py +++ b/Search-based Planning/env.py @@ -4,34 +4,44 @@ @author: huiming zhou """ -x_range, y_range = 51, 31 # size of background +class Env(): + def __init__(self): + self.x_range = 51 # size of background + self.y_range = 31 + self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] + self.obs = self.obs_map() -def obs_map(): - """ - Initialize obstacles' positions - :return: map of obstacles - """ + def obs_map(self): + """ + Initialize obstacles' positions - obs = [] - for i in range(x_range): - obs.append((i, 0)) - for i in range(x_range): - obs.append((i, y_range - 1)) + :return: map of obstacles + """ - for i in range(y_range): - obs.append((0, i)) - for i in range(y_range): - obs.append((x_range - 1, i)) + x = self.x_range + y = self.y_range + obs = [] - for i in range(10, 21): - obs.append((i, 15)) - for i in range(15): - obs.append((20, i)) + for i in range(x): + obs.append((i, 0)) + for i in range(x): + obs.append((i, y - 1)) - for i in range(15, 30): - obs.append((30, i)) - for i in range(16): - obs.append((40, i)) + for i in range(y): + obs.append((0, i)) + for i in range(y): + obs.append((x - 1, i)) + + for i in range(10, 21): + obs.append((i, 15)) + for i in range(15): + obs.append((20, i)) + + for i in range(15, 30): + obs.append((30, i)) + for i in range(16): + obs.append((40, i)) + + return obs - return obs diff --git a/Search-based Planning/motion_model.py b/Search-based Planning/motion_model.py deleted file mode 100644 index 144bbcc..0000000 --- a/Search-based Planning/motion_model.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - -motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets \ No newline at end of file diff --git a/Search-based Planning/plotting.py b/Search-based Planning/plotting.py new file mode 100644 index 0000000..4a939b0 --- /dev/null +++ b/Search-based Planning/plotting.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import matplotlib.pyplot as plt + + +def animation(xI, xG, obs, path, visited, name): + """ + generate animation for exploring process of algorithm + + :param xI: starting state + :param xG: goal state + :param obs: obstacle map + :param path: optimal path + :param visited: visited nodes + :param name: name of this figure + :return: animation + """ + + visited.remove(xI) + path.remove(xI) + path.remove(xG) + + # plot gridworld + obs_x = [obs[i][0] for i in range(len(obs))] + obs_y = [obs[i][1] for i in range(len(obs))] + plt.plot(xI[0], xI[1], "bs") + plt.plot(xG[0], xG[1], "gs") + plt.plot(obs_x, obs_y, "sk") + plt.title(name) + plt.axis("equal") + + # animation for the exploring order of visited nodes + count = 0 + for x in visited: + count += 1 + plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o') + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + if count % 20 == 0: plt.pause(0.01) + + # plot optimal path + path_x = [path[i][0] for i in range(len(path))] + path_y = [path[i][1] for i in range(len(path))] + plt.plot(path_x, path_y, linewidth='3', color='r', marker='o') + + # show animation + plt.pause(0.01) + plt.show() diff --git a/Search-based Planning/tools.py b/Search-based Planning/tools.py deleted file mode 100644 index 7d4aede..0000000 --- a/Search-based Planning/tools.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - -import matplotlib.pyplot as plt - - -def extract_path(xI, xG, parent, actions): - """ - Extract the path based on the relationship of nodes. - - :param xI: Starting node - :param xG: Goal node - :param parent: Relationship between nodes - :param actions: Action needed for transfer between two nodes - :return: The planning path - """ - - path_back = [xG] - acts_back = [actions[xG]] - x_current = xG - while True: - x_current = parent[x_current] - path_back.append(x_current) - acts_back.append(actions[x_current]) - if x_current == xI: break - - return list(reversed(path_back)), list(reversed(acts_back)) - - -def show_map(xI, xG, obs_map, name): - obs_x = [obs_map[i][0] for i in range(len(obs_map))] - obs_y = [obs_map[i][1] for i in range(len(obs_map))] - - plt.plot(xI[0], xI[1], "bs") - plt.plot(xG[0], xG[1], "gs") - plt.plot(obs_x, obs_y, "sk") - plt.title(name, fontdict=None) - plt.axis("equal") - - -def showPath(xI, xG, path): - """ - Plot the path. - - :param xI: Starting node - :param xG: Goal node - :param path: Planning path - :return: A plot - """ - - path.remove(xI) - path.remove(xG) - path_x = [path[i][0] for i in range(len(path))] - path_y = [path[i][1] for i in range(len(path))] - plt.plot(path_x, path_y, linewidth='5', color='r', linestyle='-') - plt.pause(0.001) - plt.show() - - -def plot_dots(x, length): - plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o') - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - if length % 15 == 0: plt.pause(0.001) -