From a0e6fc92a5b481404d6bbcd788dda510a660a0fc Mon Sep 17 00:00:00 2001 From: zhm-real Date: Thu, 18 Jun 2020 17:08:10 -0700 Subject: [PATCH] update --- Search-based Planning/.idea/workspace.xml | 14 +++---- .../__pycache__/env.cpython-37.pyc | Bin 1463 -> 1463 bytes Search-based Planning/a_star.py | 5 +-- Search-based Planning/bfs.py | 5 +-- Search-based Planning/dfs.py | 5 +-- Search-based Planning/dijkstra.py | 5 +-- Search-based Planning/env.py | 23 +++++------ .../__pycache__/env.cpython-37.pyc | Bin 0 -> 2000 bytes .../__pycache__/motion_model.cpython-37.pyc | Bin 0 -> 980 bytes .../__pycache__/tools.cpython-37.pyc | Bin 0 -> 1984 bytes Stochastic Shortest Path/env.py | 37 ++++++++++++------ Stochastic Shortest Path/motion model.py | 7 ---- Stochastic Shortest Path/motion_model.py | 37 ++++++++++++++++++ Stochastic Shortest Path/value_iteration.py | 37 ++++++++++++++++++ 14 files changed, 125 insertions(+), 50 deletions(-) create mode 100644 Stochastic Shortest Path/__pycache__/env.cpython-37.pyc create mode 100644 Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc create mode 100644 Stochastic Shortest Path/__pycache__/tools.cpython-37.pyc delete mode 100644 Stochastic Shortest Path/motion model.py create mode 100644 Stochastic Shortest Path/motion_model.py diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 978aad7..77f181c 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -2,18 +2,16 @@ - - - - + - - + + + - + - + diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc index e8ca2a55d6c4de88344ac68fdecaaef199ff4822..7f7c2da54272cd2c4f7bffcbe5414a467315bbfb 100644 GIT binary patch delta 257 zcmdnay`7uaiI&0yPu~0trpVA`uW<2t){j2r(d`1H{ElK!S~-h!G^n0wkiCl8U34(t)@* zTaz)0BP~C#BqcMsBqVC`UKVlHD3*ep{E}O&C7C5TsZlKHMVTp4EQuAF#hPrlm?}Jh z*u6+(@=q3dpc5onbsS`YMrv~0Vvmnc$xn`tj}pvHECHF2lbNJfPzhp1u|en}36K+b QK?FaD06SQAax1G50Il;vo&W#< delta 257 zcmdnay`7uaiIUObRaIy z)?~cJk(QrVl9HKR5^`(uUKVlHTPy`R`6ahlOEODxQg5-O7iFg0Vo9vXEY@VZ#Z=)5 z#O_4`lYg?v1Dznrs^g#nG*Xk}7JGbrN`7*D{4K%U#1fDRIhjd%1(jg4*dTO~49E$* RAc7x6fP7S>GP#x22mp89Mh*Y~ diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index 8da19b1..2cc6c8c 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -10,10 +10,9 @@ import env import motion_model class Astar: - def __init__(self, x_start, x_goal, x_range, y_range, heuristic_type): + 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.x_range, self.y_range = x_range, y_range self.obs = env.obs_map() # position of obstacles self.heuristic_type = heuristic_type @@ -84,6 +83,6 @@ class Astar: if __name__ == '__main__': 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") + astar = Astar(x_Start, x_Goal, "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 f756286..2626b2c 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -14,10 +14,9 @@ class BFS: BFS -> Breadth-first Searching """ - def __init__(self, x_start, x_goal, x_range, y_range): + 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.x_range, self.y_range = x_range, y_range self.obs = env.obs_map() # position of obstacles env.show_map(self.xI, self.xG, self.obs, "breadth-first searching") @@ -53,6 +52,6 @@ class BFS: if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node - bfs = BFS(x_Start, x_Goal, env.x_range, env.y_range) + 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 3bd4a2d..14dc0bd 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -14,10 +14,9 @@ class DFS: DFS -> Depth-first Searching """ - def __init__(self, x_start, x_goal, x_range, y_range): + 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.x_range, self.y_range = x_range, y_range self.obs = env.obs_map() # position of obstacles env.show_map(self.xI, self.xG, self.obs, "depth-first searching") @@ -53,6 +52,6 @@ class DFS: if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node - dfs = DFS(x_Start, x_Goal, env.x_range, env.y_range) + dfs = DFS(x_Start, x_Goal) [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 95e0759..9a6d5ad 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -10,10 +10,9 @@ import tools import motion_model class Dijkstra: - def __init__(self, x_start, x_goal, x_range, y_range): + 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.x_range, self.y_range = x_range, y_range self.obs = env.obs_map() # position of obstacles env.show_map(self.xI, self.xG, self.obs, "dijkstra searching") @@ -66,6 +65,6 @@ class Dijkstra: if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node - dijkstra = Dijkstra(x_Start, x_Goal, env.x_range, env.y_range) + dijkstra = Dijkstra(x_Start, x_Goal) [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 index f777d06..56136c1 100644 --- a/Search-based Planning/env.py +++ b/Search-based Planning/env.py @@ -6,7 +6,7 @@ import matplotlib.pyplot as plt -x_range, y_range = 51, 31 # size of background +x_range, y_range = 51, 31 # size of background def obs_map(): """ @@ -15,28 +15,28 @@ def obs_map(): :return: map of obstacles """ - obs_map = [] + obs = [] for i in range(x_range): - obs_map.append((i, 0)) + obs.append((i, 0)) for i in range(x_range): - obs_map.append((i, y_range-1)) + obs.append((i, y_range - 1)) for i in range(y_range): - obs_map.append((0, i)) + obs.append((0, i)) for i in range(y_range): - obs_map.append((x_range-1, i)) + obs.append((x_range - 1, i)) for i in range(10, 21): - obs_map.append((i, 15)) + obs.append((i, 15)) for i in range(15): - obs_map.append((20, i)) + obs.append((20, i)) for i in range(15, 30): - obs_map.append((30, i)) + obs.append((30, i)) for i in range(16): - obs_map.append((40, i)) + obs.append((40, i)) - return obs_map + return obs def show_map(xI, xG, obs_map, name): @@ -49,3 +49,4 @@ def show_map(xI, xG, obs_map, name): plt.title(name, fontdict=None) plt.grid(True) plt.axis("equal") + diff --git a/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9a289c4ae3024a31aef2bf0d4a02091fcdbfc154 GIT binary patch literal 2000 zcmbtV&2Ah;5bo}szunl5ZGvNmC>e!FVL4b}2nWluViJkuVj)b3)Y`BbPmgyeo}C$Y z_rls*pByQ<!i*?%Kj!LQ7TE(^Xwn-Su_Nm$S1C0^|3; z|N8z{n~*>8V>}p49ssG&feFHipn@f|$9RnrQEM@7aa&lzeo43^9N~g=g(vDDJ;8QJ zvvG92@gV5yZYJ0LZa?nDX~#e6X8pzpT$1f38+`_HbQ@TRD>yK$1N0C`U896_$T1n* z(nQk}`iMMVJf=hPmJO+(d+d}_axnLd4DL+yOJlvR3ARisn^mwuAGPy71Raijd{g-- z^_33|2VN!<8wobPhNBDXgk8wQuA=;0MiKUfD9*&gZJ$Ch zGcqLRY9BXz7*Enz$3YSwMSixcbPy(yy65Mag5@k#4P&$}Bi)zjy59?OKiivH8m)vo zCL_ZT45RmvT}5^c*)3%A$UXwrvaLoZi$I*5&|}+Z0P+9QIIjro zu_hX@uJ*V_V_%~2G<#bc65*@>+Jw78gm(&$-mFryO3iA0vT6?<(I~%O)Q0ZR6IaBH zn0;g6t{2wO5=6|sVaA6ouHw4|Z;qI+X6`A0rh>>`*bWhkIItC1Kv*%g46Fm`JfA(>Qu_r}NA8DP_t&=8wxDG=y+H0qlG|aHWU@)Qx0j__#G(F{+tEQk zNSdB`T}3U4(xR3pnp=62>B82rP9kn~WGuK93}VGCtY@>%+29G#S0xHC$-+v5Uc_x| z9D`CEa>u;JK_w5zcWDk~@Ke6oF-AVclVME>y+LmQ7P>KJuX1#FRZIOlT zHecuk8dFW;U1(c4<4!5%O)!<%J0y18X`;qQSv_o_(>fO3MlTcnB-+F#p|E3HY=(Kv IyY0FE05Hgy00000 literal 0 HcmV?d00001 diff --git a/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..79388a0e24fc9abfb0c9bf596615d9a8d30a29cc GIT binary patch literal 980 zcmaJ=y^ho{5VoD4-OKH@EC_^LAjBec1!aWbbaw>}2 z;=OCA={X!C*x_ymI7Cy@kmFFFH^B$+e8d>NnJ(@Boms}cL`~56Q}_M)r{5w9@1Qj)udSCB57m5_YYZ}FnVb?ZR^tq0jAVU@aMl5nwF>r@L?Bp%D` zC#1LG)<5Y%JbrG3Hsi;ukxXDD$A`ikTUCuKX6Ljjn3OJgR?L*n?YuPOBdfAGGd9oY z5lpld#?ogHJYIg7v}@09b<%E?>a9p!+3t5THvrJ~73||a9;}r_B*M4wfZWA;2mB+f z-As>ywwI<{Wog<5vM%8^BCEEye8P00T@@QJfOBpC0X$z9;$Z+g69T|+0!4vnGIo+_C*xV$ zY13$WK&?bC9QY5~$gJ|Vy2;Ho%a+=r^(gNBh7VU#%;9Xc(S66P}RgtR>7vjBRZRagjpz?>b@s7~Ij z-H%6lAY?NdjM8D6_oK-`jA|F)ifknwc2Fu%(S8V3wV^3#pB{OoThbK*mS57NV7yj3 zWw1hyg3>!9WpGX@DSfb9oVBB%Bzulr-X_nMzag4xr}U2LF}0SDRu5s(*r9~fA}F7a zwTu%T=>d<5SP!CZtT>BAZf7LVVx5Xy4bmbKy(kxqt2Mi^S;R6PM&l>VXh+9V-NrO(^+3m+%0MigCtIpl+IA3f5F+ zFj`gI-|5E5UaJaoYG#f*$w=b0o)cLG1~0#T*la&l;I936#(WEyZ$IK{Pm7{$U;7$U zob`oF^wRrq+r>Q|D9GI_Lw>Qn#QXwSd;BM;#hM ze}jf3pmG7`6Bk#ZfUvF7aI7RWrv%hvm18Z68v2p+C<^=}MAe)URLvu3k6gRr{>zG| z-CfkG$7~p|%1=He+CB4Gh#tWT&IyD{6|`5I{9xaHI}cjtn=3)+wtRu2TXaMC6 zv{4ckk9-Yhq0WF9`mv$%lD-ehuV!h^4^pNF#!o~hq!;JU;dj7|YFT2~HZb2Xf zk`Ki`H$KE#pBp#hxp9k3!{y;YjbFqg#f^toZ1@s2CIf5`N(-U~f@M7G52p_}gNZL@ z!mFow8%7HEkgx{&y#v2n&`X?8@Ex&WkGaz3$uGdrWS5qZkXL{><^W}ojxiNm1(Gx8 zob0-19wud0Jt60i_e#oY%di9T9U+MfMi0bu;AHb)| z{(;g}S_jtZRN@?{r_upTNvC6$hOx#CS=xmhW~CEM`zadCRF-VJbMC?~4-D6bf{il1 Qfg)2OG@es;!rS4}-#}3Fh5!Hn literal 0 HcmV?d00001 diff --git a/Stochastic Shortest Path/env.py b/Stochastic Shortest Path/env.py index f777d06..ba435c3 100644 --- a/Stochastic Shortest Path/env.py +++ b/Stochastic Shortest Path/env.py @@ -6,7 +6,7 @@ import matplotlib.pyplot as plt -x_range, y_range = 51, 31 # size of background +x_range, y_range = 51, 31 # size of background def obs_map(): """ @@ -15,37 +15,50 @@ def obs_map(): :return: map of obstacles """ - obs_map = [] + obs = [] for i in range(x_range): - obs_map.append((i, 0)) + obs.append((i, 0)) for i in range(x_range): - obs_map.append((i, y_range-1)) + obs.append((i, y_range - 1)) for i in range(y_range): - obs_map.append((0, i)) + obs.append((0, i)) for i in range(y_range): - obs_map.append((x_range-1, i)) + obs.append((x_range - 1, i)) for i in range(10, 21): - obs_map.append((i, 15)) + obs.append((i, 15)) for i in range(15): - obs_map.append((20, i)) + obs.append((20, i)) for i in range(15, 30): - obs_map.append((30, i)) + obs.append((30, i)) for i in range(16): - obs_map.append((40, i)) + obs.append((40, i)) - return obs_map + return obs -def show_map(xI, xG, obs_map, name): +def lose_map(): + lose = [] + for i in range(27, 34): + lose.append((i, 13)) + return lose + + +def show_map(xI, xG, obs_map, lose_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))] + lose_x = [lose_map[i][0] for i in range(len(lose_map))] + lose_y = [lose_map[i][1] for i in range(len(lose_map))] + plt.plot(xI[0], xI[1], "bs") plt.plot(xG[0], xG[1], "gs") plt.plot(obs_x, obs_y, "sk") + plt.plot(lose_x, lose_y, marker = 's', color = '#A52A2A') plt.title(name, fontdict=None) plt.grid(True) plt.axis("equal") + plt.show() + diff --git a/Stochastic Shortest Path/motion model.py b/Stochastic Shortest Path/motion model.py deleted file mode 100644 index 144bbcc..0000000 --- a/Stochastic Shortest Path/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/Stochastic Shortest Path/motion_model.py b/Stochastic Shortest Path/motion_model.py new file mode 100644 index 0000000..592d6d9 --- /dev/null +++ b/Stochastic Shortest Path/motion_model.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" +import numpy as np + +motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets + +def move_prob(x, u, obs, eta = 0.2): + """ + Motion model of robots, + + :param x: current state (node) + :param u: input + :param obs: obstacle map + :param eta: noise in motion model + :return: next states and corresponding probability + """ + + p_next = [1 - eta, eta / 2, eta / 2] + x_next = [] + if u == (0, 1): + u_real = [(0, 1), (-1, 0), (1, 0)] + elif u == (0, -1): + u_real = [(0, -1), (-1, 0), (1, 0)] + elif u == (-1, 0): + u_real = [(-1, 0), (0, 1), (0, -1)] + else: + u_real = [(1, 0), (0, 1), (0, -1)] + + for act in u_real: + if (x[0] + act[0], x[1] + act[1]) in obs: + x_next.append(x) + else: + x_next.append((x[0] + act[0], x[1] + act[1])) + return x_next, p_next \ No newline at end of file diff --git a/Stochastic Shortest Path/value_iteration.py b/Stochastic Shortest Path/value_iteration.py index d9a2968..7dd5a7d 100644 --- a/Stochastic Shortest Path/value_iteration.py +++ b/Stochastic Shortest Path/value_iteration.py @@ -4,4 +4,41 @@ @author: huiming zhou """ +import env +import tools +import motion_model +import numpy as np +import copy + +class Value_iteration: + 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.T = 500 + self.gamma = 0.9 + self.obs = env.obs_map() # position of obstacles + self.lose = env.lose_map() + self.name = "value_iteration, T=" + str(self.T) + ", gamma=" + str(self.gamma) + + env.show_map(self.xI, self.xG, self.obs, self.lose, self.name) + + def iteration(self): + value_table = {} + policy = {} + + for i in range(env.x_range): + for j in range(env.y_range): + if (i, j) not in self.obs: + value_table[(i, j)] = 0 + + for k in range(self.T): + value_table_update = copy.deepcopy(value_table) + for key in value_table: + + + +if __name__ == '__main__': + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node + VI = Value_iteration(x_Start, x_Goal) \ No newline at end of file