From 7f38cc3a2186ca72f1e61af2d1d1de790297e290 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Fri, 19 Jun 2020 17:25:37 -0700 Subject: [PATCH] update --- .../.idea/dictionaries/Huiming_Zhou.xml | 7 + Model-free Control/Q-learning.py | 165 +++++++++++++++++ Model-free Control/Sarsa.py | 166 ++++++++++++++++++ .../__pycache__/env.cpython-37.pyc | Bin 0 -> 1083 bytes .../__pycache__/motion_model.cpython-37.pyc | Bin 0 -> 974 bytes .../__pycache__/tools.cpython-37.pyc | Bin 0 -> 3276 bytes Model-free Control/env.py | 31 +--- Model-free Control/motion_model.py | 1 + Model-free Control/tools.py | 12 +- 9 files changed, 352 insertions(+), 30 deletions(-) create mode 100644 Model-free Control/.idea/dictionaries/Huiming_Zhou.xml create mode 100644 Model-free Control/Q-learning.py create mode 100644 Model-free Control/__pycache__/env.cpython-37.pyc create mode 100644 Model-free Control/__pycache__/motion_model.cpython-37.pyc create mode 100644 Model-free Control/__pycache__/tools.cpython-37.pyc diff --git a/Model-free Control/.idea/dictionaries/Huiming_Zhou.xml b/Model-free Control/.idea/dictionaries/Huiming_Zhou.xml new file mode 100644 index 0000000..a1d33a5 --- /dev/null +++ b/Model-free Control/.idea/dictionaries/Huiming_Zhou.xml @@ -0,0 +1,7 @@ + + + + sarsa + + + \ No newline at end of file diff --git a/Model-free Control/Q-learning.py b/Model-free Control/Q-learning.py new file mode 100644 index 0000000..b942ea7 --- /dev/null +++ b/Model-free Control/Q-learning.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import env +import tools +import motion_model + +import matplotlib.pyplot as plt +import numpy as np +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 + self.gamma = 0.9 # discount factor + self.alpha = 0.5 + self.epsilon = 0.1 + 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.name2 = "convergence of error" + + + def Monte_Carlo(self): + """ + Monte_Carlo experiments + + :return: Q_table, policy + """ + + Q_table = self.table_init() + policy = {} + count = 0 + + for k in range(self.M): + count += 1 + x = self.state_init() + while x != self.xG: + u = self.epsilon_greedy(int(np.argmax(Q_table[x])), self.epsilon) + x_next = self.move_next(x, self.u_set[u]) + reward = env.get_reward(x_next, self.lose) + Q_table[x][u] = (1 - self.alpha) * Q_table[x][u] + \ + self.alpha * (reward + self.gamma * max(Q_table[x_next])) + x = x_next + + for x in Q_table: + policy[x] = int(np.argmax(Q_table[x])) + + return Q_table, policy + + + def table_init(self): + """ + Initialize Q_table: Q(s, a) + :return: Q_table + """ + + 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 + + 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) + if (i, j) not in self.obs: + return (i, j) + + + def epsilon_greedy(self, u, error): + """ + generate a policy using epsilon_greedy algorithm + + :param u: original input + :param error: epsilon value + :return: epsilon policy + """ + + if np.random.random_sample() < 3 / 4 * error: + u_e = u + while u_e == u: + p = np.random.random_sample() + if p < 0.25: u_e = 0 + elif p < 0.5: u_e = 1 + elif p < 0.75: u_e = 2 + else: u_e = 3 + return u_e + return u + + + def move_next(self, x, u): + """ + get next state. + + :param x: current state + :param u: input + :return: next state + """ + + x_next = (x[0] + u[0], x[1] + u[1]) + if x_next in self.obs: + return x + return x_next + + + def simulation(self, xI, xG, policy): + """ + 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 = 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 + else: + x = x_next + if x_next == xG: + break + else: + tools.plot_dots(x) # each state in optimal path + path.append(x) + plt.show() + + return path + + +if __name__ == '__main__': + x_Start = (1, 1) + 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 e69de29..01f9ffa 100644 --- a/Model-free Control/Sarsa.py +++ b/Model-free Control/Sarsa.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import env +import tools +import motion_model + +import matplotlib.pyplot as plt +import numpy as np +import sys + + +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 + self.gamma = 0.9 # discount factor + self.alpha = 0.5 + self.epsilon = 0.1 + self.obs = env.obs_map() # position of obstacles + self.lose = env.lose_map() # position of lose states + self.name1 = "SARSA, M=" + str(self.M) + self.name2 = "convergence of error" + + + def Monte_Carlo(self): + """ + Monte_Carlo experiments + + :return: Q_table, policy + """ + + Q_table = self.table_init() + policy = {} + count = 0 + + for k in range(self.M): + count += 1 + x = self.state_init() + u = self.epsilon_greedy(int(np.argmax(Q_table[x])), self.epsilon) + while x != self.xG: + x_next = self.move_next(x, self.u_set[u]) + reward = env.get_reward(x_next, self.lose) + 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]) + x, u = x_next, u_next + + for x in Q_table: + policy[x] = int(np.argmax(Q_table[x])) + + return Q_table, policy + + + def table_init(self): + """ + Initialize Q_table: Q(s, a) + :return: Q_table + """ + + 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 + + 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) + if (i, j) not in self.obs: + return (i, j) + + + def epsilon_greedy(self, u, error): + """ + generate a policy using epsilon_greedy algorithm + + :param u: original input + :param error: epsilon value + :return: epsilon policy + """ + + if np.random.random_sample() < 3 / 4 * error: + u_e = u + while u_e == u: + p = np.random.random_sample() + if p < 0.25: u_e = 0 + elif p < 0.5: u_e = 1 + elif p < 0.75: u_e = 2 + else: u_e = 3 + return u_e + return u + + + def move_next(self, x, u): + """ + get next state. + + :param x: current state + :param u: input + :return: next state + """ + + x_next = (x[0] + u[0], x[1] + u[1]) + if x_next in self.obs: + return x + return x_next + + + def simulation(self, xI, xG, policy): + """ + 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 = 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 + else: + x = x_next + if x_next == xG: + break + else: + tools.plot_dots(x) # each state in optimal path + path.append(x) + plt.show() + + return path + + +if __name__ == '__main__': + x_Start = (1, 1) + 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 new file mode 100644 index 0000000000000000000000000000000000000000..fb6bf059834e720c1fef8cf87fb184434c988721 GIT binary patch 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 literal 0 HcmV?d00001 diff --git a/Model-free Control/__pycache__/motion_model.cpython-37.pyc b/Model-free Control/__pycache__/motion_model.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5a5a3aa6ef2eba708c717507fb51cbd5233479dd GIT binary patch literal 974 zcmaJ1jS5VpNPciCJh2?&H-AjBa^k#dC~N>b2-1_5Gqu=1UKcdP8$i|s|1Xj35Z z5~PBXM?s>aThURV6ubZpjCb#ps~BlMkG~mvW<1{C+KLd!`=j4)es~D|beomee`4!05PaJvKCMpIIg<4~X1!6)#1#u&YwcJ}|yba5|H9n}8RylD{JmYturo}W(8fubW&+8q@~6(E9hMb;luTiDyEq%E4$9Av56^wWs_Xc zf|ct|VOdP2%8Y=6kp8lYE=mhqX$c`>vCL{HlblWz6k>XZEB$){gaJ zeYQ*6jd(SGN3HDkk6b!&6SA@tk`Mb0UNpFBJZPZtpxPuXQ&&tAF4{E-t5ge?Cmx&F zFGvsI)<3Tf;?Z*>v>82Fe#r#BA&+?hlnVlDAbmX4@!Bh)D z50tc8<)iLN(snRA(qsCOcD4R1mwa}xkr$9Dw5@*)w%}aTe+cQTTs#V3XF>o-M+V+MJ$mzZ literal 0 HcmV?d00001 diff --git a/Model-free Control/__pycache__/tools.cpython-37.pyc b/Model-free Control/__pycache__/tools.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f12ddbf738d9bb72c347a93ad5769046457a8a8d GIT binary patch literal 3276 zcmb_eOOG5i5Vrd@J+qHIm9R+&4T!@WvI+tuG>VqIP{<+6iUbg?fl*J}Gt;|0?TPKl z&W?I7BvSU01OH(6n7_lZ4_r9qzyWbvaiGfc&W1~rZrQGOwaZnmugZ0MWu-#k`Sgds zeqliUi64{40^%k#^*MBmgoIIMrL=2>HYLnv&NC7^%w-;oE-Nt~Mvs+Q1;!Gy9+K+P z@M7g=)Yly$YeA=JBK9qjwmI$3zUts=F7{;jwLT_k;fN)B${d~cb?Lx)KD%`EmVs}U;wEE z@O(YcGKzJeJ3Qz`x)U@b#aSRSGb4E#=|p6zlk@`73NpdCTCbRuwO%BnZZNoC3m)o7 zYBUY`3(R}9;GT%m6AW0(GhGY5o8O?x^)BbxXqS00pdczy3vL-A$T(-9WJ|~Zw9izF z%V4rt?}~W=lIygiAPqYN@aW_o3Y@ zypghDX{<(A#`TS66z_y(m{VhN(1`mI&3Z|s4=z0Z?oO@#y#jIdZ^o_J+7IArsx{s`>*s#q?TBavt0fnU*50Yavp#-rFmd`N&}nV*{e)JalBqqZR5< zi~2CGQlEI1L}14@a^Z2IA;|DsN$3s;0LOsFP!!?woU|xn{F#q{IUop_yU_33hGV~h z<7j&uVd^jw39R(Or9|6@F7r_$P~H)NXek5iD#KsRsc)rX2Do>FCf%t8TPNIGn|S{g z_+DtUc?`%m4@NhJ+&O6KJ?PqS9zUPnrw0&xYY^{i27 zaL@&2R+fQ(T)BY|ibc1#aSdhS1|At+Q5~_n1;4J}NJSi_Y6FPLk8h*Rt2u4t5^A;# z9ca0NG$POFBUgc1hc;HEpkEYJ9@A?8`f{3Nd^cgbQ@F86g$&J%(t9ay|NHXq-ybx$ zzS@Ks@FeewC%kYW%-X!LQ=S!eFV$c?9BAS8qQ2sVgC|3S z101V~Csz2AqxlfN6zWSV6#(!hc+bNqufR-mtW9Kz0m}ku!sM$#C2^aC~iT+c{y7W1%krG<;0*FC2h3a;V`@!&zG3xVguc7iLPi z&sNwf%$+iUVnOgY>@+*`!a`{fHfJ$x*lB>>x#96*xM0RiZ;EgD3zx``UvvY zyfX6r$^0xzZ_N~Wr*``jN4;jasWL|C>8^TE-{(YM?DJ%0U5dojlpQx3q5mSkg?1-&RCW-cA1s4w1rr~x^Q&= zE6*H2u~MrA;{s)_9GeB-^oE@I zsi2=t9a2ysKLEX}j>H>tp< zfkuXx(Ut}iTFpoE=&!l!&6@!xwA*nj^x`Oc5~*;h+e;g<$TA*lc?l!}PU+zFjmy(r zzJ*^DYDE02bQv!$ymza9QEGq|aidY30e|4{Y0`u$4k;)r%{bBlYc+G*8Pu5NbKA`u Y+ZC*z@(sKc6|QouW!t~#pYt#O2i>{(6951J literal 0 HcmV?d00001 diff --git a/Model-free Control/env.py b/Model-free Control/env.py index 3c2781c..bcba4d8 100644 --- a/Model-free Control/env.py +++ b/Model-free Control/env.py @@ -4,7 +4,7 @@ @author: huiming zhou """ -x_range, y_range = 51, 31 # size of background +x_range, y_range = 14, 6 # size of background def obs_map(): @@ -25,16 +25,6 @@ def obs_map(): for i in range(y_range): obs.append((x_range - 1, i)) - for i in range(10, 21): - obs.append((i, 15)) - for i in range(15): - obs.append((20, i)) - - for i in range(15, 30): - obs.append((30, i)) - for i in range(16): - obs.append((40, i)) - return obs @@ -45,13 +35,13 @@ def lose_map(): """ lose = [] - for i in range(25, 36): - lose.append((i, 13)) + for i in range(2, 12): + lose.append((i, 1)) return lose -def get_reward(x_next, xG, lose): +def get_reward(x_next, lose): """ calculate reward of next state @@ -59,13 +49,8 @@ def get_reward(x_next, xG, lose): :return: reward """ - reward = [] - for x in x_next: - if x in xG: - reward.append(10) # reward : 10, for goal states - elif x in lose: - reward.append(-10) # reward : -10, for lose states - else: - reward.append(0) # reward : 0, for other states + if x_next in lose: + return -100 # reward : -100, for lose states + return -1 # reward : -1, for other states + - return reward \ No newline at end of file diff --git a/Model-free Control/motion_model.py b/Model-free Control/motion_model.py index dfa6e27..8c9cfd1 100644 --- a/Model-free Control/motion_model.py +++ b/Model-free Control/motion_model.py @@ -7,6 +7,7 @@ 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, diff --git a/Model-free Control/tools.py b/Model-free Control/tools.py index 5ef1d54..bd5a53c 100644 --- a/Model-free Control/tools.py +++ b/Model-free Control/tools.py @@ -67,13 +67,11 @@ def show_map(xI, xG, obs_map, lose_map, name): 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") # plot starting state (blue) + 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) - for x in xG: - plt.plot(x[0], x[1], "gs") # plot goal states (green) - - plt.plot(obs_x, obs_y, "sk") # plot obstacles (black) - plt.plot(lose_x, lose_y, marker = 's', color = '#A52A2A') # plot losing states (grown) + plt.plot(obs_x, obs_y, "sk", ms = 24) # plot obstacles (black) + plt.plot(lose_x, lose_y, marker = 's', color = '#808080', ms = 24) # plot losing states (grown) plt.title(name, fontdict=None) plt.axis("equal") @@ -86,7 +84,7 @@ def plot_dots(x): :return: a plot """ - plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o') # plot dots for animation + plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o', ms = 24) # 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.001)