update

Search-based Planning/.idea/workspace.xml

@@ -2,14 +2,18 @@
 <project version="4">
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/env.py" afterDir="false" />
+      <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 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$/environment.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/tools.py" beforeDir="false" afterPath="$PROJECT_DIR$/tools.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" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -48,7 +52,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.dfs">
+  <component name="RunManager" selected="Python.dijkstra">
     <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -156,10 +160,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>
@@ -182,7 +186,9 @@
       <map>
         <entry key="MAIN">
           <value>
-            <State />
+            <State>
+              <option name="COLUMN_ORDER" />
+            </State>
           </value>
         </entry>
       </map>

Search-based Planning/a_star.py

@@ -5,18 +5,20 @@
 """
 
 import queue
-import env
 import tools
 import env
+import motion_model
 
 class Astar:
     def __init__(self, x_start, x_goal, x_range, y_range, heuristic_type):
-        self.u_set = env.motions  # feasible input set
+        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(self.xI, self.xG, "a_star searching")  # position of obstacles
+        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")
+
     def searching(self):
         """
         Searching using A_star.
@@ -27,7 +29,7 @@ class Astar:
         q_astar = queue.QueuePrior()                            # priority queue
         q_astar.put(self.xI, 0)
         parent = {self.xI: self.xI}                             # record parents of nodes
-        action = {self.xI: (0, 0)}                             # record actions of nodes
+        action = {self.xI: (0, 0)}                              # record actions of nodes
         cost = {self.xI: 0}
 
         while not q_astar.empty():
@@ -43,7 +45,7 @@ class Astar:
                     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, self.xG, self.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] = x_current
                         action[x_next] = u_next
         [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, action)

Search-based Planning/bfs.py

@@ -7,6 +7,7 @@
 import queue
 import tools
 import env
+import motion_model
 
 class BFS:
     """
@@ -14,10 +15,12 @@ class BFS:
     """
 
     def __init__(self, x_start, x_goal, x_range, y_range):
-        self.u_set = env.motions                                                # feasible input set
+        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(self.xI, self.xG, "breadth-first searching")     # position of obstacles
+        self.obs = env.obs_map()                                      # position of obstacles
+
+        env.show_map(self.xI, self.xG, self.obs, "breadth-first searching")
 
     def searching(self):
         """

Search-based Planning/dfs.py

@@ -7,6 +7,7 @@
 import queue
 import tools
 import env
+import motion_model
 
 class DFS:
     """
@@ -14,10 +15,12 @@ class DFS:
     """
 
     def __init__(self, x_start, x_goal, x_range, y_range):
-        self.u_set = env.motions                                            # feasible input set
+        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(self.xI, self.xG, "depth-first searching")   # position of obstacles
+        self.obs = env.obs_map()                                # position of obstacles
+
+        env.show_map(self.xI, self.xG, self.obs, "depth-first searching")
 
     def searching(self):
         """

Search-based Planning/dijkstra.py

@@ -7,13 +7,16 @@
 import queue
 import env
 import tools
+import motion_model
 
 class Dijkstra:
     def __init__(self, x_start, x_goal, x_range, y_range):
-        self.u_set = env.motions                                                # feasible input set
+        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(self.xI, self.xG, "dijkstra searching")     # position of obstacles
+        self.obs = env.obs_map()                               # position of obstacles
+
+        env.show_map(self.xI, self.xG, self.obs, "dijkstra searching")
 
     def searching(self):
         """
@@ -25,7 +28,7 @@ class Dijkstra:
         q_dijk = queue.QueuePrior()                            # priority queue
         q_dijk.put(self.xI, 0)
         parent = {self.xI: self.xI}                            # record parents of nodes
-        action = {self.xI: (0, 0)}                            # record actions of nodes
+        action = {self.xI: (0, 0)}                             # record actions of nodes
         cost = {self.xI: 0}
 
         while not q_dijk.empty():
@@ -36,7 +39,7 @@ class Dijkstra:
                 tools.plot_dots(x_current, len(parent))
             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:   # node not visited and not in obstacles
+                if x_next not in self.obs:                     # node not visited and not in obstacles
                     new_cost = cost[x_current] + self.get_cost(x_current, u_next)
                     if x_next not in cost or new_cost < cost[x_next]:
                         cost[x_next] = new_cost

Search-based Planning/env.py

@@ -7,15 +7,11 @@
 import matplotlib.pyplot as plt
 
 x_range, y_range = 51, 31                                   # size of background
-motions = [(1, 0), (-1, 0), (0, 1), (0, -1)]                # feasible motion sets
 
-def obs_map(xI, xG, name):
+def obs_map():
     """
     Initialize obstacles' positions
 
-    :param xI: starting node
-    :param xG: goal node
-    :param name: title of figure
     :return: map of obstacles
     """
 
@@ -40,15 +36,16 @@ def obs_map(xI, xG, name):
     for i in range(16):
         obs_map.append((40, i))
 
+    return obs_map
+
+
+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.title(name, fontdict=None)
     plt.grid(True)
     plt.axis("equal")
-
-    return obs_map
-

Search-based Planning/motion_model.py

@@ -0,0 +1,7 @@
+#!/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/env.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+import matplotlib.pyplot as plt
+
+x_range, y_range = 51, 31                                   # size of background
+
+def obs_map():
+    """
+    Initialize obstacles' positions
+
+    :return: map of obstacles
+    """
+
+    obs_map = []
+    for i in range(x_range):
+        obs_map.append((i, 0))
+    for i in range(x_range):
+        obs_map.append((i, y_range-1))
+
+    for i in range(y_range):
+        obs_map.append((0, i))
+    for i in range(y_range):
+        obs_map.append((x_range-1, i))
+
+    for i in range(10, 21):
+        obs_map.append((i, 15))
+    for i in range(15):
+        obs_map.append((20, i))
+
+    for i in range(15, 30):
+        obs_map.append((30, i))
+    for i in range(16):
+        obs_map.append((40, i))
+
+    return obs_map
+
+
+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")

Stochastic Shortest Path/environment.py

@@ -1,49 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: huiming zhou
-"""
-
-import numpy as np
-
-col, row = 50, 30                                               # size of background
-motions = [(1, 0), (-1, 0), (0, 1), (0, -1)]                    # feasible motion sets
-
-
-def obstacles():
-    """
-    Design the obstacles' positions.
-    :return: the map of obstacles.
-    """
-
-    background = [[[1., 1., 1.]
-                   for x in range(col)] for y in range(row)]
-    for j in range(col):
-        background[0][j] = [0., 0., 0.]
-        background[row - 1][j] = [0., 0., 0.]
-    for i in range(row):
-        background[i][0] = [0., 0., 0.]
-        background[i][col - 1] = [0., 0., 0.]
-    for i in range(10, 20):
-        background[15][i] = [0., 0., 0.]
-    for i in range(15):
-        background[row - 1 - i][30] = [0., 0., 0.]
-        background[i + 1][20] = [0., 0., 0.]
-        background[i + 1][40] = [0., 0., 0.]
-    return background
-
-
-def map_obs():
-    """
-    Using a matrix to represent the position of obstacles,
-    which is used for obstacle detection.
-    :return: a matrix, in which '1' represents obstacle.
-    """
-
-    obs_map = np.zeros((col, row))
-    pos_map = obstacles()
-    for i in range(col):
-        for j in range(row):
-            if pos_map[j][i] == [0., 0., 0.]:
-                obs_map[i][j] = 1
-    return obs_map

Stochastic Shortest Path/motion model.py

@@ -0,0 +1,7 @@
+#!/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/tools.py

@@ -5,30 +5,10 @@
 """
 
 import matplotlib.pyplot as plt
-import environment
-
-
-def obs_detect(x, u, obs_map):
-    """
-    Detect if the next state is in obstacles using this input.
-
-    :param x: current state
-    :param u: input
-    :param obs_map: map of obstacles
-    :return: in obstacles: True / not in obstacles: False
-    """
-
-    x_next = [x[0] + u[0], x[1] + u[1]]                   # next state using input 'u'
-    if u not in environment.motions or \
-            obs_map[x_next[0]][x_next[1]] == 1:           # if 'u' is feasible and next state is not in obstacles
-        return True
-    return False
-
 
 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
@@ -47,28 +27,27 @@ def extract_path(xI, xG, parent, actions):
     return list(reversed(path_back)), list(reversed(acts_back))
 
 
-def showPath(xI, xG, path, visited, name):
+def showPath(xI, xG, path):
     """
     Plot the path.
-
     :param xI: Starting node
     :param xG: Goal node
     :param path: Planning path
-    :param visited: Visited nodes
-    :param name: Name of this figure
     :return: A plot
     """
-
-    background = environment.obstacles()
-    fig, ax = plt.subplots()
-    for k in range(len(visited)):
-        background[visited[k][1]][visited[k][0]] = [.5, .5, .5]    # visited nodes: gray color
-    for k in range(len(path)):
-        background[path[k][1]][path[k][0]] = [1., 0., 0.]          # path: red color
-    background[xI[1]][xI[0]] = [0., 0., 1.]                        # starting node: blue color
-    background[xG[1]][xG[0]] = [0., 1., .5]                        # goal node: green color
-    ax.imshow(background)
-    ax.invert_yaxis()                                              # put origin of coordinate to left-bottom
-    plt.title(name, fontdict=None)
+    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 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)
+

Stochastic Shortest Path/value_iteration.py

@@ -0,0 +1,7 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+