From 45089174cbfcf22b172b1b1472ff721447444cbf Mon Sep 17 00:00:00 2001 From: zhm-real Date: Sat, 20 Jun 2020 19:41:17 -0700 Subject: [PATCH] update --- Model-free Control/Q-learning.py | 97 +++++++------- Model-free Control/Sarsa.py | 94 +++++++------- .../__pycache__/env.cpython-37.pyc | Bin 1083 -> 1922 bytes .../__pycache__/motion_model.cpython-37.pyc | Bin 974 -> 1268 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 0 -> 4361 bytes .../__pycache__/tools.cpython-37.pyc | Bin 3276 -> 0 bytes Model-free Control/env.py | 92 +++++++------ Model-free Control/motion_model.py | 59 +++++---- Model-free Control/plotting.py | 121 ++++++++++++++++++ Model-free Control/tools.py | 92 ------------- Search-based Planning/.idea/workspace.xml | 16 +-- Search-based Planning/a_star.py | 29 +++-- Search-based Planning/bfs.py | 18 +-- Search-based Planning/dfs.py | 6 +- Search-based Planning/dijkstra.py | 6 +- Search-based Planning/queue.py | 1 - 16 files changed, 342 insertions(+), 289 deletions(-) create mode 100644 Model-free Control/__pycache__/plotting.cpython-37.pyc delete mode 100644 Model-free Control/__pycache__/tools.cpython-37.pyc create mode 100644 Model-free Control/plotting.py delete mode 100644 Model-free Control/tools.py diff --git a/Model-free Control/Q-learning.py b/Model-free Control/Q-learning.py index 98a7dcc..175e627 100644 --- a/Model-free Control/Q-learning.py +++ b/Model-free Control/Q-learning.py @@ -5,7 +5,7 @@ """ import env -import tools +import plotting import motion_model import matplotlib.pyplot as plt @@ -15,18 +15,29 @@ import sys class QLEARNING: 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.M = 500 # iteration numbers - self.gamma = 0.9 # discount factor + self.M = 500 # iteration numbers + self.gamma = 0.9 # discount factor self.alpha = 0.5 - self.epsilon = 0.1 # epsilon error - self.obs = env.obs_map() # position of obstacles - self.lose = env.lose_map() # position of lose states - self.name1 = "Qlearning, M=" + str(self.M) + self.epsilon = 0.1 + + self.env = env.Env(self.xI, self.xG) + self.motion = motion_model.Motion_model(self.xI, self.xG) + self.plotting = plotting.Plotting(self.xI, self.xG) + + self.u_set = self.env.motions # feasible input set + self.stateSpace = self.env.stateSpace # state space + self.obs = self.env.obs_map() # position of obstacles + self.lose = self.env.lose_map() # position of lose states + + self.name1 = "SARSA, M=" + str(self.M) + + [self.value, self.policy] = self.Monte_Carlo(self.xI, self.xG) + self.path = self.extract_path(self.xI, self.xG, self.policy) + self.plotting.animation(self.path, self.name1) - def Monte_Carlo(self): + def Monte_Carlo(self, xI, xG): """ Monte_Carlo experiments @@ -38,10 +49,10 @@ class QLEARNING: for k in range(self.M): # iterations x = self.state_init() # initial state - while x != self.xG: # stop condition + while x != xG: # stop condition u = self.epsilon_greedy(int(np.argmax(Q_table[x])), self.epsilon) # epsilon_greedy policy x_next = self.move_next(x, self.u_set[u]) # next state - reward = env.get_reward(x_next, self.lose) # reward observed + reward = self.env.get_reward(x_next) # reward observed Q_table[x][u] = (1 - self.alpha) * Q_table[x][u] + \ self.alpha * (reward + self.gamma * max(Q_table[x_next])) x = x_next @@ -51,7 +62,6 @@ class QLEARNING: return Q_table, policy - def table_init(self): """ Initialize Q_table: Q(s, a) @@ -60,28 +70,25 @@ class QLEARNING: Q_table = {} - for i in range(env.x_range): - for j in range(env.y_range): - u = [] - if (i, j) not in self.obs: - for k in range(len(self.u_set)): - if (i, j) == self.xG: - u.append(0) - else: - u.append(np.random.random_sample()) - Q_table[(i, j)] = u - + for x in self.stateSpace: + u = [] + if x not in self.obs: + for k in range(len(self.u_set)): + if x == self.xG: + u.append(0) + else: + u.append(np.random.random_sample()) + Q_table[x] = u return Q_table - def state_init(self): """ initialize a starting state :return: starting state """ while True: - i = np.random.randint(0, env.x_range - 1) - j = np.random.randint(0, env.y_range - 1) + i = np.random.randint(0, self.env.x_range - 1) + j = np.random.randint(0, self.env.y_range - 1) if (i, j) not in self.obs: return (i, j) @@ -121,40 +128,36 @@ class QLEARNING: return x return x_next - - def simulation(self, xI, xG, policy): + def extract_path(self, xI, xG, policy): """ - simulate a path using converged policy. + extract path from converged policy. :param xI: starting state - :param xG: goal state + :param xG: goal states :param policy: converged policy - :return: simulation path + :return: path """ - plt.figure(1) # path animation - tools.show_map(xI, xG, self.obs, self.lose, self.name1) # show background - - x, path = xI, [] - while True: + x, path = xI, [xI] + while x not in xG: u = self.u_set[policy[x]] x_next = (x[0] + u[0], x[1] + u[1]) if x_next in self.obs: - print("Collision!") # collision: simulation failed + print("Collision! Please run again!") + break else: + path.append(x_next) x = x_next - if x_next == xG: - break - else: - tools.plot_dots(x) # each state in optimal path - path.append(x) - plt.show() - self.message() - return path - def message(self): + """ + print important message. + + :param count: iteration numbers + :return: print + """ + print("starting state: ", self.xI) print("goal state: ", self.xG) print("iteration numbers: ", self.M) @@ -168,5 +171,3 @@ if __name__ == '__main__': x_Goal = (12, 1) Q_CALL = QLEARNING(x_Start, x_Goal) - [value_SARSA, policy_SARSA] = Q_CALL.Monte_Carlo() - path_VI = Q_CALL.simulation(x_Start, x_Goal, policy_SARSA) diff --git a/Model-free Control/Sarsa.py b/Model-free Control/Sarsa.py index 57f1f4c..5dbc65f 100644 --- a/Model-free Control/Sarsa.py +++ b/Model-free Control/Sarsa.py @@ -5,27 +5,37 @@ """ import env -import tools +import plotting import motion_model -import matplotlib.pyplot as plt import numpy as np class SARSA: 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.M = 500 # iteration numbers - self.gamma = 0.9 # discount factor + self.M = 500 # iteration numbers + self.gamma = 0.9 # discount factor self.alpha = 0.5 - self.epsilon = 0.1 # epsilon error - self.obs = env.obs_map() # position of obstacles - self.lose = env.lose_map() # position of lose states + self.epsilon = 0.1 + + self.env = env.Env(self.xI, self.xG) + self.motion = motion_model.Motion_model(self.xI, self.xG) + self.plotting = plotting.Plotting(self.xI, self.xG) + + self.u_set = self.env.motions # feasible input set + self.stateSpace = self.env.stateSpace # state space + self.obs = self.env.obs_map() # position of obstacles + self.lose = self.env.lose_map() # position of lose states + self.name1 = "SARSA, M=" + str(self.M) + [self.value, self.policy] = self.Monte_Carlo(self.xI, self.xG) + self.path = self.extract_path(self.xI, self.xG, self.policy) + self.plotting.animation(self.path, self.name1) - def Monte_Carlo(self): + + def Monte_Carlo(self, xI, xG): """ Monte_Carlo experiments @@ -38,9 +48,9 @@ class SARSA: for k in range(self.M): # iterations x = self.state_init() # initial state u = self.epsilon_greedy(int(np.argmax(Q_table[x])), self.epsilon) - while x != self.xG: # stop condition + while x != xG: # stop condition x_next = self.move_next(x, self.u_set[u]) # next state - reward = env.get_reward(x_next, self.lose) # reward observed + reward = self.env.get_reward(x_next) # reward observed u_next = self.epsilon_greedy(int(np.argmax(Q_table[x_next])), self.epsilon) Q_table[x][u] = (1 - self.alpha) * Q_table[x][u] + \ self.alpha * (reward + self.gamma * Q_table[x_next][u_next]) @@ -60,17 +70,15 @@ class SARSA: Q_table = {} - for i in range(env.x_range): - for j in range(env.y_range): - u = [] - if (i, j) not in self.obs: - for k in range(len(self.u_set)): - if (i, j) == self.xG: - u.append(0) - else: - u.append(np.random.random_sample()) - Q_table[(i, j)] = u - + for x in self.stateSpace: + u = [] + if x not in self.obs: + for k in range(len(self.u_set)): + if x == self.xG: + u.append(0) + else: + u.append(np.random.random_sample()) + Q_table[x] = u return Q_table @@ -80,8 +88,8 @@ class SARSA: :return: starting state """ while True: - i = np.random.randint(0, env.x_range - 1) - j = np.random.randint(0, env.y_range - 1) + i = np.random.randint(0, self.env.x_range - 1) + j = np.random.randint(0, self.env.y_range - 1) if (i, j) not in self.obs: return (i, j) @@ -121,40 +129,36 @@ class SARSA: return x return x_next - - def simulation(self, xI, xG, policy): + def extract_path(self, xI, xG, policy): """ - simulate a path using converged policy. + extract path from converged policy. :param xI: starting state - :param xG: goal state + :param xG: goal states :param policy: converged policy - :return: simulation path + :return: path """ - plt.figure(1) # path animation - tools.show_map(xI, xG, self.obs, self.lose, self.name1) # show background - - x, path = xI, [] - while True: + x, path = xI, [xI] + while x not in xG: u = self.u_set[policy[x]] x_next = (x[0] + u[0], x[1] + u[1]) if x_next in self.obs: - print("Collision!") # collision: simulation failed + print("Collision! Please run again!") + break else: + path.append(x_next) x = x_next - if x_next == xG: - break - else: - tools.plot_dots(x) # each state in optimal path - path.append(x) - plt.show() - self.message() - return path - def message(self): + """ + print important message. + + :param count: iteration numbers + :return: print + """ + print("starting state: ", self.xI) print("goal state: ", self.xG) print("iteration numbers: ", self.M) @@ -168,5 +172,3 @@ if __name__ == '__main__': x_Goal = (12, 1) SARSA_CALL = SARSA(x_Start, x_Goal) - [value_SARSA, policy_SARSA] = SARSA_CALL.Monte_Carlo() - path_VI = SARSA_CALL.simulation(x_Start, x_Goal, policy_SARSA) diff --git a/Model-free Control/__pycache__/env.cpython-37.pyc b/Model-free Control/__pycache__/env.cpython-37.pyc index fb6bf059834e720c1fef8cf87fb184434c988721..5acdbd86373d218f2ecdd477ac09cd0bccd450d5 100644 GIT binary patch literal 1922 zcmZuxL2uhO6ecNIQf0?Yvo*_hD1=>B7idmB8O0DJ9ohn0FrXU(7+wj@WNam}q>ysw z81^pd&*&jN_OO4kU!m(xzU&9=vb{&zikp=NA0PRip2_cf?;UP!g#^YQfBya2_X+t6 zH|ybmasX5Bf)J!fo)bX@drJhnVm&IHSES?4z6cNENjb_@HyTZnagq(A*(jfc1Nf*O z473B7`Uwb0;A7HbHdxOQj&R?So+}!{gS8=i5y0w+J0gVD7ftx1R^@z`y&2%0aNz2~ zJc6lTfDm*}INS^*+)S8YLl<^zjuvN*7T8}m-V@L~(0ro>pxrTAC#VTb23)8q>QCYV>4_O+ zQt~5g))>~%eB@c7i)0n z?5KXxP-ap=wOULfQ?s?PM#BJh%Lw$F$Jgm$Qov&2S}d^a3>FnVyA220u$5{=)55WB zl45ojlfnF>Z6N~$9S5#BO+p~L*cZ>pxu@C!NEgf$#Cd)FL7WaIDLzig-(n>&*E2aS z?OQkMp@>yHj;0)q-A$MOrG*Vt|3mT)`>E-h)G!aN)YOD$gWY;`6%J*|ZHz;>zs4^~ z5K@=U+W6`l^KmD@9+t&p$$1rWKF-A?MZLxO#UxIxMSTtdOpz(fFJ%yafZ9#=8O;Xsdyj9iFg23lvi|_V?y8mm literal 1083 zcmZWo&2G~`5Z+xowh2vDDkz+a#RY;&A0R@gqCJ!YKcYgFg27s{+r+BlHR}zeQF@^~ z01vy`Tr=&E`3sl5cEE1wCQkDJAdMkIDGX(!aIv z8%MBPq>kBz7{qWcV+cB3V7#m`Du0b(C?YIl^f8$dD9D){HbYoXvcx1jO(rr_?+U|v zsVwe>xhmjTWku5lb+j}iopr(?&qH95P;IwtXrA?D>5t>e@OYlfOqBjv zb8vdmQqgUbLtSn_T7Dgsc%bLj@%WcnJ|J%q)DJBlrH1B@w5#;K>*1U({x z*|~UaGwJcPH%cM>P|J^83v7EP$EK>O9*dl7J`Be(Iy#r#D`i#y7B%@gpU-C?bDKO2 zgIC?wn9RRbHT%-U)$JW{7x+h!AfPVQ*wtrkuk_I_SIHxgDzp>wCRej VheIVsseFX&TDn{oIKhpe^#@Pk0C@la diff --git a/Model-free Control/__pycache__/motion_model.cpython-37.pyc b/Model-free Control/__pycache__/motion_model.cpython-37.pyc index 5a5a3aa6ef2eba708c717507fb51cbd5233479dd..5f39a530fd0e48af14060d88f3cbf749f4f43a7f 100644 GIT binary patch literal 1268 zcmZ8hy>1*g5GJ`lw_2YRHlYUCqS$wNP;XbA*dx!x7OZr&E*>B z!>Y1gBB`X(h1)!V8^6M~DFQV~pC*~%>6EhunBnldkFzu6%st-TP6^1{cYpoThJ^fq zo3#YsJOXX*gHa?SiYivpj6NkQP~kaI;cJ#L#l9n>Xt|d@lC7H=JLa>tm>1OvU(QUM zUcq5;qlB?1Lv@2IjaCb6NKRIHRBs8(mI&w)h70 z=^JA4R{r_vU_AN0(YBdZ|`9 z97Vg}W6}kReR^$Ag0t|1oJAc|!6~!*_+%;qG_Jk`EC%ecif^#{fQ5kl07#%>&r4Ob z_7~rF@ge#7@NCdgHTZ;d1GIYtcGT~{?vW`wOS%N_{xr51U4$=kNhx_T^?tJQeuVqy__s`7Dlv{IbT=RK)Lm1c6$~j+*dEQ#9E5{oro#vlaaNncbw06vk zs&3tF9Nv4(!8n_@@9SEUJE`-H@0S|5*^^ts4$yck-KEP4&K|+?W{{SY5 BIvM}~ delta 615 zcmaJ-J!=$E6n*!;`P$hT3ke1#i9sbW78W*XL?y8j0+N8jfDUf%ZjfYW#y4(Ic_9e? zf$$13m6jI%4IvP1V)d}Ikbrjfdf#joQ+eQ?Irrmn=bqo4FX8G^n#O|hYUS6XZ&HZw zE}6h%ahs!AU?PzV0>UlGktBKKQ?TLXl46Re^BQuGa7|?0y}K9hWq1tyL^)#phjF6Y zkHWS-*1S~w*njJAaBJPIbci@Yj%AMK(0qcq3`lOGx;EauA$NlEx&{ikwnTyZpSCuq z!K665+ta}v@i5;BwV?34(4pgL!5Op}^RyVC#?3(OYyTa0-UEoIgR%bKoAKNOp?&QQ z+|AL;qj;XNQayW8mGi@j`sNLuH6QV^`HEBKC(fEwE|^8Rc)z{<__P5h+W9as$(1I$ zHS%8*Z1A|Q`z2+Oh36JVR#v^ne3qBI4?-p80u;NQVYS^aYE|{R&b}EY(LDzqM377Z zV~pVnBzP6o8SZDGocEb;!=j*Sy(p|-j)wIMi)C%YVS8HBj;e$QPf^OLjc)MSqpki; NUvS<8T;qM1$iJ=-eUktH diff --git a/Model-free Control/__pycache__/plotting.cpython-37.pyc b/Model-free Control/__pycache__/plotting.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..cfc1bf0d826125a8201ea79a1146664d6c105404 GIT binary patch literal 4361 zcmcgv&2J<}6|buPn4aiqGzh=b$xWb zdhb`idiA5(*&4$$|JQ&1@{>Aaf1{VlV?fwMN?t}Lnad=XBII4+8a$Ux*L=XFAj&K+>NJLXaldwk53#K_<&#H1*hEV1BlDTdF_EB& zPMB-tpLR`YNgMB$tVjo?EvvGIvLa_>9i<~@_95@{Q-Kubjn1OFKolFdViu52e{OB+< zevGUV?bh6-Byy$P_HjK7Hrj+V(q%uafOI3tPFc z)%JZqIY{H)cF#-q8okhqA~K^Fc48Hzd);LFtyucuE4#|~8~5TURdKl8E7rZ*dz?9~ zRuBbgt2Kj35(gRMb9{-{`EYS!pVdj93x*;MwV}xGDI0UiPdSGv#$qH2$v9=Y#vJK3 z?RR7N>sq6DyeQ~;X%I)NwX$xl=P9q-parcp;$Dg#p?vqPHc@>r9u{btY}v5l>9cidMU5wI#Jy&$o`m$dI*Z&A5`g zE;YuEoFTW$I{p)jUZ|O?}3V=t{^l3scx2KZR5S_&A-gFuvHEJp1H6+Xr+)ji zwjKsa+K#)ujV4+Wn!{$p7fY9sDjFKA>tWpX!ej%&WXR`KZJu?t%bKBrP@h%gPfA0CnK_>hNbJa%lbr_A)Ue5Sm^A&? zsD)m%$@P)+M+hT6&QEIMEU@O_cO&rUdmkJbfQxv89f?%z8(=#y266%3K!`#y1c3-= zy?F{A9c%8C)f09qaFmG@e0a=d?FKtwYG2MAnbAsFTffGNmJ>OP7E{h0nP60N85$f2 zwT!Pia-LZ9(ek@YE&!;D!$Aqu@?4+;EmqQ3Bpe1wkovL_;UrI<=F*~h?P9<3n^QzT zbqY>-B;&6QE4RM?D*ZAZHx0KM29f`MAk#hdbr@6KLY6Ha_>Wu44}A~5^AADo<&Gwr zCDMEwDIrFpKuIrpDaA2%%p)OLhhxFUR%ZK2+w1u`H;c@`XEW0u1Sx3D(%-#&t~%?X z*WHoc#uj~_CSrW}(nD-f3gA{X!xV-%zfbZpXGAk+#Fu9@@{C}4_6oAjzd!ufM{n&s zyhBWBE592b`Z+5SOKQAkMyI`-*=;X6^pb3*+Y4LmIEwsss?nWUJ+F_j&C;CdRtgPo{#S|Yw9}?rf27r+SF}oc#Se5F7-MxMDu&``vvK# z=c)P)$}TB)4RyajN(grh!FL@X00hL`#lTU6hEy#haiTpsNlV(BM&c=gMtGJ<@dVU` zdR&yqcaThJ7Lp|`lB9iP41S`Kv2kQ#q&+f5X6}0(II?jCcyPwJQsvhvqClrkz|7N80@tVTO}-im?A5xD=N28Q`L|Tmi0s{QNH~zxn&0 z@90ntuqCZKzqXx0g*Lelj<%X#x!uNS)&UjMfa^%R%^Ao zvFwK=*IO-$Nb?p&(V`+U&;g{Pqh9@hGTH} zcWyYdirSiXR%x|l+{T29xL49{34gl$SwV>TgI!V?F25r%+9D+!)7CSgWD?vy4E_Ag4LrzC9><~&OXV#14>Em0%hZ>`RCD>aTbWotVo_lIuV&_C*43agG?~4mMdmwtsBXx6Z9X{ zf=4=%8cjp~4D)_1xG$pg1OxW+OxJ>MrVnUvz0G+xJY`x8D2Phbf>lEV8Rra?Yzi5G z_L*vO8H_jUZ80rCa@~_zEqILfrBRllnrNtzU$qM_>UMd?3MWmJE_}(KatSw3EfrS( z0rdNYH&iw(jnoLsxWC?r;;pa@YidOH>v2z_S+9un!G%ZP+^ubVt3cewS0mSl;QEbw zTy1I5-7wdEtW%V>giLh1qc$GGEvDC+lJnrM$g~vch87}K%iWz~o(~;XH#Q(z&ci_F zJX)a+wWtsCBK3)9Nd$ImBNv(r9YKcwm4soB0B{Uw3`G$>FG!Oj#-IBLm_34kxd-E} zZ8-KDIF7bA5vC3^k-$nXT}rgQ?=l}H0_7bLh?X+It}^(^-1>SdCV+c4XwvOkuzte5 zwXyeC!S_O&!(%|cX&BuYa_69{_h4wjef)5Gm+nFEEkV35k)N$64u*@wJ;tY1tFV@@ z7c{BT!a*09Rapl9QRNmwC>EXW$~BaU2Y6&~MYYBDI()i(D;05+sudu{AHIz?hf~_f zIn-<(2GDW=X+)mUM=k=j41J_XL2njR9?@$6`f{3Nd^=&fUAVDGg$&J#(mN?{{rl?g z-yb&CzgUA9@Feeur@U|>%v!v#Q=S!eH`QP~Txj8Tqn_f0gC8{d!%nj^FD;Y?VRIJ4hMfl3of{m@h6`q#mK42=1v{b_Kq^jJ z8D}RWNR9I`5c)(yZec<;9A)H>61ixodME1Eg04`B$+wvCVO2+Q%4a@Jh2n4c7M3x) zo;Wm%I=mL(h!S->Q9(0l_2fviagavSt8Wt+(_uOt7)5Dh+iSsb8!$8zrD)T;ic)zB zqz@r)O)DecAFofMbakT0JGI*%JL)yVO_ecBPj7YJ|Cx66q-pmirVZVLdqb=@~@n!Bra*Ioe51b5OSgX>|Z^Dbnl30f>f6|N6swXcX z{~``3QZB(zxH{1(51pu=sA_pCF`3&(Jqrf`8M+9K{&3m}J#%4@vCKTmSe7()nVplg zg;>G5aQObtL&~zo{VXJ}MJhiA?(y|v&c@ViuKo@Xnru=?C-Bn1XhBkedXhmc2`{Jz zsetTT2V~RUcZSL4IXS=tK_EfQnFKKp31VUJc#T;t4JhnE7EgarYH2B^%a)($xe&!1Q zRvL90EL!;<Wa0 diff --git a/Model-free Control/env.py b/Model-free Control/env.py index bcba4d8..39938c3 100644 --- a/Model-free Control/env.py +++ b/Model-free Control/env.py @@ -4,53 +4,73 @@ @author: huiming zhou """ -x_range, y_range = 14, 6 # size of background +class Env(): + def __init__(self, xI, xG): + self.x_range = 14 # size of background + self.y_range = 6 + self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] + self.xI = xI + self.xG = xG + self.obs = self.obs_map() + self.lose = self.lose_map() + self.stateSpace = self.state_space() + def obs_map(self): + """ + Initialize obstacles' positions -def obs_map(): - """ - Initialize obstacles' positions + :return: map of obstacles + """ + x = self.x_range + y = self.y_range + obs = [] - :return: map of obstacles - """ + for i in range(x): + obs.append((i, 0)) + for i in range(x): + obs.append((i, y - 1)) - obs = [] - for i in range(x_range): - obs.append((i, 0)) - for i in range(x_range): - obs.append((i, y_range - 1)) + for i in range(y): + obs.append((0, i)) + for i in range(y): + obs.append((x - 1, i)) - for i in range(y_range): - obs.append((0, i)) - for i in range(y_range): - obs.append((x_range - 1, i)) + return obs - return obs + def lose_map(self): + """ + Initialize losing states' positions + :return: losing states + """ + lose = [] + for i in range(2, 12): + lose.append((i, 1)) -def lose_map(): - """ - Initialize losing states' positions - :return: losing states - """ + return lose - lose = [] - for i in range(2, 12): - lose.append((i, 1)) + def state_space(self): + """ + generate state space + :return: state space + """ - return lose + state_space = [] + for i in range(self.x_range): + for j in range(self.y_range): + if (i, j) not in self.obs: + state_space.append((i, j)) + return state_space -def get_reward(x_next, lose): - """ - calculate reward of next state - - :param x_next: next state - :return: reward - """ - - if x_next in lose: - return -100 # reward : -100, for lose states - return -1 # reward : -1, for other states + def get_reward(self, x_next): + """ + calculate reward of next state + :param x_next: next state + :return: reward + """ + if x_next in self.lose: + return -100 # reward : -100, for lose states + return -1 # reward : -1, for other states diff --git a/Model-free Control/motion_model.py b/Model-free Control/motion_model.py index 8c9cfd1..a1d6372 100644 --- a/Model-free Control/motion_model.py +++ b/Model-free Control/motion_model.py @@ -3,37 +3,42 @@ """ @author: huiming zhou """ -import numpy as np -motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets +import env + +class Motion_model(): + def __init__(self, xI, xG): + self.env = env.Env(xI, xG) + self.obs = self.env.obs_map() -def move_prob(x, u, obs, eta = 0.2): - """ - Motion model of robots, + def move_next(self, x, u, 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 - """ + :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) + 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: - x_next.append((x[0] + act[0], x[1] + act[1])) + u_real = [(1, 0), (0, 1), (0, -1)] - return x_next, p_next \ No newline at end of file + for act in u_real: + x_check = (x[0] + act[0], x[1] + act[1]) + if x_check in self.obs: + x_next.append(x) + else: + x_next.append(x_check) + + return x_next, p_next \ No newline at end of file diff --git a/Model-free Control/plotting.py b/Model-free Control/plotting.py new file mode 100644 index 0000000..44c038d --- /dev/null +++ b/Model-free Control/plotting.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import matplotlib.pyplot as plt +import env + +class Plotting(): + def __init__(self, xI, xG): + self.xI, self.xG = xI, xG + self.env = env.Env(self.xI, self.xG) + self.obs = self.env.obs_map() + self.lose = self.env.lose_map() + + + def animation(self, path, name): + """ + animation. + + :param path: optimal path + :param name: tile of figure + :return: an animation + """ + + plt.figure(1) + self.plot_grid(name) + self.plot_lose() + self.plot_path(path) + + + def plot_grid(self, name): + """ + plot the obstacles in environment. + + :param name: title of figure + :return: plot + """ + + obs_x = [self.obs[i][0] for i in range(len(self.obs))] + obs_y = [self.obs[i][1] for i in range(len(self.obs))] + + plt.plot(self.xI[0], self.xI[1], "bs", ms = 24) + plt.plot(self.xG[0], self.xG[1], "gs", ms = 24) + + plt.plot(obs_x, obs_y, "sk", ms = 24) + plt.title(name) + plt.axis("equal") + + + def plot_lose(self): + """ + plot losing states in environment. + :return: a plot + """ + + lose_x = [self.lose[i][0] for i in range(len(self.lose))] + lose_y = [self.lose[i][1] for i in range(len(self.lose))] + + plt.plot(lose_x, lose_y, color = '#A52A2A', marker = 's', ms = 24) + + + def plot_visited(self, visited): + """ + animation of order of visited nodes. + + :param visited: visited nodes + :return: animation + """ + + visited.remove(self.xI) + count = 0 + + for x in visited: + count += 1 + 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 count < len(visited) / 3: + length = 15 + elif count < len(visited) * 2 / 3: + length = 30 + else: + length = 45 + + if count % length == 0: plt.pause(0.001) + + + def plot_path(self, path): + path.remove(self.xI) + path.remove(self.xG) + + for x in path: + plt.plot(x[0], x[1], color='#808080', marker='o', ms = 23) + plt.gcf().canvas.mpl_connect('key_release_event', lambda event: + [exit(0) if event.key == 'escape' else None]) + plt.pause(0.001) + plt.show() + plt.pause(0.5) + + + def plot_diff(self, diff, name): + plt.figure(2) + plt.title(name, fontdict=None) + plt.xlabel('iterations') + plt.ylabel('difference of successive iterations') + plt.grid('on') + + count = 0 + for x in diff: + plt.plot(count, x, color='#808080', marker='o') # plot dots for animation + plt.gcf().canvas.mpl_connect('key_release_event', lambda event: + [exit(0) if event.key == 'escape' else None]) + plt.pause(0.07) + count += 1 + + plt.plot(diff, color='#808080') + plt.pause(0.01) + plt.show() diff --git a/Model-free Control/tools.py b/Model-free Control/tools.py deleted file mode 100644 index c862a65..0000000 --- a/Model-free Control/tools.py +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - -import matplotlib.pyplot as plt - - -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): - """ - Plot the path. - - :param xI: Starting node - :param xG: Goal node - :param path: Planning path - :return: A plot - """ - - 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 show_map(xI, xG, obs_map, lose_map, name): - """ - Plot the background you designed. - - :param xI: starting state - :param xG: goal states - :param obs_map: positions of obstacles - :param lose_map: positions of losing state - :param name: name of this figure - :return: a figure - """ - - 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", ms = 24) # plot starting state (blue) - plt.plot(xG[0], xG[1], "gs", ms = 24) # plot goal states (green) - - plt.plot(obs_x, obs_y, "sk", ms = 24) # plot obstacles (black) - plt.plot(lose_x, lose_y, marker = 's', color = '#A52A2A', ms = 24) # plot losing states (grown) - plt.title(name, fontdict=None) - plt.axis("equal") - - -def plot_dots(x): - """ - Plot state x for animation - - :param x: current node - :return: a plot - """ - - plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o', ms = 23) # plot dots for animation - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - plt.pause(0.1) - - diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index c26564b..6be7330 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -2,21 +2,17 @@ + + + + + - - - - - - - - - - + diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index 7445e09..892e773 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -12,16 +12,16 @@ class Astar: def __init__(self, x_start, x_goal, heuristic_type): self.xI, self.xG = x_start, x_goal - self.Env = env.Env() - self.plotting = plotting.Plotting(self.xI, self.xG) + self.Env = env.Env() # class Env + self.plotting = plotting.Plotting(self.xI, self.xG) # class Plotting - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG, heuristic_type) self.fig_name = "A* Algorithm" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG, heuristic_type): @@ -31,26 +31,26 @@ class Astar: :return: planning path, action in each node, visited nodes in the planning process """ - q_astar = queue.QueuePrior() # priority queue + q_astar = queue.QueuePrior() # priority queue q_astar.put(xI, 0) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes visited = [] cost = {xI: 0} while not q_astar.empty(): x_current = q_astar.get() - if x_current == xG: # stop condition + if x_current == xG: # stop condition break visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node + 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: 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 + 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, xG, 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], action[x_next] = x_current, u_next [path, policy] = self.extract_path(xI, xG, parent, action) @@ -113,6 +113,7 @@ class Astar: if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node + astar = Astar(x_Start, x_Goal, "manhattan") \ No newline at end of file diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index 4977937..991912d 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -15,13 +15,13 @@ class BFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) self.fig_name = "Breadth-first Searching" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG): @@ -31,10 +31,10 @@ class BFS: :return: planning path, action in each node, visited nodes in the planning process """ - q_bfs = queue.QueueFIFO() # first-in-first-out queue + q_bfs = queue.QueueFIFO() # first-in-first-out queue q_bfs.put(xI) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes visited = [] while not q_bfs.empty(): @@ -42,13 +42,13 @@ class BFS: if x_current == xG: break visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node + 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 + 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], action[x_next] = x_current, u_next - [path, policy] = self.extract_path(xI, xG, parent, action) # extract path + [path, policy] = self.extract_path(xI, xG, parent, action) # extract path return path, policy, visited diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py index d7afd81..435512d 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -15,13 +15,13 @@ class DFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) self.fig_name = "Depth-first Searching" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG): diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 847e6e7..290d063 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -15,13 +15,13 @@ class Dijkstra: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) self.fig_name = "Dijkstra's Algorithm" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG): diff --git a/Search-based Planning/queue.py b/Search-based Planning/queue.py index 8a5f398..8b41446 100644 --- a/Search-based Planning/queue.py +++ b/Search-based Planning/queue.py @@ -3,7 +3,6 @@ """ @author: Huiming Zhou -@description: this file defines three kinds of queues that will be used in algorithms. """ import collections