From 97db5259e43762ecec2599f94bee45e2639c001d Mon Sep 17 00:00:00 2001 From: zhm-real Date: Thu, 18 Jun 2020 14:57:03 -0700 Subject: [PATCH] update searchi-based --- Search-based Planning/.idea/vcs.xml | 6 ++ Search-based Planning/.idea/workspace.xml | 37 +++++++-- .../__pycache__/env.cpython-37.pyc | Bin 0 -> 1505 bytes .../__pycache__/environment.cpython-37.pyc | Bin 1583 -> 0 bytes .../__pycache__/tools.cpython-37.pyc | Bin 1963 -> 1983 bytes Search-based Planning/a_star.py | 43 +++++----- Search-based Planning/bfs.py | 46 +++++------ Search-based Planning/dfs.py | 47 +++++------ Search-based Planning/dijkstra.py | 42 +++++----- Search-based Planning/env.py | 54 +++++++++++++ Search-based Planning/tools.py | 49 ++++-------- Stochastic Shortest Path/.idea/.gitignore | 3 + .../.idea/Stochastic Shortest Path.iml | 12 +++ .../inspectionProfiles/profiles_settings.xml | 6 ++ Stochastic Shortest Path/.idea/misc.xml | 4 + Stochastic Shortest Path/.idea/modules.xml | 8 ++ Stochastic Shortest Path/.idea/vcs.xml | 6 ++ .../environment.py | 0 Stochastic Shortest Path/tools.py | 74 ++++++++++++++++++ 19 files changed, 300 insertions(+), 137 deletions(-) create mode 100644 Search-based Planning/.idea/vcs.xml create mode 100644 Search-based Planning/__pycache__/env.cpython-37.pyc delete mode 100644 Search-based Planning/__pycache__/environment.cpython-37.pyc create mode 100644 Search-based Planning/env.py create mode 100644 Stochastic Shortest Path/.idea/.gitignore create mode 100644 Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml create mode 100644 Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 Stochastic Shortest Path/.idea/misc.xml create mode 100644 Stochastic Shortest Path/.idea/modules.xml create mode 100644 Stochastic Shortest Path/.idea/vcs.xml rename {Search-based Planning => Stochastic Shortest Path}/environment.py (100%) create mode 100644 Stochastic Shortest Path/tools.py 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 0000000000000000000000000000000000000000..a24cb11a39380819408ff2de1d87d438a1963166 GIT binary patch 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>bBPj4GV6rY*3y^fO@)Ru%sQ0<{2wPd1Fai}6IRBeP15(p`3BUz@_W@ocrV(&OJ zW9nG;Dg6TQ1psm43-Dp~%E{uwxfl4o@!v)jvzj+=-@f0xH}igPpKfng7{-^s{QYuN zWb7|`S&RadCz$4Q2+5jEaw)pJDWumDO|Myy1zD6O=_iF|QI_S#Yt}5uirmENOL4+# zTch2|lW<@=N*@NDLE1~RBp7wnpfbgl?Aw}{KEgV^2LYX9(IlY%i!s;TqA8o2nCZxr zMVM}QkYsp*YtMGo;%GNF02D9E-a^uOrD~1)&@4@&%ChEu+i17KpmDj+JHXFW~hvyk5Yo>@xd$ z-Dj4^SWqr<pRxO89POvH!}Jb` z_h>_6{PY1t&C{D$a!;#QddnH|%2^m)Bw7tJne!iVp590&-&ls<2Evt(l}dbzSNScz zH<$189cX)ydt6hUmhDD;NOf@S_7aL^zj79#F{k%AewKA?S`W9}=U8Jj6t&saDLmPe8^pQC!`U7&ID&MS_kFRaG-^Vvo zgT`$dJy>ex+;6v~irVeGm<@XUE4>TzDtPYqlxz1{7KB-g<2sAjnwJ|r WB?sO3h#K4w^9rIY%6!W!@BRl;c3`^z diff --git a/Search-based Planning/__pycache__/tools.cpython-37.pyc b/Search-based Planning/__pycache__/tools.cpython-37.pyc index 9ac48b19c013e34c3622caa8aa37ee1f2bfc18e4..0cbfdb0e68d7b9494b26dbc8d328010501206290 100644 GIT binary patch literal 1983 zcmb_c&u=3&6t?GACTY5Cm!(j)h>Ro6UX%I%4lhT3Ha4BIf^G->_V?GO@_gRUB&&JwK~+s?a@wWGUhd{}g@EPf^f(x=STqLUq!K~-bgX5Z z=t%c?G>mmGYR8JRNMv?K@-)_ck*QvP7>Q1l3C7i0#qO*RV;K*k@zZ*=t7ED0YPkNl z#jSd@CF1lS3$T}Gx*mNoe?Xh-L(a3}lzBG*5JA+Vht`NP&KZF02pIwTOm(=7W}Ee) zn0Fw#9?7g8eTnx=<19m%cv0)W=9(ZL4td6mm-dx5q2vc#!VA<&#u-0_x@Cd_tSL=l zG|ITY)sB<>Mj7UmHBEWkN=6c|^@2zg)77&__2x4L?wU`g)Hi_o=3}n*wHP+-b6;bM z)2@(x-5aRpE{|o>yI(M|GX>q$LZoVKcx0-);H-rX$|#t-(2#|IR;WiE8bW`Ih9sb} z3iFAJt587JR%bX?5}Gpt+OfK^7DWYpM>-VceHx->&IoGeF|^07U2*?q#nbK{D%E2) z2w3TRpAzk!`z%C{U_DiDr~Og!fK!7jyabL7eScDV>!Wr23>7t3z5=sy z8JB`VEcdxIOZ)t&1u2q4%4|X2$n-Ur$Pa?ug=&?RlfB~5I~tJ08`RxnWaIjaYNc~LlU#n35vczgQ?1rZFkOH_~yWHV<6Zl<(nun O6++`VRVQ2y@B9T9vGV%> literal 1963 zcmb_dOK&4Z5bmD0oh*rs;=tC_jUR4ty^sZ?Z=<~{EoB; z`5gzV=791LKJ_UGPP&9s?qqc8bX{8OUf1I;_r51xpZh$3KHwpbpbxooLOQLh?e@c@ z)RRKSY*MDvG#|68Nl~_6LrU^&=wb)WYrZGYt1m%Ru)q$fD9UXdId%kcn~a@>TTxA( z)ALY0uAIunS>ShZ)(*@)cjO^=PYCy56aLk&Z3gW?Xff0*9Wgx-EEjXlluoo@sbXo) ziUEvHMHj(*56}#D$Ren`C8>gIN=fB|=AzY(f{FmTTgT+Z))~=MJC*l}zM@w1Q5zxg zXaW=^_MSeUYncG<2>LA16IP$5$gM?+EU{NLNi8~g!G(GY@TW05(TUW^3iPj|J3orq zQITZ-aR6BWiI{!0zQ91}OCfSQS*@pm4I*AKdt?=0r{Hkqqe3#+eXd4AvQ@CYEY=gO z2y}|)Yap@*wfQ@)36j}N)0+1i=3DaA0x>t6Kqo{B06PnC%$iKY>i@E(o$2 z(M|du-L>C6dJkwE0g>e{_&SjZ`+Z&v`+aj~nrPg6mJXmUq6Tlu_HrAh+5_8iXz6dm g*H*_@AniMf_NN6eGw~Td3R_M+XOl*5bSK*S2N~f95C8xG 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() +