update searchi-based

Search-based Planning/.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>

Search-based Planning/.idea/workspace.xml

@@ -1,7 +1,16 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
   <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/env.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" />
+    </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -15,10 +24,15 @@
       </list>
     </option>
   </component>
+  <component name="Git.Settings">
+    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$/.." />
+  </component>
   <component name="ProjectId" id="1dQBIivqkvFljqtAc1O2MqInYWf" />
-  <component name="ProjectLevelVcsManager" settingsEditedManually="true" />
+  <component name="ProjectLevelVcsManager" settingsEditedManually="true">
+    <ConfirmationsSetting value="2" id="Add" />
+  </component>
   <component name="PropertiesComponent">
-    <property name="last_opened_file_path" value="$PROJECT_DIR$/../path planning algorithms/Search-based Planning" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
@@ -34,7 +48,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.a_star">
+  <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="" />
@@ -142,10 +156,10 @@
     </configuration>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.a_star" />
         <item itemvalue="Python.dfs" />
-        <item itemvalue="Python.dijkstra" />
         <item itemvalue="Python.bfs" />
+        <item itemvalue="Python.a_star" />
+        <item itemvalue="Python.dijkstra" />
         <item itemvalue="Python.searching" />
       </list>
     </recent_temporary>
@@ -163,4 +177,15 @@
     </task>
     <servers />
   </component>
+  <component name="Vcs.Log.Tabs.Properties">
+    <option name="TAB_STATES">
+      <map>
+        <entry key="MAIN">
+          <value>
+            <State />
+          </value>
+        </entry>
+      </map>
+    </option>
+  </component>
 </project>

Search-based Planning/a_star.py

@@ -5,17 +5,16 @@
 """
 
 import queue
-import environment
+import env
 import tools
+import env
 
 class Astar:
-    def __init__(self, Start_State, Goal_State, n, m, heuristic_type):
-        self.xI = Start_State
-        self.xG = Goal_State
-        self.u_set = environment.motions                              # feasible input set
-        self.obs_map = environment.map_obs()                          # position of obstacles
-        self.n = n
-        self.m = m
+    def __init__(self, x_start, x_goal, x_range, y_range, heuristic_type):
+        self.u_set = env.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.heuristic_type = heuristic_type
 
     def searching(self):
@@ -28,29 +27,27 @@ class Astar:
         q_astar = queue.QueuePrior()                            # priority queue
         q_astar.put(self.xI, 0)
         parent = {self.xI: self.xI}                             # record parents of nodes
-        actions = {self.xI: (0, 0)}                             # record actions of nodes
+        action = {self.xI: (0, 0)}                             # record actions of nodes
         cost = {self.xI: 0}
-        visited = []
 
         while not q_astar.empty():
             x_current = q_astar.get()
-            visited.append(x_current)                           # record visited nodes
             if x_current == self.xG:                            # stop condition
                 break
+            if x_current != self.xI:
+                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 neighbor node is not in obstacles -> ...
-                if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
-                        and not tools.obs_detect(x_current, u_next, self.obs_map):
-                    new_cost = cost[x_current] + int(self.get_cost(x_current, u_next))
+                if x_next not in self.obs:
+                    new_cost = cost[x_current] + self.get_cost(x_current, u_next)
                     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"
                         parent[x_next] = x_current
-                        actions[x_next] = u_next
-        [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, actions)
-        return path_astar, actions_astar, visited
+                        action[x_next] = u_next
+        [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, action)
+        return path_astar, actions_astar
 
     def get_cost(self, x, u):
         """
@@ -83,8 +80,8 @@ class Astar:
 
 
 if __name__ == '__main__':
-    x_Start = (15, 10)          # Starting node
-    x_Goal = (48, 15)           # Goal node
-    astar = Astar(x_Start, x_Goal, environment.col, environment.row, "manhattan")
-    [path_astar, actions_astar, visited_astar] = astar.searching()
-    tools.showPath(x_Start, x_Goal, path_astar, visited_astar, 'Astar_searching')      # Plot path and visited nodes
+    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")
+    [path_astar, actions_astar] = astar.searching()
+    tools.showPath(x_Start, x_Goal, path_astar)      # Plot path and visited nodes

Search-based Planning/bfs.py

@@ -5,21 +5,19 @@
 """
 
 import queue
-import environment
 import tools
+import env
 
 class BFS:
     """
     BFS -> Breadth-first Searching
     """
 
-    def __init__(self, Start_State, Goal_State, n, m):
-        self.xI = Start_State
-        self.xG = Goal_State
-        self.u_set = environment.motions                              # feasible input set
-        self.obs_map = environment.map_obs()                          # position of obstacles
-        self.n = n
-        self.m = m
+    def __init__(self, x_start, x_goal, x_range, y_range):
+        self.u_set = env.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
 
     def searching(self):
         """
@@ -31,29 +29,27 @@ class BFS:
         q_bfs = queue.QueueFIFO()                                     # first-in-first-out queue
         q_bfs.put(self.xI)
         parent = {self.xI: self.xI}                                   # record parents of nodes
-        actions = {self.xI: (0, 0)}                                   # record actions of nodes
-        visited = []
+        action = {self.xI: (0, 0)}                                    # record actions of nodes
+
         while not q_bfs.empty():
             x_current = q_bfs.get()
-            visited.append(x_current)                                 # record visited nodes
-            if x_current == self.xG:                                  # stop condition
+            if x_current == self.xG:
                 break
+            if x_current != self.xI:
+                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))])   # neighbor node
-                # if neighbor node is not in obstacles and has not been visited -> ...
-                if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
-                        and x_next not in parent \
-                        and not tools.obs_detect(x_current, u_next, self.obs_map):
+                x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+                if x_next not in parent and x_next not in self.obs:   # node not visited and not in obstacles
                     q_bfs.put(x_next)
                     parent[x_next] = x_current
-                    actions[x_next] = u_next
-        [path_bfs, actions_bfs] = tools.extract_path(self.xI, self.xG, parent, actions)     # extract path
-        return path_bfs, actions_bfs, visited
+                    action[x_next] = u_next
+        [path_bfs, action_bfs] = tools.extract_path(self.xI, self.xG, parent, action)     # extract path
+        return path_bfs, action_bfs
 
 
 if __name__ == '__main__':
-    x_Start = (15, 10)           # Starting node
-    x_Goal = (48, 15)            # Goal node
-    bfs = BFS(x_Start, x_Goal, environment.col, environment.row)
-    [path_bf, actions_bf, visited_bfs] = bfs.searching()
-    tools.showPath(x_Start, x_Goal, path_bf, visited_bfs, 'breadth_first_searching')    # Plot path and visited nodes
+    x_Start = (5, 5)                    # Starting node
+    x_Goal = (49, 5)                    # Goal node
+    bfs = BFS(x_Start, x_Goal, env.x_range, env.y_range)
+    [path_bf, actions_bf] = bfs.searching()
+    tools.showPath(x_Start, x_Goal, path_bf)

Search-based Planning/dfs.py

@@ -5,21 +5,19 @@
 """
 
 import queue
-import environment
 import tools
+import env
 
 class DFS:
     """
     DFS -> Depth-first Searching
     """
 
-    def __init__(self, Start_State, Goal_State, n, m):
-        self.xI = Start_State
-        self.xG = Goal_State
-        self.u_set = environment.motions                              # feasible input set
-        self.obs_map = environment.map_obs()                          # position of obstacles
-        self.n = n
-        self.m = m
+    def __init__(self, x_start, x_goal, x_range, y_range):
+        self.u_set = env.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
 
     def searching(self):
         """
@@ -31,30 +29,27 @@ class DFS:
         q_dfs = queue.QueueLIFO()                               # last-in-first-out queue
         q_dfs.put(self.xI)
         parent = {self.xI: self.xI}                             # record parents of nodes
-        actions = {self.xI: (0, 0)}                             # record actions of nodes
-        visited = []
+        action = {self.xI: (0, 0)}                              # record actions of nodes
 
         while not q_dfs.empty():
             x_current = q_dfs.get()
-            visited.append(x_current)                           # record visited nodes
-            if x_current == self.xG:                            # stop condition
+            if x_current == self.xG:
                 break
-            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))])   # neighbor node
-                # if neighbor node is not in obstacles and has not been visited -> ...
-                if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
-                        and x_next not in parent \
-                        and not tools.obs_detect(x_current, u_next, self.obs_map):
+            if x_current != self.xI:
+                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 parent and x_next not in self.obs:     # node not visited and not in obstacles
                     q_dfs.put(x_next)
                     parent[x_next] = x_current
-                    actions[x_next] = u_next
-        [path_dfs, actions_dfs] = tools.extract_path(self.xI, self.xG, parent, actions)
-        return path_dfs, actions_dfs, visited
+                    action[x_next] = u_next
+        [path_dfs, action_dfs] = tools.extract_path(self.xI, self.xG, parent, action)
+        return path_dfs, action_dfs
 
 
 if __name__ == '__main__':
-    x_Start = (15, 10)       # Starting node
-    x_Goal = (48, 15)        # Goal node
-    dfs = DFS(x_Start, x_Goal, environment.col, environment.row)
-    [path_dfs, actions_dfs, visited_dfs] = dfs.searching()
-    tools.showPath(x_Start, x_Goal, path_dfs, visited_dfs, 'depth_first_searching')    # Plot path and visited nodes
+    x_Start = (5, 5)                # Starting node
+    x_Goal = (49, 5)                # Goal node
+    dfs = DFS(x_Start, x_Goal, env.x_range, env.y_range)
+    [path_dfs, action_dfs] = dfs.searching()
+    tools.showPath(x_Start, x_Goal, path_dfs)

Search-based Planning/dijkstra.py

@@ -5,17 +5,15 @@
 """
 
 import queue
-import environment
+import env
 import tools
 
 class Dijkstra:
-    def __init__(self, Start_State, Goal_State, n, m):
-        self.xI = Start_State
-        self.xG = Goal_State
-        self.u_set = environment.motions                       # feasible input set
-        self.obs_map = environment.map_obs()                   # position of obstacles
-        self.n = n
-        self.m = m
+    def __init__(self, x_start, x_goal, x_range, y_range):
+        self.u_set = env.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
 
     def searching(self):
         """
@@ -27,29 +25,27 @@ class Dijkstra:
         q_dijk = queue.QueuePrior()                            # priority queue
         q_dijk.put(self.xI, 0)
         parent = {self.xI: self.xI}                            # record parents of nodes
-        actions = {self.xI: (0, 0)}                            # record actions of nodes
+        action = {self.xI: (0, 0)}                            # record actions of nodes
         cost = {self.xI: 0}
-        visited = []
 
         while not q_dijk.empty():
             x_current = q_dijk.get()
-            visited.append(x_current)                          # record visited nodes
             if x_current == self.xG:                           # stop condition
                 break
+            if x_current != self.xI:
+                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 neighbor node is not in obstacles -> ...
-                if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
-                        and not tools.obs_detect(x_current, u_next, self.obs_map):
-                    new_cost = cost[x_current] + int(self.get_cost(x_current, u_next))
+                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
                         priority = new_cost
                         q_dijk.put(x_next, priority)           # put node into queue using cost to come as priority
                         parent[x_next] = x_current
-                        actions[x_next] = u_next
-        [path_dijk, actions_dijk] = tools.extract_path(self.xI, self.xG, parent, actions)
-        return path_dijk, actions_dijk, visited
+                        action[x_next] = u_next
+        [path_dijk, action_dijk] = tools.extract_path(self.xI, self.xG, parent, action)
+        return path_dijk, action_dijk
 
     def get_cost(self, x, u):
         """
@@ -65,8 +61,8 @@ class Dijkstra:
 
 
 if __name__ == '__main__':
-    x_Start = (15, 10)              # Starting node
-    x_Goal = (48, 15)               # Goal node
-    dijkstra = Dijkstra(x_Start, x_Goal, environment.col, environment.row)
-    [path_dijk, actions_dijk, visited_dijk] = dijkstra.searching()
-    tools.showPath(x_Start, x_Goal, path_dijk, visited_dijk, 'dijkstra_searching')
+    x_Start = (5, 5)                # Starting node
+    x_Goal = (49, 5)                # Goal node
+    dijkstra = Dijkstra(x_Start, x_Goal, env.x_range, env.y_range)
+    [path_dijk, actions_dijk] = dijkstra.searching()
+    tools.showPath(x_Start, x_Goal, path_dijk)

Search-based Planning/env.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+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):
+    """
+    Initialize obstacles' positions
+
+    :param xI: starting node
+    :param xG: goal node
+    :param name: title of figure
+    :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))
+
+    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")
+
+    return obs_map
+

Search-based Planning/tools.py

@@ -5,25 +5,6 @@
 """
 
 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):
     """
@@ -47,28 +28,28 @@ 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/.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/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()
+