update

Search-based Planning/.idea/workspace.xml

@@ -2,18 +2,16 @@
 <project version="4">
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/motion_model.py" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/env.py" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/motion model.py" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/value_iteration.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/motion_model.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/environment.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/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/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" />
     <option name="SHOW_DIALOG" value="false" />
@@ -52,7 +50,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.dijkstra">
+  <component name="RunManager" selected="Python.dfs">
     <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -160,10 +158,10 @@
     </configuration>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.dijkstra" />
         <item itemvalue="Python.dfs" />
         <item itemvalue="Python.bfs" />
         <item itemvalue="Python.a_star" />
+        <item itemvalue="Python.dijkstra" />
         <item itemvalue="Python.searching" />
       </list>
     </recent_temporary>

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

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)

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)

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)

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")
+

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()
+

Stochastic Shortest Path/motion model.py

@@ -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

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

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)