From d4e622da39a2d5688dafdcbce6c82db9353017e8 Mon Sep 17 00:00:00 2001
From: zhm-real <zhou.hm0420@gmail.com>
Date: Thu, 18 Jun 2020 21:17:57 -0700
Subject: [PATCH] update

---
 Search-based Planning/.idea/workspace.xml     |   9 +-
 .../__pycache__/env.cpython-37.pyc            | Bin 2000 -> 1021 bytes
 .../__pycache__/tools.cpython-37.pyc          | Bin 1984 -> 2955 bytes
 Stochastic Shortest Path/env.py               |  18 +--
 Stochastic Shortest Path/policy_iteration.py  | 125 ++++++++++++++++++
 Stochastic Shortest Path/tools.py             |  26 +++-
 Stochastic Shortest Path/value_iteration.py   |  91 +++++++++++--
 7 files changed, 231 insertions(+), 38 deletions(-)
 create mode 100644 Stochastic Shortest Path/policy_iteration.py

diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 77f181c..51a21ab 100644
--- a/Search-based Planning/.idea/workspace.xml	
+++ b/Search-based Planning/.idea/workspace.xml	
@@ -2,15 +2,10 @@
 <project version="4">
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/motion_model.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/policy_iteration.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/a_star.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/bfs.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/dfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/dfs.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/dijkstra.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/env.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/env.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/motion model.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/tools.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/tools.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/value_iteration.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/value_iteration.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
diff --git a/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc
index 9a289c4ae3024a31aef2bf0d4a02091fcdbfc154..493f4761b05c0b90603a6a30288b1434787ef23c 100644
GIT binary patch
delta 258
zcmcb>|CgQ5iI<m)0SFGLyou{&p2#P`s54Prhm|RqL6c)+!gofVTg>@M#kUwUS27d{
zO-^Q-#3(ZP8<TgvBv8*w6%fG-Bs7_dcz{$9ADDfMB`3c)wMY;oA_OOdfvj5`Aes2w
z!~!iKub2Z!Ffg((3NQ*W7J*FE<haEiAD@z+93Ov6FgLNJASb^hCo@T}pc2Ho#RjFB
v3tV$bia-_@@q-8v5CPJUWF6RIh@CdM`6;D2sdlVD6<|AAn0S~tm^kDCg;Y8r

literal 2000
zcmbtV&2Ah;5bo}szunl5ZGvNmC>e!FVL4b}2nWluViJkuVj)b3)Y`BbPmgyeo}C$Y
z_rls*pByQ<<iL$%eB=R;;8o@brxkBtE>!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*&gZ<PmIxaU0xcVbag9=r=4>J$Ch
zGcqLRY9BXz7*Enz$3YSwMSixcbPy(yy65Mag5@k#4P&$}Bi)zjy59?OKiivH8m)vo
zCL_ZT45RmvT}5^c*)3%A$UXwrv<h1WX(uY&LA#tdL7qpcDBQzxp$PziTnZZhLgWJM
zIs5M6dh4l*q-s5`K)?Y6tw)jC*IC}m1KsuWBuG=lN04+f8S8FOwRUtCb_1p3(BA<J
z=}2k+YcOs_=?|;<Vc|lUHk|1S1W|W^5!a$~bP;HYjpe-gUSwk_-vhe~XanGvX!{r@
z25nensQtMb0a8S*s`7kQ18aNq0qhvvK{gAl$>a<OIcMhFN-{V!UY)syk}bk#;h_dI
z$B4=UMyN$GLwOzMA!dZ54uo%^2BiJ~_KNNS?ge}4A>LoZi$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<N
zui;TbQe=hv#11UjL=A||4L4cARzES<4$HW8dhutCjU-k&%zF9e8ageT&CyNO&3?ps
ztZpP(7$j;FCS${QN(a-f(vg_?<@v}JP+<$K5_zXkcw3i+LL^O(ZbIGvpP~NiRF{PM
zVj8Nf4esS^S8>(>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+Lm<q$Rpk^_>Q7P>KJuX1#FRZIOlT
zHecuk8dFW;U1(c4<4!5%O)!<%J0y18X`;qQSv_o_(>fO3MlTcnB-+F#p|E3HY=(Kv
IyY0FE05Hgy00000

diff --git a/Stochastic Shortest Path/__pycache__/tools.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/tools.cpython-37.pyc
index 816b7b4974a56f520558e13935076613715800c6..ca2d9cabb6755709dac4bdf05566d8479e85f5e7 100644
GIT binary patch
delta 1038
zcmZ`&%}*0S6rY)$ot@o&P*f0*D3On3jU-BJG{gg-F`m3gqBc=dIt3~%g_%_gDd~YG
zkQhwN9=yWEyD{-#IQVx=;?08xW8%rH?`;92;%wf>yx;qndGq#D*UPinnM@`iuzrqw
zd-B73o$Z^s2|Gm!L{Y`6w8jb!?dZI~m7{o@6kO#h0a7SWNsylMRRB^dcAtc)cMnU9
z2Ja8p8%o1?mhaF@@n`<FbpbM)ELM{N4d?)TXF%%nU@5t1hlQsRKn{@k3T&G$P&`Xp
zMkJy;bPK-QWQ%UP3F4f9h;0Hf{1^dmh-qPvmq6}c5F#>Sdrp%m(NBPT{Y1%qxO(Ga
z6Usdv*z^<JPjElMeZ<$=4@iQzO`!s`MC_^4^qNwoR9a<rd1B#BKCFn!?lRo&xvFcQ
zCYn$=)D(fvb>EA>J5%uiPmf!#K5yn~#Z~P?4>Wc~cLBQ!WEe0(HU`a7{RR5OOZ@J2
zokJh=4w+7)o<W^zZ2MnXCct!<<*4F_mDRc_pHTAJAI|yK|LP3=TPOcJR8xd8*0OnH
z*&>ixw$yBR!_)bR>51u(Sy3zM<+9d2;1*IV7wRjKs+6Kx>z3D^6|14A`&yW%AK3u1
zbHJ<{RibLy@<m-ymKPfp69yV5O5-SXIxvKee5F_`7hLRZqa)S}BDr_tP_A3rIO-yX
zbb_(}Rwn(Pf`(s#t?2_saEcjHFN29@S$c|Qai+%sE8&%Kq4a=m0hy`zgPYGq1ZE&O
ze)&}Fi7{7?f<v4aZF(pEC`RKiB7cMG%V6xh-|;9|f3;eV=2Sg0Nlpnagbva)mYxJX
n^y5cf*lV5g%5$B<1o)FBr_z)0D{m}^6T(@VN+x?`w><p|Nx01X

delta 322
zcmeAcKfurD#LLUY00ftwy^g!iI+0JpNC(JEVTfW#VN7AlVa#QUVrFDWVNPLbVTfW$
zVNGEJ(yS@$DI7qWErltVL6dW1Mlus4=jJm^(-|2xCa17;FltVI%d*T^4ro#_AINkL
zE=Dd!DIgR9(nTQgWG0X}5P)cQAa(}gVxP(VoTf50j451VK>M?qiabgf7O<vpFJuI|
zfdOb2&*UqdY8<!NaubWPQ;R15=WJm#n%u&r%KDNYsDJWGE>j1lTZ|RA*m6?y(n~Um
zIDw)N0_4kEoCP`gCGjcwCB^z6>%cx|lwsrnx{8Of2qdP-K3R-gQ?Q5|q!Hv$uxJtI
Y<cD0slXJQGcsPJ!EKEF19HJb803DA%!T<mO

diff --git a/Stochastic Shortest Path/env.py b/Stochastic Shortest Path/env.py
index ba435c3..6663b68 100644
--- a/Stochastic Shortest Path/env.py	
+++ b/Stochastic Shortest Path/env.py	
@@ -41,24 +41,8 @@ def obs_map():
 
 def lose_map():
     lose = []
-    for i in range(27, 34):
+    for i in range(25, 36):
         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/policy_iteration.py b/Stochastic Shortest Path/policy_iteration.py
new file mode 100644
index 0000000..06c5306
--- /dev/null
+++ b/Stochastic Shortest Path/policy_iteration.py	
@@ -0,0 +1,125 @@
+#!/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 copy
+import sys
+
+
+class Policy_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.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.name2 = "convergence of error"
+
+
+    def policy_evaluation(self, policy, value):
+        delta = sys.maxsize
+
+        while delta > self.e:
+            x_value = 0
+            for x in value:
+                if x in self.xG: continue
+                else:
+                    [x_next, p_next] = motion_model.move_prob(x, policy[x], self.obs)
+                    v_Q = self.cal_Q_value(x_next, p_next, value)
+                    v_diff = abs(value[x] - v_Q)
+                    value[x] = v_Q
+                    if v_diff > 0:
+                        x_value = max(x_value, v_diff)
+            delta = x_value
+        return value
+
+
+    def policy_improvement(self, policy, value):
+        for x in value:
+            if x in self.xG: continue
+            else:
+                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))
+                policy[x] = self.u_set[int(np.argmax(value_list))]
+        return policy
+
+
+    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
+                    policy[(i, j)] = self.u_set[0]
+
+        while True:
+            policy_back = copy.deepcopy(policy)
+            value_table = self.policy_evaluation(policy, value_table)
+            policy = self.policy_improvement(policy, value_table)
+            if policy_back == policy: break
+        return value_table, policy
+
+
+    def simulation(self, xI, xG, policy):
+        path = []
+        x = xI
+        while x not in xG:
+            u = policy[x]
+            x_next = (x[0] + u[0], x[1] + u[1])
+            if x_next not in self.obs:
+                x = x_next
+                path.append(x)
+        path.pop()
+        return path
+
+
+    def animation(self, path):
+        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()
+
+
+    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
+
+    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
+
+
+if __name__ == '__main__':
+    x_Start = (5, 5)
+    x_Goal = [(49, 5), (49, 25)]
+
+    PI = Policy_iteration(x_Start, x_Goal)
+    [value_PI, policy_PI] = PI.iteration()
+    path_PI = PI.simulation(x_Start, x_Goal, policy_PI)
+
+    PI.animation(path_PI)
diff --git a/Stochastic Shortest Path/tools.py b/Stochastic Shortest Path/tools.py
index 2de4a72..ebd5300 100644
--- a/Stochastic Shortest Path/tools.py	
+++ b/Stochastic Shortest Path/tools.py	
@@ -5,6 +5,7 @@
 """
 
 import matplotlib.pyplot as plt
+import env
 
 def extract_path(xI, xG, parent, actions):
     """
@@ -44,10 +45,29 @@ def showPath(xI, xG, path):
     plt.show()
 
 
-def plot_dots(x, length):
+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")
+
+    for x in xG:
+        plt.plot(x[0], x[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")
+
+
+def plot_dots(x):
     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)
+    plt.pause(0.001)
+
 
diff --git a/Stochastic Shortest Path/value_iteration.py b/Stochastic Shortest Path/value_iteration.py
index 7dd5a7d..b6685fb 100644
--- a/Stochastic Shortest Path/value_iteration.py	
+++ b/Stochastic Shortest Path/value_iteration.py	
@@ -8,37 +8,106 @@ import env
 import tools
 import motion_model
 
+import matplotlib.pyplot as plt
 import numpy as np
-import copy
+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.T = 500
+        self.e = 0.001
         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)
+        self.name1 = "value_iteration, e=" + str(self.e) + ", gamma=" + str(self.gamma)
+        self.name2 = "convergence of error"
 
-        env.show_map(self.xI, self.xG, self.obs, self.lose, self.name)
 
     def iteration(self):
         value_table = {}
         policy = {}
+        diff = []
+        delta = sys.maxsize
 
         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:
-                
+        while delta > self.e:
+            x_value = 0
+            for x in value_table:
+                if x in self.xG: continue
+                else:
+                    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)
+                    if v_diff > 0:
+                        x_value = max(x_value, v_diff)
+            delta = x_value
+            diff.append(delta)
+        return value_table, policy, diff
+
+
+    def simulation(self, xI, xG, policy):
+        path = []
+        x = xI
+        while x not in xG:
+            u = policy[x]
+            x_next = (x[0] + u[0], x[1] + u[1])
+            if x_next not in self.obs:
+                x = x_next
+                path.append(x)
+        path.pop()
+        return path
+
+
+    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.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
+
+
+    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
 
 
 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
+    x_Start = (5, 5)
+    x_Goal = [(49, 5), (49, 25)]
+
+    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)
+
+    VI.animation(path_VI, diff_VI)