update

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 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$/motion_model.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/tools.py" beforeDir="false" afterPath="$PROJECT_DIR$/plotting.py" afterDir="false" />
+    </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -23,7 +32,7 @@
     <ConfirmationsSetting value="2" id="Add" />
   </component>
   <component name="PropertiesComponent">
-    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../test" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
@@ -148,9 +157,9 @@
     <recent_temporary>
       <list>
         <item itemvalue="Python.a_star" />
-        <item itemvalue="Python.dijkstra" />
         <item itemvalue="Python.dfs" />
         <item itemvalue="Python.bfs" />
+        <item itemvalue="Python.dijkstra" />
         <item itemvalue="Python.searching" />
       </list>
     </recent_temporary>

Search-based Planning/a_star.py

@@ -5,22 +5,26 @@
 """
 
 import queue
-import tools
+import plotting
 import env
-import motion_model
 
 
 class Astar:
     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.obs = env.obs_map()                            # position of obstacles
-        self.heuristic_type = heuristic_type
 
-        tools.show_map(self.xI, self.xG, self.obs, "a_star searching")
+        self.Env = env.Env()
+        self.u_set = self.Env.motions                               # feasible input set
+        self.obs = self.Env.obs                                     # position of obstacles
 
+        [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG, heuristic_type)
 
-    def searching(self):
+        self.fig_name = "A* Algorithm"
+        plotting.animation(self.xI, self.xG, self.obs,
+                           self.path, self.visited, self.fig_name)  # animation generate
+
+
+    def searching(self, xI, xG, heuristic_type):
         """
         Searching using A_star.
 
@@ -28,30 +32,53 @@ 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
-        cost = {self.xI: 0}
+        q_astar.put(xI, 0)
+        parent = {xI: xI}                                       # record parents of nodes
+        action = {xI: (0, 0)}                                   # record actions of nodes
+        visited = []
+        cost = {xI: 0}
 
         while not q_astar.empty():
             x_current = q_astar.get()
-            if x_current == self.xG:                            # stop condition
+            if x_current == xG:                                 # stop condition
                 break
-            if x_current != self.xI:
-                tools.plot_dots(x_current, len(parent))
+            visited.append(x_current)
             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:
                     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)
+                        priority = new_cost + self.Heuristic(x_next, xG, heuristic_type)
                         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)
+                        parent[x_next], action[x_next] = x_current, u_next
+
+        [path, policy] = self.extract_path(xI, xG, parent, action)
+
+        return path, policy, visited
+
+
+    def extract_path(self, xI, xG, parent, policy):
+        """
+        Extract the path based on the relationship of nodes.
+
+        :param xI: Starting node
+        :param xG: Goal node
+        :param parent: Relationship between nodes
+        :param policy: Action needed for transfer between two nodes
+        :return: The planning path
+        """
+
+        path_back = [xG]
+        acts_back = [policy[xG]]
+        x_current = xG
+        while True:
+            x_current = parent[x_current]
+            path_back.append(x_current)
+            acts_back.append(policy[x_current])
+            if x_current == xI: break
 
-        return path_astar, actions_astar
+        return list(path_back), list(acts_back)
 
 
     def get_cost(self, x, u):
@@ -88,6 +115,4 @@ class Astar:
 if __name__ == '__main__':
     x_Start = (5, 5)                # Starting node
     x_Goal = (49, 5)                # Goal node
-    astar = Astar(x_Start, x_Goal, "manhattan")
-    [path_astar, actions_astar] = astar.searching()
-    tools.showPath(x_Start, x_Goal, path_astar)
+    astar = Astar(x_Start, x_Goal, "manhattan")

Search-based Planning/bfs.py

@@ -5,21 +5,25 @@
 """
 
 import queue
-import tools
+import plotting
 import env
-import motion_model
 
 
 class BFS:
     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.obs = env.obs_map()                                      # position of obstacles
 
-        tools.show_map(self.xI, self.xG, self.obs, "breadth-first searching")
+        self.Env = env.Env()
+        self.u_set = self.Env.motions                               # feasible input set
+        self.obs = self.Env.obs                                     # position of obstacles
+        [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG)
 
+        self.fig_name = "Dijkstra's Algorithm"
+        plotting.animation(self.xI, self.xG, self.obs,
+                           self.path, self.visited, self.fig_name)  # animation generate
 
-    def searching(self):
+
+    def searching(self, xI, xG):
         """
         Searching using BFS.
 
@@ -27,30 +31,51 @@ 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
-        action = {self.xI: (0, 0)}                                    # record actions of nodes
+        q_bfs.put(xI)
+        parent = {xI: xI}                                   # record parents of nodes
+        action = {xI: (0, 0)}                                    # record actions of nodes
+        visited = []
 
         while not q_bfs.empty():
             x_current = q_bfs.get()
-            if x_current == self.xG:
+            if x_current == xG:
                 break
-            if x_current != self.xI:
-                tools.plot_dots(x_current, len(parent))
+            visited.append(x_current)
             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_bfs.put(x_next)
-                    parent[x_next] = x_current
-                    action[x_next] = u_next
-        [path_bfs, action_bfs] = tools.extract_path(self.xI, self.xG, parent, action)     # extract path
+                    parent[x_next], action[x_next] = x_current, u_next
+
+        [path, policy] = self.extract_path(xI, xG, parent, action)     # extract path
+
+        return path, policy, visited
+
+
+    def extract_path(self, xI, xG, parent, policy):
+        """
+        Extract the path based on the relationship of nodes.
+
+        :param xI: Starting node
+        :param xG: Goal node
+        :param parent: Relationship between nodes
+        :param policy: Action needed for transfer between two nodes
+        :return: The planning path
+        """
+
+        path_back = [xG]
+        acts_back = [policy[xG]]
+        x_current = xG
+        while True:
+            x_current = parent[x_current]
+            path_back.append(x_current)
+            acts_back.append(policy[x_current])
+            if x_current == xI: break
 
-        return path_bfs, action_bfs
+        return list(path_back), list(acts_back)
 
 
 if __name__ == '__main__':
     x_Start = (5, 5)                    # Starting node
     x_Goal = (49, 5)                    # Goal node
     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

@@ -5,21 +5,25 @@
 """
 
 import queue
-import tools
+import plotting
 import env
-import motion_model
 
 
 class DFS:
     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.obs = env.obs_map()                                # position of obstacles
 
-        tools.show_map(self.xI, self.xG, self.obs, "depth-first searching")
+        self.Env = env.Env()
+        self.u_set = self.Env.motions                   # feasible input set
+        self.obs = self.Env.obs                         # position of obstacles
+        [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG)
 
+        self.fig_name = "Dijkstra's Algorithm"
+        plotting.animation(self.xI, self.xG, self.obs,
+                           self.path, self.visited, self.fig_name)      # animation generate
 
-    def searching(self):
+
+    def searching(self, xI, xG):
         """
         Searching using DFS.
 
@@ -27,30 +31,51 @@ 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
-        action = {self.xI: (0, 0)}                              # record actions of nodes
+        q_dfs.put(xI)
+        parent = {xI: xI}                                       # record parents of nodes
+        action = {xI: (0, 0)}                                   # record actions of nodes
+        visited = []
 
         while not q_dfs.empty():
             x_current = q_dfs.get()
-            if x_current == self.xG:
+            if x_current == xG:
                 break
-            if x_current != self.xI:
-                tools.plot_dots(x_current, len(parent))
+            visited.append(x_current)
             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
-                    action[x_next] = u_next
-        [path_dfs, action_dfs] = tools.extract_path(self.xI, self.xG, parent, action)
+                    parent[x_next], action[x_next] = x_current, u_next
+
+        [path, policy] = self.extract_path(xI, xG, parent, action)
+
+        return path, policy, visited
+
+
+    def extract_path(self, xI, xG, parent, policy):
+        """
+        Extract the path based on the relationship of nodes.
+
+        :param xI: Starting node
+        :param xG: Goal node
+        :param parent: Relationship between nodes
+        :param policy: Action needed for transfer between two nodes
+        :return: The planning path
+        """
+
+        path_back = [xG]
+        acts_back = [policy[xG]]
+        x_current = xG
+        while True:
+            x_current = parent[x_current]
+            path_back.append(x_current)
+            acts_back.append(policy[x_current])
+            if x_current == xI: break
 
-        return path_dfs, action_dfs
+        return list(path_back), list(acts_back)
 
 
 if __name__ == '__main__':
     x_Start = (5, 5)                # Starting node
     x_Goal = (49, 5)                # Goal node
     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

@@ -6,51 +6,55 @@
 
 import queue
 import env
-import tools
-import motion_model
+import plotting
 
 
 class Dijkstra:
     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.obs = env.obs_map()                               # position of obstacles
 
-        tools.show_map(self.xI, self.xG, self.obs, "dijkstra searching")
+        self.Env = env.Env()
+        self.u_set = self.Env.motions                               # feasible input set
+        self.obs = self.Env.obs                                     # position of obstacles
+        [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG)
 
+        self.fig_name = "Dijkstra's Algorithm"
+        plotting.animation(self.xI, self.xG, self.obs,
+                           self.path, self.visited, self.fig_name)  # animation generate
 
-    def searching(self):
+
+    def searching(self, xI, xG):
         """
         Searching using Dijkstra.
 
         :return: planning path, action in each node, visited nodes in the planning process
         """
 
-        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
-        cost = {self.xI: 0}
+        q_dijk = queue.QueuePrior()                             # priority queue
+        q_dijk.put(xI, 0)
+        parent = {xI: xI}                                       # record parents of nodes
+        action = {xI: (0, 0)}                                   # record actions of nodes
+        visited = []                                            # record visited nodes
+        cost = {xI: 0}
 
         while not q_dijk.empty():
             x_current = q_dijk.get()
-            if x_current == self.xG:                           # stop condition
+            if x_current == 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
+            visited.append(x_current)
+            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
                         priority = new_cost
-                        q_dijk.put(x_next, priority)           # put node into queue using cost to come as priority
-                        parent[x_next] = x_current
-                        action[x_next] = u_next
-        [path_dijk, action_dijk] = tools.extract_path(self.xI, self.xG, parent, action)
+                        q_dijk.put(x_next, priority)            # put node into queue using cost to come as priority
+                        parent[x_next], action[x_next] = x_current, u_next
+
+        [path, policy] = self.extract_path(xI, xG, parent, action)
 
-        return path_dijk, action_dijk
+        return path, policy, visited
 
 
     def get_cost(self, x, u):
@@ -66,9 +70,30 @@ class Dijkstra:
         return 1
 
 
+    def extract_path(self, xI, xG, parent, policy):
+        """
+        Extract the path based on the relationship of nodes.
+
+        :param xI: Starting node
+        :param xG: Goal node
+        :param parent: Relationship between nodes
+        :param policy: Action needed for transfer between two nodes
+        :return: The planning path
+        """
+
+        path_back = [xG]
+        acts_back = [policy[xG]]
+        x_current = xG
+        while True:
+            x_current = parent[x_current]
+            path_back.append(x_current)
+            acts_back.append(policy[x_current])
+            if x_current == xI: break
+
+        return list(path_back), list(acts_back)
+
+
 if __name__ == '__main__':
     x_Start = (5, 5)                # Starting node
     x_Goal = (49, 5)                # Goal node
     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

@@ -4,34 +4,44 @@
 @author: huiming zhou
 """
 
-x_range, y_range = 51, 31     # size of background
+class Env():
+    def __init__(self):
+        self.x_range = 51           # size of background
+        self.y_range = 31
+        self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)]
+        self.obs = self.obs_map()
 
-def obs_map():
-    """
-    Initialize obstacles' positions
 
-    :return: map of obstacles
-    """
+    def obs_map(self):
+        """
+        Initialize obstacles' positions
 
-    obs = []
-    for i in range(x_range):
-        obs.append((i, 0))
-    for i in range(x_range):
-        obs.append((i, y_range - 1))
+        :return: map of obstacles
+        """
 
-    for i in range(y_range):
-        obs.append((0, i))
-    for i in range(y_range):
-        obs.append((x_range - 1, i))
+        x = self.x_range
+        y = self.y_range
+        obs = []
 
-    for i in range(10, 21):
-        obs.append((i, 15))
-    for i in range(15):
-        obs.append((20, i))
+        for i in range(x):
+            obs.append((i, 0))
+        for i in range(x):
+            obs.append((i, y - 1))
 
-    for i in range(15, 30):
-        obs.append((30, i))
-    for i in range(16):
-        obs.append((40, i))
+        for i in range(y):
+            obs.append((0, i))
+        for i in range(y):
+            obs.append((x - 1, i))
+
+        for i in range(10, 21):
+            obs.append((i, 15))
+        for i in range(15):
+            obs.append((20, i))
+
+        for i in range(15, 30):
+            obs.append((30, i))
+        for i in range(16):
+            obs.append((40, i))
+
+        return obs
 
-    return obs

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

Search-based Planning/plotting.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+import matplotlib.pyplot as plt
+
+
+def animation(xI, xG, obs, path, visited, name):
+    """
+    generate animation for exploring process of algorithm
+
+    :param xI: starting state
+    :param xG: goal state
+    :param obs: obstacle map
+    :param path: optimal path
+    :param visited: visited nodes
+    :param name: name of this figure
+    :return: animation
+    """
+
+    visited.remove(xI)
+    path.remove(xI)
+    path.remove(xG)
+
+    # plot gridworld
+    obs_x = [obs[i][0] for i in range(len(obs))]
+    obs_y = [obs[i][1] for i in range(len(obs))]
+    plt.plot(xI[0], xI[1], "bs")
+    plt.plot(xG[0], xG[1], "gs")
+    plt.plot(obs_x, obs_y, "sk")
+    plt.title(name)
+    plt.axis("equal")
+
+    # animation for the exploring order of visited nodes
+    count = 0
+    for x in visited:
+        count += 1
+        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 count % 20 == 0: plt.pause(0.01)
+
+    # plot optimal path
+    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='3', color='r', marker='o')
+
+    # show animation
+    plt.pause(0.01)
+    plt.show()

Search-based Planning/tools.py

@@ -1,68 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: huiming zhou
-"""
-
-import matplotlib.pyplot as plt
-
-
-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 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.axis("equal")
-
-
-def showPath(xI, xG, path):
-    """
-    Plot the path.
-
-    :param xI: Starting node
-    :param xG: Goal node
-    :param path: Planning path
-    :return: A plot
-    """
-
-    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)
-