From a01e6acc1da503ea12196ae78c835b6a12ea03d5 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Thu, 18 Jun 2020 15:52:17 -0700 Subject: [PATCH] update --- Search-based Planning/.idea/workspace.xml | 18 ++++--- .../__pycache__/env.cpython-37.pyc | Bin 1505 -> 1463 bytes .../__pycache__/motion_model.cpython-37.pyc | Bin 0 -> 285 bytes Search-based Planning/a_star.py | 12 +++-- Search-based Planning/bfs.py | 7 ++- Search-based Planning/dfs.py | 7 ++- Search-based Planning/dijkstra.py | 11 ++-- Search-based Planning/env.py | 15 +++--- Search-based Planning/motion_model.py | 7 +++ Stochastic Shortest Path/env.py | 51 ++++++++++++++++++ Stochastic Shortest Path/environment.py | 49 ----------------- Stochastic Shortest Path/motion model.py | 7 +++ Stochastic Shortest Path/tools.py | 51 ++++++------------ Stochastic Shortest Path/value_iteration.py | 7 +++ 14 files changed, 129 insertions(+), 113 deletions(-) create mode 100644 Search-based Planning/__pycache__/motion_model.cpython-37.pyc create mode 100644 Search-based Planning/motion_model.py create mode 100644 Stochastic Shortest Path/env.py delete mode 100644 Stochastic Shortest Path/environment.py create mode 100644 Stochastic Shortest Path/motion model.py create mode 100644 Stochastic Shortest Path/value_iteration.py 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 a24cb11a39380819408ff2de1d87d438a1963166..e8ca2a55d6c4de88344ac68fdecaaef199ff4822 100644 GIT binary patch literal 1463 zcmbVM&2Jk;6n``O*=-WiP(o5w?Fv-_7l@F!MNw4IO0<^>5l|(|t+m-%+uN>pH#1{! zEa#L;aN&T&fn$5*#*x2euAD3+{(zi#Z+7iAf)gXXH#6^h-}}vevA!MwuEF1bJ^S4S z_>(Tn#qn?tNqvG$Koih>8%wK|*kf2&Mj%+cN9-M9 z*NNRCwn^+GWF6OdGAW0d38ww#;3svRmBIw4jiAF#fVK27nPK^FAh*!(;^_WAxUDdrh(WAI7lCmU>B*n0jxgL#G?`Q2W9 zDygiiNa_wU2wb+owve{@a^JM~Exz2XM~Voyi(uvkl^$qoLk79 zvoqIf5K#Zpc$&X*<{;c%9E|XGLHMunU05lzQf8&WO6tx%5jMBy?%WX|uKdKUhbJNe zH2V7DDxCz4dhKya!=XiQgNlC_N$nsTV$!FZXISc-VQScgpZST4PRvf#wjSrf_db)? z&>Y3oGAou8!Q-e z)447(;|^smjGIhz)$t9VK0^A^Hp94OGS0dlL3Y|~r%j=4v#3udTsGt`6-P}3c7xsE z+iV*Fx4|MN@1U#`nxNkoRod^HYvV*y2}OR4ShdCY%`R`Esaz*U@2r^#xi^EM_wr^$ XCOjBdVp3#>1XUFxbNLz%d3gIDLXkxl literal 1505 zcmbVM&2A$_5bo~z&5(E#?_!cgT1H&J0-}}RWJS>mRxG)!5CN?;@?v$|UJx_Um|+6oAa-~agQ z>rWj*{=|>fut9kMqV59-5)nZK%V^FbixOe=S!73!u!SRB;U#wD3SVqICy^)EG3f@g zH-ZOzqQ|A|h2u$@r$rLZ#^oefKsxfc%N8HPym$)$bPAJ10R3+mjcz81OuB5T14}Zr z0C2dC;J5k(mM7$UGQFpXrWf=fdD=d=F3Afvw*(!rmz0vz))O*)d+qP6{Mr(1mozc^ z5Q90a>p29STaEvVCdbm>nu6?+AK9r56`*E=U!kdMwtv5-X_wX>P#6Fkxdkh)jz8)3 zzf!SO{m+`W6T#c*KaABgT~_^y>v33RyeP09JWEQM>T$06$1#_~@dqbd#UlK?3Fybg zxBJ!ERdA4{N)OAtI(!c~B4L-z-fVW>Kgh}<&(t9(tGNGlI4puW20Ypp>R38BG?vWR zBz3KQrC{T{K!sZDk7UtQ`L2AHa!A)RNL4ehFs)e4W`TKY{|XhYp5dg zO{Dq}010f`qHT~4Tg}_%yUkYfU3!;wXa{0iG@t=9XYcy8HyDU=I2c4bIoHTAOHbfx wP>bBsWZfv=SIDiM4D z8=XY`VP?KzX83157z70Mb2)GRu>LpAOFqI3!wxY4i3xZ>p#y&6#~yUoBh($ literal 0 HcmV?d00001 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 +""" + +