diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index d2c67cf..978aad7 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -2,14 +2,18 @@ - + + + + - - + + + - + + - @@ -182,7 +186,9 @@ - + + diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc index a24cb11..e8ca2a5 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 new file mode 100644 index 0000000..9049963 Binary files /dev/null and b/Search-based Planning/__pycache__/motion_model.cpython-37.pyc differ diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index 7adf002..8da19b1 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -5,18 +5,20 @@ """ import queue -import env import tools import env +import motion_model class Astar: def __init__(self, x_start, x_goal, x_range, y_range, heuristic_type): - self.u_set = env.motions # feasible input set + self.u_set = motion_model.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.obs = env.obs_map() # position of obstacles self.heuristic_type = heuristic_type + env.show_map(self.xI, self.xG, self.obs, "a_star searching") + def searching(self): """ Searching using A_star. @@ -27,7 +29,7 @@ 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 + action = {self.xI: (0, 0)} # record actions of nodes cost = {self.xI: 0} while not q_astar.empty(): @@ -43,7 +45,7 @@ class Astar: 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" + 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) diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index d10011e..f756286 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -7,6 +7,7 @@ import queue import tools import env +import motion_model class BFS: """ @@ -14,10 +15,12 @@ class BFS: """ def __init__(self, x_start, x_goal, x_range, y_range): - self.u_set = env.motions # feasible input set + self.u_set = motion_model.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 + self.obs = env.obs_map() # position of obstacles + + env.show_map(self.xI, self.xG, self.obs, "breadth-first searching") def searching(self): """ diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py index f32bac7..3bd4a2d 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -7,6 +7,7 @@ import queue import tools import env +import motion_model class DFS: """ @@ -14,10 +15,12 @@ class DFS: """ def __init__(self, x_start, x_goal, x_range, y_range): - self.u_set = env.motions # feasible input set + self.u_set = motion_model.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 + self.obs = env.obs_map() # position of obstacles + + env.show_map(self.xI, self.xG, self.obs, "depth-first searching") def searching(self): """ diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 29419c7..95e0759 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -7,13 +7,16 @@ import queue import env import tools +import motion_model class Dijkstra: def __init__(self, x_start, x_goal, x_range, y_range): - self.u_set = env.motions # feasible input set + self.u_set = motion_model.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 + self.obs = env.obs_map() # position of obstacles + + env.show_map(self.xI, self.xG, self.obs, "dijkstra searching") def searching(self): """ @@ -25,7 +28,7 @@ class Dijkstra: 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 + action = {self.xI: (0, 0)} # record actions of nodes cost = {self.xI: 0} while not q_dijk.empty(): @@ -36,7 +39,7 @@ class Dijkstra: 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 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 diff --git a/Search-based Planning/env.py b/Search-based Planning/env.py index 1c071c0..f777d06 100644 --- a/Search-based Planning/env.py +++ b/Search-based Planning/env.py @@ -7,15 +7,11 @@ 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): +def obs_map(): """ Initialize obstacles' positions - :param xI: starting node - :param xG: goal node - :param name: title of figure :return: map of obstacles """ @@ -40,15 +36,16 @@ def obs_map(xI, xG, name): for i in range(16): obs_map.append((40, i)) + return obs_map + + +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.title(name, fontdict=None) plt.grid(True) plt.axis("equal") - - return obs_map - diff --git a/Search-based Planning/motion_model.py b/Search-based Planning/motion_model.py new file mode 100644 index 0000000..144bbcc --- /dev/null +++ b/Search-based Planning/motion_model.py @@ -0,0 +1,7 @@ +#!/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/Stochastic Shortest Path/env.py b/Stochastic Shortest Path/env.py new file mode 100644 index 0000000..f777d06 --- /dev/null +++ b/Stochastic Shortest Path/env.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import matplotlib.pyplot as plt + +x_range, y_range = 51, 31 # size of background + +def obs_map(): + """ + Initialize obstacles' positions + + :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)) + + return obs_map + + +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.grid(True) + plt.axis("equal") diff --git a/Stochastic Shortest Path/environment.py b/Stochastic Shortest Path/environment.py deleted file mode 100644 index 3a6533c..0000000 --- a/Stochastic Shortest Path/environment.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - -import numpy as np - -col, row = 50, 30 # size of background -motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets - - -def obstacles(): - """ - Design the obstacles' positions. - :return: the map of obstacles. - """ - - background = [[[1., 1., 1.] - for x in range(col)] for y in range(row)] - for j in range(col): - background[0][j] = [0., 0., 0.] - background[row - 1][j] = [0., 0., 0.] - for i in range(row): - background[i][0] = [0., 0., 0.] - background[i][col - 1] = [0., 0., 0.] - for i in range(10, 20): - background[15][i] = [0., 0., 0.] - for i in range(15): - background[row - 1 - i][30] = [0., 0., 0.] - background[i + 1][20] = [0., 0., 0.] - background[i + 1][40] = [0., 0., 0.] - return background - - -def map_obs(): - """ - Using a matrix to represent the position of obstacles, - which is used for obstacle detection. - :return: a matrix, in which '1' represents obstacle. - """ - - obs_map = np.zeros((col, row)) - pos_map = obstacles() - for i in range(col): - for j in range(row): - if pos_map[j][i] == [0., 0., 0.]: - obs_map[i][j] = 1 - return obs_map \ No newline at end of file diff --git a/Stochastic Shortest Path/motion model.py b/Stochastic Shortest Path/motion model.py new file mode 100644 index 0000000..144bbcc --- /dev/null +++ b/Stochastic Shortest Path/motion model.py @@ -0,0 +1,7 @@ +#!/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/Stochastic Shortest Path/tools.py b/Stochastic Shortest Path/tools.py index ff7ae20..2de4a72 100644 --- a/Stochastic Shortest Path/tools.py +++ b/Stochastic Shortest Path/tools.py @@ -5,30 +5,10 @@ """ 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 @@ -47,28 +27,27 @@ 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/value_iteration.py b/Stochastic Shortest Path/value_iteration.py new file mode 100644 index 0000000..d9a2968 --- /dev/null +++ b/Stochastic Shortest Path/value_iteration.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +