diff --git a/Search-based Planning/.idea/vcs.xml b/Search-based Planning/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/Search-based Planning/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index e522e54..d2c67cf 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -1,7 +1,16 @@ - + + + + + + + + + + + + - + + + - + @@ -34,7 +48,7 @@ - + - - + + @@ -163,4 +177,15 @@ + + + \ No newline at end of file diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc new file mode 100644 index 0000000..a24cb11 Binary files /dev/null and b/Search-based Planning/__pycache__/env.cpython-37.pyc differ diff --git a/Search-based Planning/__pycache__/environment.cpython-37.pyc b/Search-based Planning/__pycache__/environment.cpython-37.pyc deleted file mode 100644 index fbb784b..0000000 Binary files a/Search-based Planning/__pycache__/environment.cpython-37.pyc and /dev/null differ diff --git a/Search-based Planning/__pycache__/tools.cpython-37.pyc b/Search-based Planning/__pycache__/tools.cpython-37.pyc index 9ac48b1..0cbfdb0 100644 Binary files a/Search-based Planning/__pycache__/tools.cpython-37.pyc and b/Search-based Planning/__pycache__/tools.cpython-37.pyc differ diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index 6fa6329..7adf002 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -5,17 +5,16 @@ """ import queue -import environment +import env import tools +import env class Astar: - def __init__(self, Start_State, Goal_State, n, m, heuristic_type): - self.xI = Start_State - self.xG = Goal_State - self.u_set = environment.motions # feasible input set - self.obs_map = environment.map_obs() # position of obstacles - self.n = n - self.m = m + def __init__(self, x_start, x_goal, x_range, y_range, heuristic_type): + self.u_set = env.motions # feasible input set + self.xI, self.xG = x_start, x_goal + self.x_range, self.y_range = x_range, y_range + self.obs = env.obs_map(self.xI, self.xG, "a_star searching") # position of obstacles self.heuristic_type = heuristic_type def searching(self): @@ -28,29 +27,27 @@ class Astar: q_astar = queue.QueuePrior() # priority queue q_astar.put(self.xI, 0) parent = {self.xI: self.xI} # record parents of nodes - actions = {self.xI: (0, 0)} # record actions of nodes + action = {self.xI: (0, 0)} # record actions of nodes cost = {self.xI: 0} - visited = [] while not q_astar.empty(): x_current = q_astar.get() - visited.append(x_current) # record visited nodes if x_current == self.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 x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - # if neighbor node is not in obstacles -> ... - if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \ - and not tools.obs_detect(x_current, u_next, self.obs_map): - new_cost = cost[x_current] + int(self.get_cost(x_current, u_next)) + 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) q_astar.put(x_next, priority) # put node into queue using priority "f+h" parent[x_next] = x_current - actions[x_next] = u_next - [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, actions) - return path_astar, actions_astar, visited + action[x_next] = u_next + [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, action) + return path_astar, actions_astar def get_cost(self, x, u): """ @@ -83,8 +80,8 @@ class Astar: if __name__ == '__main__': - x_Start = (15, 10) # Starting node - x_Goal = (48, 15) # Goal node - astar = Astar(x_Start, x_Goal, environment.col, environment.row, "manhattan") - [path_astar, actions_astar, visited_astar] = astar.searching() - tools.showPath(x_Start, x_Goal, path_astar, visited_astar, 'Astar_searching') # Plot path and visited nodes \ No newline at end of file + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node + astar = Astar(x_Start, x_Goal, env.x_range, env.y_range, "manhattan") + [path_astar, actions_astar] = astar.searching() + tools.showPath(x_Start, x_Goal, path_astar) # Plot path and visited nodes \ No newline at end of file diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index 4e82c5d..d10011e 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -5,21 +5,19 @@ """ import queue -import environment import tools +import env class BFS: """ BFS -> Breadth-first Searching """ - def __init__(self, Start_State, Goal_State, n, m): - self.xI = Start_State - self.xG = Goal_State - self.u_set = environment.motions # feasible input set - self.obs_map = environment.map_obs() # position of obstacles - self.n = n - self.m = m + def __init__(self, x_start, x_goal, x_range, y_range): + self.u_set = env.motions # feasible input set + self.xI, self.xG = x_start, x_goal + self.x_range, self.y_range = x_range, y_range + self.obs = env.obs_map(self.xI, self.xG, "breadth-first searching") # position of obstacles def searching(self): """ @@ -31,29 +29,27 @@ 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 - actions = {self.xI: (0, 0)} # record actions of nodes - visited = [] + action = {self.xI: (0, 0)} # record actions of nodes + while not q_bfs.empty(): x_current = q_bfs.get() - visited.append(x_current) # record visited nodes - if x_current == self.xG: # stop condition + if x_current == self.xG: 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 - x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) # neighbor node - # if neighbor node is not in obstacles and has not been visited -> ... - if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \ - and x_next not in parent \ - and not tools.obs_detect(x_current, u_next, self.obs_map): + 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 - actions[x_next] = u_next - [path_bfs, actions_bfs] = tools.extract_path(self.xI, self.xG, parent, actions) # extract path - return path_bfs, actions_bfs, visited + action[x_next] = u_next + [path_bfs, action_bfs] = tools.extract_path(self.xI, self.xG, parent, action) # extract path + return path_bfs, action_bfs if __name__ == '__main__': - x_Start = (15, 10) # Starting node - x_Goal = (48, 15) # Goal node - bfs = BFS(x_Start, x_Goal, environment.col, environment.row) - [path_bf, actions_bf, visited_bfs] = bfs.searching() - tools.showPath(x_Start, x_Goal, path_bf, visited_bfs, 'breadth_first_searching') # Plot path and visited nodes \ No newline at end of file + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node + bfs = BFS(x_Start, x_Goal, env.x_range, env.y_range) + [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 04e4ab5..f32bac7 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -5,21 +5,19 @@ """ import queue -import environment import tools +import env class DFS: """ DFS -> Depth-first Searching """ - def __init__(self, Start_State, Goal_State, n, m): - self.xI = Start_State - self.xG = Goal_State - self.u_set = environment.motions # feasible input set - self.obs_map = environment.map_obs() # position of obstacles - self.n = n - self.m = m + def __init__(self, x_start, x_goal, x_range, y_range): + self.u_set = env.motions # feasible input set + self.xI, self.xG = x_start, x_goal + self.x_range, self.y_range = x_range, y_range + self.obs = env.obs_map(self.xI, self.xG, "depth-first searching") # position of obstacles def searching(self): """ @@ -31,30 +29,27 @@ 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 - actions = {self.xI: (0, 0)} # record actions of nodes - visited = [] + action = {self.xI: (0, 0)} # record actions of nodes while not q_dfs.empty(): x_current = q_dfs.get() - visited.append(x_current) # record visited nodes - if x_current == self.xG: # stop condition + if x_current == self.xG: break - 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))]) # neighbor node - # if neighbor node is not in obstacles and has not been visited -> ... - if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \ - and x_next not in parent \ - and not tools.obs_detect(x_current, u_next, self.obs_map): + if x_current != self.xI: + tools.plot_dots(x_current, len(parent)) + 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 - actions[x_next] = u_next - [path_dfs, actions_dfs] = tools.extract_path(self.xI, self.xG, parent, actions) - return path_dfs, actions_dfs, visited + action[x_next] = u_next + [path_dfs, action_dfs] = tools.extract_path(self.xI, self.xG, parent, action) + return path_dfs, action_dfs if __name__ == '__main__': - x_Start = (15, 10) # Starting node - x_Goal = (48, 15) # Goal node - dfs = DFS(x_Start, x_Goal, environment.col, environment.row) - [path_dfs, actions_dfs, visited_dfs] = dfs.searching() - tools.showPath(x_Start, x_Goal, path_dfs, visited_dfs, 'depth_first_searching') # Plot path and visited nodes \ No newline at end of file + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node + dfs = DFS(x_Start, x_Goal, env.x_range, env.y_range) + [path_dfs, action_dfs] = dfs.searching() + tools.showPath(x_Start, x_Goal, path_dfs) \ No newline at end of file diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 06dea0f..29419c7 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -5,17 +5,15 @@ """ import queue -import environment +import env import tools class Dijkstra: - def __init__(self, Start_State, Goal_State, n, m): - self.xI = Start_State - self.xG = Goal_State - self.u_set = environment.motions # feasible input set - self.obs_map = environment.map_obs() # position of obstacles - self.n = n - self.m = m + def __init__(self, x_start, x_goal, x_range, y_range): + self.u_set = env.motions # feasible input set + self.xI, self.xG = x_start, x_goal + self.x_range, self.y_range = x_range, y_range + self.obs = env.obs_map(self.xI, self.xG, "dijkstra searching") # position of obstacles def searching(self): """ @@ -27,29 +25,27 @@ class Dijkstra: q_dijk = queue.QueuePrior() # priority queue q_dijk.put(self.xI, 0) parent = {self.xI: self.xI} # record parents of nodes - actions = {self.xI: (0, 0)} # record actions of nodes + action = {self.xI: (0, 0)} # record actions of nodes cost = {self.xI: 0} - visited = [] while not q_dijk.empty(): x_current = q_dijk.get() - visited.append(x_current) # record visited nodes if x_current == self.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 x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - # if neighbor node is not in obstacles -> ... - if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \ - and not tools.obs_detect(x_current, u_next, self.obs_map): - new_cost = cost[x_current] + int(self.get_cost(x_current, u_next)) + 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 - actions[x_next] = u_next - [path_dijk, actions_dijk] = tools.extract_path(self.xI, self.xG, parent, actions) - return path_dijk, actions_dijk, visited + action[x_next] = u_next + [path_dijk, action_dijk] = tools.extract_path(self.xI, self.xG, parent, action) + return path_dijk, action_dijk def get_cost(self, x, u): """ @@ -65,8 +61,8 @@ class Dijkstra: if __name__ == '__main__': - x_Start = (15, 10) # Starting node - x_Goal = (48, 15) # Goal node - dijkstra = Dijkstra(x_Start, x_Goal, environment.col, environment.row) - [path_dijk, actions_dijk, visited_dijk] = dijkstra.searching() - tools.showPath(x_Start, x_Goal, path_dijk, visited_dijk, 'dijkstra_searching') \ No newline at end of file + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node + dijkstra = Dijkstra(x_Start, x_Goal, env.x_range, env.y_range) + [path_dijk, actions_dijk] = dijkstra.searching() + tools.showPath(x_Start, x_Goal, path_dijk) \ No newline at end of file diff --git a/Search-based Planning/env.py b/Search-based Planning/env.py new file mode 100644 index 0000000..1c071c0 --- /dev/null +++ b/Search-based Planning/env.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import matplotlib.pyplot as plt + +x_range, y_range = 51, 31 # size of background +motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets + +def obs_map(xI, xG, name): + """ + Initialize obstacles' positions + + :param xI: starting node + :param xG: goal node + :param name: title of figure + :return: map of obstacles + """ + + obs_map = [] + for i in range(x_range): + obs_map.append((i, 0)) + for i in range(x_range): + obs_map.append((i, y_range-1)) + + for i in range(y_range): + obs_map.append((0, i)) + for i in range(y_range): + obs_map.append((x_range-1, i)) + + for i in range(10, 21): + obs_map.append((i, 15)) + for i in range(15): + obs_map.append((20, i)) + + for i in range(15, 30): + obs_map.append((30, i)) + for i in range(16): + obs_map.append((40, i)) + + 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.grid(True) + plt.axis("equal") + + return obs_map + diff --git a/Search-based Planning/tools.py b/Search-based Planning/tools.py index ff7ae20..4097243 100644 --- a/Search-based Planning/tools.py +++ b/Search-based Planning/tools.py @@ -5,25 +5,6 @@ """ import matplotlib.pyplot as plt -import environment - - -def obs_detect(x, u, obs_map): - """ - Detect if the next state is in obstacles using this input. - - :param x: current state - :param u: input - :param obs_map: map of obstacles - :return: in obstacles: True / not in obstacles: False - """ - - x_next = [x[0] + u[0], x[1] + u[1]] # next state using input 'u' - if u not in environment.motions or \ - obs_map[x_next[0]][x_next[1]] == 1: # if 'u' is feasible and next state is not in obstacles - return True - return False - def extract_path(xI, xG, parent, actions): """ @@ -47,28 +28,28 @@ def extract_path(xI, xG, parent, actions): return list(reversed(path_back)), list(reversed(acts_back)) -def showPath(xI, xG, path, visited, name): +def showPath(xI, xG, path): """ Plot the path. :param xI: Starting node :param xG: Goal node :param path: Planning path - :param visited: Visited nodes - :param name: Name of this figure :return: A plot """ - - background = environment.obstacles() - fig, ax = plt.subplots() - for k in range(len(visited)): - background[visited[k][1]][visited[k][0]] = [.5, .5, .5] # visited nodes: gray color - for k in range(len(path)): - background[path[k][1]][path[k][0]] = [1., 0., 0.] # path: red color - background[xI[1]][xI[0]] = [0., 0., 1.] # starting node: blue color - background[xG[1]][xG[0]] = [0., 1., .5] # goal node: green color - ax.imshow(background) - ax.invert_yaxis() # put origin of coordinate to left-bottom - plt.title(name, fontdict=None) + 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) + diff --git a/Stochastic Shortest Path/.idea/.gitignore b/Stochastic Shortest Path/.idea/.gitignore new file mode 100644 index 0000000..0e40fe8 --- /dev/null +++ b/Stochastic Shortest Path/.idea/.gitignore @@ -0,0 +1,3 @@ + +# Default ignored files +/workspace.xml \ No newline at end of file diff --git a/Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml b/Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml new file mode 100644 index 0000000..7c9d48f --- /dev/null +++ b/Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml b/Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/Stochastic Shortest Path/.idea/misc.xml b/Stochastic Shortest Path/.idea/misc.xml new file mode 100644 index 0000000..a2e120d --- /dev/null +++ b/Stochastic Shortest Path/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Stochastic Shortest Path/.idea/modules.xml b/Stochastic Shortest Path/.idea/modules.xml new file mode 100644 index 0000000..f86c9be --- /dev/null +++ b/Stochastic Shortest Path/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Stochastic Shortest Path/.idea/vcs.xml b/Stochastic Shortest Path/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/Stochastic Shortest Path/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Search-based Planning/environment.py b/Stochastic Shortest Path/environment.py similarity index 100% rename from Search-based Planning/environment.py rename to Stochastic Shortest Path/environment.py diff --git a/Stochastic Shortest Path/tools.py b/Stochastic Shortest Path/tools.py new file mode 100644 index 0000000..ff7ae20 --- /dev/null +++ b/Stochastic Shortest Path/tools.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import matplotlib.pyplot as plt +import environment + + +def obs_detect(x, u, obs_map): + """ + Detect if the next state is in obstacles using this input. + + :param x: current state + :param u: input + :param obs_map: map of obstacles + :return: in obstacles: True / not in obstacles: False + """ + + x_next = [x[0] + u[0], x[1] + u[1]] # next state using input 'u' + if u not in environment.motions or \ + obs_map[x_next[0]][x_next[1]] == 1: # if 'u' is feasible and next state is not in obstacles + return True + return False + + +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 showPath(xI, xG, path, visited, name): + """ + Plot the path. + + :param xI: Starting node + :param xG: Goal node + :param path: Planning path + :param visited: Visited nodes + :param name: Name of this figure + :return: A plot + """ + + background = environment.obstacles() + fig, ax = plt.subplots() + for k in range(len(visited)): + background[visited[k][1]][visited[k][0]] = [.5, .5, .5] # visited nodes: gray color + for k in range(len(path)): + background[path[k][1]][path[k][0]] = [1., 0., 0.] # path: red color + background[xI[1]][xI[0]] = [0., 0., 1.] # starting node: blue color + background[xG[1]][xG[0]] = [0., 1., .5] # goal node: green color + ax.imshow(background) + ax.invert_yaxis() # put origin of coordinate to left-bottom + plt.title(name, fontdict=None) + plt.show() +