update searchi-based

Stochastic Shortest Path/.idea/.gitignore

@@ -0,0 +1,3 @@
+
+# Default ignored files
+/workspace.xml

Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+  <component name="TestRunnerService">
+    <option name="projectConfiguration" value="pytest" />
+    <option name="PROJECT_TEST_RUNNER" value="pytest" />
+  </component>
+</module>

Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

Stochastic Shortest Path/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
+</project>

Stochastic Shortest Path/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/Stochastic Shortest Path.iml" filepath="$PROJECT_DIR$/.idea/Stochastic Shortest Path.iml" />
+    </modules>
+  </component>
+</project>

Stochastic Shortest Path/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

Stochastic Shortest Path/environment.py

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

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+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
+    :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, visited, name):
+    """
+    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)
+    plt.show()
+