From fd59affd71594f09103e022f40fe0802a34b8f2a Mon Sep 17 00:00:00 2001 From: zhm-real Date: Fri, 19 Jun 2020 14:18:34 -0700 Subject: [PATCH] update --- Search-based Planning/.idea/workspace.xml | 16 +- .../__pycache__/env.cpython-37.pyc | Bin 1463 -> 794 bytes .../__pycache__/tools.cpython-37.pyc | Bin 1983 -> 2581 bytes Search-based Planning/a_star.py | 9 +- Search-based Planning/bfs.py | 9 +- Search-based Planning/dfs.py | 11 +- Search-based Planning/dijkstra.py | 8 +- Search-based Planning/env.py | 15 -- Search-based Planning/tools.py | 17 +- .../Q-policy_iteration.py | 12 +- Stochastic Shortest Path/Q-value_iteration.py | 12 +- Stochastic Shortest Path/policy_iteration.py | 10 +- Stochastic Shortest Path/value_iteration.py | 154 +++++++++++------- 13 files changed, 165 insertions(+), 108 deletions(-) diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 9151c1d..dcef113 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -2,9 +2,17 @@ - + + + + + + + + + - + - - + + diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc index 7f7c2da54272cd2c4f7bffcbe5414a467315bbfb..21dfed443fcc5758c7878b302dabaabb2061f88e 100644 GIT binary patch delta 133 zcmdnaJ&TRciIfgM820NpX-GfVV3c**#7>g}sDdk+CWMd>mq`dt79&dIt_>tjX1k)RI#p;B zaN#hO6DKBq0VhuUk=!{T_yLF$uhW$iCmcPG-w(fjAKUrhe5p3>)$1O?@#@!)XE{4< ztUkJlo*I=ZEts%R2zP`dDmYgJeN23JeAT-f4s}11cQ*UOIE@Fr z&2c{)df&~?yPx-71C=oQ24A5%`$TU~BW4mf9E6Rq5b8bB8?q0k@rkR6hPUJxj^RG? z5i2v+KQgwaAFT-ytVNLR77@-FN-m>NMxl&W{zj*4VheZfK4%k45OLuhElX_Sok3|i zqG~Kb@PxJ92vD3E6k*g~MyOS!9(LVZIzp`zz}}=qPU!)|B9Nv=UPAI|LEE?FvN1uf zOc!x!cKPjXC$Z8|mgYM*&{w8~L&oc>pS=vyFmG=sSrjH}2aUzT|I%Z08$DALB(E4} zq3Aq8NI z<}6P#UD!I-Ntd&pj0I=mbQCM!Dd^}S;(=T-C2SC;U2bO`6^!Q7;r#0GzA#@3E8NAE zT*r`FLL#sRYqSYXXyE68+`yRc7L_0nSri0(ISsYZki?yKerPQ2tj`T;o=?^dT{Kko S*_i#LTQwJbMr+ii?zKOZ<)|zG diff --git a/Search-based Planning/__pycache__/tools.cpython-37.pyc b/Search-based Planning/__pycache__/tools.cpython-37.pyc index 0cbfdb0e68d7b9494b26dbc8d328010501206290..1febd5e99846d991a0135cb519d6cda99e719f05 100644 GIT binary patch delta 1088 zcmZuw%}>-o6rY)XY`41$B7`q7f|@`B5)waxhCqT|MGP0x7&qOvEGzw3rvn1P>;V#u zi3ien@*+o%#*<$B1H5@M6aNJcUX2IeYhgjO&HSeG=KbD#zcR{$@_3iYVR_6$J;eMxrdF1-i%T+Mcr|Lkajy;F9Grw$N6ZGd?CfkH`UrJ0px z)&f{3bF;dmc}DK^MBPz5;uT-BjG}qw0j0zvs9_;JRPNj@LRl7*Y;sD{hDBu|t|isb zX>#1)*oa@|OwdS#8;8x=%dk!ufKIlDlGXSbx~6q?p_RwsWn@(VEXq6ZyB&si!tx4? zBR;3NeGb58wZvw17|EJNZDS>lgx7F|t@%&aoxswi#+|6{OC|6lsWby2)n*V2sfmUN zd|P!k8+Kyp9DB{N=~-&2Mo#G48VcIXr>(4<%n_z$(F#2_2+^te5uE4tROr z$M5U`9s5@w6yb=~GXkg6CH2v@Oje$Y{ne6w2!DogJN==Kw(Mfii2Ous1%Cba`=8(L z*B0j%tb)|JAI2L#??U2vgl>c$0I4;d^#tUj0!urk9<$gwRtLiKxve~_;6(W%I86}J zdnDa7sfqGw56pZxeXH>?JR(W2Zx^miPvI+RY$<#Q%ea^&3LU=Yb347MyCZOB8-65m zjqS6S#!TRZHP4xyPe19mT5S!sQg>Hu-F2c3C$URmGpM?86#1^;mmoG=Ou8v`IQIH+ zr%z%Q@eWkISR`}cnxG~^hiDlpFB3kFbVC-Z;Ne!Qc30>KBm|AxfRZ@!&R{jcl2$K7sV(fax0_v`QOaqlw3 z$HUmQpIMQAvhxkV=-7@S&@b%yz$c*UGrS`p00(T4lzSj80T_N6x!Vx4Tx1)2G&Zn^ zz&K6X&XHfSL*Y*_DHf{D>av;M(VxXDz3qtMiiBN8^;iIKozG`9yg=8xdgKks(R1&H zK5?$>H0i#|jicuEyU0bC34#MtUkdJ<9RABkOQ};?Hew|6o)Bn2H^p5oKcMeirQN5D z1O3ax+Z&99`|rJ5kH)+W)X^GVqF3DHk|lJ`9WT_E8l14jhqx|xjRdz z547|jez?ZuEhew+Wp9kg(&`{prc*XWlGRm}XDwc*GK~=qjVSVJ?{tLcntoH%Z8ED{ zb&sHm3l?laNX|pRG1EQcC4|V5#B7vl%Y)C#hPkGWxQgH0hmYee8U$0eyWB>pF diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index 2cc6c8c..a6d3af0 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -9,6 +9,7 @@ import tools import env import motion_model + class Astar: def __init__(self, x_start, x_goal, heuristic_type): self.u_set = motion_model.motions # feasible input set @@ -16,7 +17,8 @@ class Astar: 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") + tools.show_map(self.xI, self.xG, self.obs, "a_star searching") + def searching(self): """ @@ -48,8 +50,10 @@ class Astar: parent[x_next] = x_current 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): """ Calculate cost for this motion @@ -62,6 +66,7 @@ class Astar: return 1 + def Heuristic(self, state, goal, heuristic_type): """ Calculate heuristic. @@ -85,4 +90,4 @@ if __name__ == '__main__': x_Goal = (49, 5) # Goal node 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 + tools.showPath(x_Start, x_Goal, path_astar) \ No newline at end of file diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index 2626b2c..38483fa 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -9,17 +9,15 @@ import tools import env import motion_model -class BFS: - """ - BFS -> Breadth-first Searching - """ +class BFS: 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.obs = env.obs_map() # position of obstacles - env.show_map(self.xI, self.xG, self.obs, "breadth-first searching") + tools.show_map(self.xI, self.xG, self.obs, "breadth-first searching") + def searching(self): """ @@ -46,6 +44,7 @@ class BFS: parent[x_next] = x_current 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 diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py index 14dc0bd..1ae5ea5 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -9,17 +9,15 @@ import tools import env import motion_model -class DFS: - """ - DFS -> Depth-first Searching - """ +class DFS: 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.obs = env.obs_map() # position of obstacles - env.show_map(self.xI, self.xG, self.obs, "depth-first searching") + tools.show_map(self.xI, self.xG, self.obs, "depth-first searching") + def searching(self): """ @@ -46,6 +44,7 @@ class DFS: parent[x_next] = x_current action[x_next] = u_next [path_dfs, action_dfs] = tools.extract_path(self.xI, self.xG, parent, action) + return path_dfs, action_dfs @@ -54,4 +53,4 @@ if __name__ == '__main__': x_Goal = (49, 5) # Goal node 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 + tools.showPath(x_Start, x_Goal, path_dfs) diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 9a6d5ad..78f00b8 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -9,13 +9,15 @@ import env import tools import motion_model + class Dijkstra: 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.obs = env.obs_map() # position of obstacles - env.show_map(self.xI, self.xG, self.obs, "dijkstra searching") + tools.show_map(self.xI, self.xG, self.obs, "dijkstra searching") + def searching(self): """ @@ -47,8 +49,10 @@ class Dijkstra: parent[x_next] = x_current 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): """ Calculate cost for this motion @@ -67,4 +71,4 @@ if __name__ == '__main__': x_Goal = (49, 5) # Goal node 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 + tools.showPath(x_Start, x_Goal, path_dijk) diff --git a/Search-based Planning/env.py b/Search-based Planning/env.py index 56136c1..a128ecf 100644 --- a/Search-based Planning/env.py +++ b/Search-based Planning/env.py @@ -4,8 +4,6 @@ @author: huiming zhou """ -import matplotlib.pyplot as plt - x_range, y_range = 51, 31 # size of background def obs_map(): @@ -37,16 +35,3 @@ def obs_map(): obs.append((40, i)) return obs - - -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/Search-based Planning/tools.py b/Search-based Planning/tools.py index 4097243..7d4aede 100644 --- a/Search-based Planning/tools.py +++ b/Search-based Planning/tools.py @@ -6,6 +6,7 @@ import matplotlib.pyplot as plt + def extract_path(xI, xG, parent, actions): """ Extract the path based on the relationship of nodes. @@ -25,9 +26,21 @@ def extract_path(xI, xG, parent, actions): 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 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.axis("equal") + + def showPath(xI, xG, path): """ Plot the path. @@ -37,6 +50,7 @@ def showPath(xI, xG, path): :param path: Planning path :return: A plot """ + path.remove(xI) path.remove(xG) path_x = [path[i][0] for i in range(len(path))] @@ -50,6 +64,5 @@ 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) + if length % 15 == 0: plt.pause(0.001) diff --git a/Stochastic Shortest Path/Q-policy_iteration.py b/Stochastic Shortest Path/Q-policy_iteration.py index d83bd3a..e4318ba 100644 --- a/Stochastic Shortest Path/Q-policy_iteration.py +++ b/Stochastic Shortest Path/Q-policy_iteration.py @@ -16,13 +16,13 @@ import sys class Q_policy_iteration: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set + self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.e = 0.001 - self.gamma = 0.9 - self.obs = env.obs_map() # position of obstacles - self.lose = env.lose_map() - self.name1 = "policy_iteration, e=" + str(self.e) + ", gamma=" + str(self.gamma) + self.e = 0.001 # threshold for convergence + self.gamma = 0.9 # discount factor + self.obs = env.obs_map() # position of obstacles + self.lose = env.lose_map() # position of lose states + self.name1 = "Q-policy_iteration, e=" + str(self.e) + ", gamma=" + str(self.gamma) self.name2 = "convergence of error" diff --git a/Stochastic Shortest Path/Q-value_iteration.py b/Stochastic Shortest Path/Q-value_iteration.py index e5234ad..6b8662a 100644 --- a/Stochastic Shortest Path/Q-value_iteration.py +++ b/Stochastic Shortest Path/Q-value_iteration.py @@ -14,13 +14,13 @@ import sys class Q_value_iteration: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set + self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.e = 0.001 - self.gamma = 0.9 - self.obs = env.obs_map() # position of obstacles - self.lose = env.lose_map() - self.name1 = "value_iteration, e=" + str(self.e) + ", gamma=" + str(self.gamma) + self.e = 0.001 # threshold for convergence + self.gamma = 0.9 # discount factor + self.obs = env.obs_map() # position of obstacles + self.lose = env.lose_map() # position of lose states + self.name1 = "Q-value_iteration, e=" + str(self.e) + ", gamma=" + str(self.gamma) self.name2 = "convergence of error" diff --git a/Stochastic Shortest Path/policy_iteration.py b/Stochastic Shortest Path/policy_iteration.py index 2660f0a..58b4efe 100644 --- a/Stochastic Shortest Path/policy_iteration.py +++ b/Stochastic Shortest Path/policy_iteration.py @@ -16,12 +16,12 @@ import sys class Policy_iteration: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set + self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.e = 0.001 - self.gamma = 0.9 - self.obs = env.obs_map() # position of obstacles - self.lose = env.lose_map() + self.e = 0.001 # threshold for convergence + self.gamma = 0.9 # discount factor + self.obs = env.obs_map() # position of obstacles + self.lose = env.lose_map() # position of lose states self.name1 = "policy_iteration, e=" + str(self.e) + ", gamma=" + str(self.gamma) self.name2 = "convergence of error" diff --git a/Stochastic Shortest Path/value_iteration.py b/Stochastic Shortest Path/value_iteration.py index b6685fb..dbcd808 100644 --- a/Stochastic Shortest Path/value_iteration.py +++ b/Stochastic Shortest Path/value_iteration.py @@ -12,94 +12,139 @@ import matplotlib.pyplot as plt import numpy as np import sys + 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.e = 0.001 - self.gamma = 0.9 + self.e = 0.001 # threshold for convergence + self.gamma = 0.9 # discount factor self.obs = env.obs_map() # position of obstacles - self.lose = env.lose_map() - self.name1 = "value_iteration, e=" + str(self.e) + ", gamma=" + str(self.gamma) - self.name2 = "convergence of error" + self.lose = env.lose_map() # position of lose states + self.name1 = "value_iteration, e=" + str(self.e) \ + + ", gamma=" + str(self.gamma) + self.name2 = "convergence of error, e=0.001" def iteration(self): - value_table = {} - policy = {} - diff = [] - delta = sys.maxsize + """ + value_iteration. + + :return: converged value table, optimal policy and variation of difference, + """ + + value_table = {} # value table + policy = {} # policy + diff = [] # maximum difference between two successive iteration + delta = sys.maxsize # initialize maximum difference + count = 0 # iteration times 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 + value_table[(i, j)] = 0 # initialize value table for feasible states - while delta > self.e: + while delta > self.e: # converged condition + count += 1 x_value = 0 for x in value_table: - if x in self.xG: continue - else: + if x not in self.xG: value_list = [] for u in self.u_set: - [x_next, p_next] = motion_model.move_prob(x, u, self.obs) - value_list.append(self.cal_Q_value(x_next, p_next, value_table)) - policy[x] = self.u_set[int(np.argmax(value_list))] - v_diff = abs(value_table[x] - max(value_list)) - value_table[x] = max(value_list) + [x_next, p_next] = motion_model.move_prob(x, u, self.obs) # recall motion model + value_list.append(self.cal_Q_value(x_next, p_next, value_table)) # cal Q value + policy[x] = self.u_set[int(np.argmax(value_list))] # update policy + v_diff = abs(value_table[x] - max(value_list)) # maximum difference + value_table[x] = max(value_list) # update value table if v_diff > 0: x_value = max(x_value, v_diff) - delta = x_value + delta = x_value # update delta diff.append(delta) + self.message(count) + return value_table, policy, diff - def simulation(self, xI, xG, policy): - path = [] - x = xI - while x not in xG: + def cal_Q_value(self, x, p, table): + """ + cal Q_value. + + :param x: next state vector + :param p: probability of each state + :param table: value table + :return: Q-value + """ + + value = 0 + reward = self.get_reward(x) # get reward of next state + for i in range(len(x)): + value += p[i] * (reward[i] + self.gamma * table[x[i]]) # cal Q-value + + return value + + + def get_reward(self, x_next): + """ + calculate reward of next state + + :param x_next: next state + :return: reward + """ + + reward = [] + for x in x_next: + if x in self.xG: + reward.append(10) # reward : 10, for goal states + elif x in self.lose: + reward.append(-10) # reward : -10, for lose states + else: + reward.append(0) # reward : 0, for other states + + return reward + + + def simulation(self, xI, xG, policy, diff): + """ + simulate a path using converged policy. + + :param xI: starting state + :param xG: goal state + :param policy: converged policy + :return: simulation path + """ + + plt.figure(1) # path animation + tools.show_map(xI, xG, self.obs, self.lose, self.name1) # show background + + x, path = xI, [] + while True: u = policy[x] x_next = (x[0] + u[0], x[1] + u[1]) - if x_next not in self.obs: + if x_next in self.obs: + print("Collision!") # collision: simulation failed + else: x = x_next - path.append(x) - path.pop() - return path + if x_next in xG: break + else: + tools.plot_dots(x) # each state in optimal path + path.append(x) - - def animation(self, path, diff): - plt.figure(1) - tools.show_map(self.xI, self.xG, self.obs, self.lose, self.name1) - for x in path: - tools.plot_dots(x) - plt.show() - - plt.figure(2) + plt.figure(2) # difference between two successive iteration plt.plot(diff, color='#808080', marker='o') plt.title(self.name2, fontdict=None) plt.xlabel('iterations') plt.grid('on') plt.show() - - def cal_Q_value(self, x, p, table): - value = 0 - reward = self.get_reward(x) - for i in range(len(x)): - value += p[i] * (reward[i] + self.gamma * table[x[i]]) - return value + return path - def get_reward(self, x_next): - reward = [] - for x in x_next: - if x in self.xG: - reward.append(10) - elif x in self.lose: - reward.append(-10) - else: - reward.append(0) - return reward + def message(self, count): + print("starting state: ", self.xI) + print("goal states: ", self.xG) + print("condition for convergence: ", self.e) + print("discount factor: ", self.gamma) + print("iteration times: ", count) if __name__ == '__main__': @@ -108,6 +153,5 @@ if __name__ == '__main__': VI = Value_iteration(x_Start, x_Goal) [value_VI, policy_VI, diff_VI] = VI.iteration() - path_VI = VI.simulation(x_Start, x_Goal, policy_VI) + path_VI = VI.simulation(x_Start, x_Goal, policy_VI, diff_VI) - VI.animation(path_VI, diff_VI)