first commit

Search-based Planning/astar.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: Huiming Zhou
+"""
+
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import colors
+
+from queue import *
+from mazemods import *
+from environment import *
+
+
+def aStarSearch(xI, xG, n, m, O, heuristic_type):
+    q_astar = QueuePrior()
+    q_astar.put(xI, 0)
+    parent = {xI: xI}
+    actions = {xI: (0, 0)}
+    rec_cost = {xI: 0}
+    u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
+
+    while not q_astar.empty():
+        x_current = q_astar.get()
+        if x_current == xG:
+            break
+        for u_next in u_set:
+            x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+            if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
+                    and not collisionCheck(x_current, u_next, O):
+                new_cost = rec_cost[x_current] + 1
+                if x_next not in rec_cost or new_cost < rec_cost[x_next]:
+                    rec_cost[x_next] = new_cost
+                    priority = new_cost + Heuristic(x_next, xG, heuristic_type)
+                    q_astar.put(x_next, priority)
+                    parent[x_next] = x_current
+                    actions[x_next] = u_next
+    [path_astar, actions_astar] = extractpath(xI, xG, parent, actions)
+    [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_astar, O)
+    return path_astar, actions_astar, len(parent), simple_cost, west_cost, east_cost
+
+
+# Heuristic function used in A* algorithm
+def Heuristic(state, goal, heuristic_type):
+    if heuristic_type == "manhattanHeuristic":
+        return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
+    elif heuristic_type == "euclideanHeuristic":
+        return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)
+    else:
+        print("Please choose right heuristic type!")

Search-based Planning/bfs.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: Huiming Zhou
+"""
+
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import colors
+
+from queue import *
+from mazemods import *
+
+
+def breadthFirstSearch(xI, xG, n, m, O):
+    q_bfs = QueueFIFO()
+    q_bfs.put(xI)
+    parent = {xI: xI}
+    actions = {xI: (0, 0)}
+    u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
+
+    while not q_bfs.empty():
+        x_current = q_bfs.get()
+        if x_current == xG:
+            break
+        for u_next in u_set:
+            x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+            if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
+                    and x_next not in parent \
+                    and not collisionCheck(x_current, u_next, O):
+                q_bfs.put(x_next)
+                parent[x_next] = x_current
+                actions[x_next] = u_next
+    [path_bfs, actions_bfs] = extractpath(xI, xG, parent, actions)
+    [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_bfs, O)
+    return path_bfs, actions_bfs, len(parent), simple_cost, west_cost, east_cost

Search-based Planning/dfs.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: Huiming Zhou
+"""
+
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import colors
+
+from queue import *
+from mazemods import *
+
+
+def depth_fist_search(xI, xG, n, m, O):
+    q_dfs = QueueLIFO()
+    q_dfs.put(xI)
+    parent = {xI: xI}
+    actions = {xI: (0, 0)}
+    u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
+
+    while not q_dfs.empty():
+        x_current = q_dfs.get()
+        if x_current == xG:
+            break
+        for u_next in u_set:
+            x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+            if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
+                    and x_next not in parent \
+                    and not collisionCheck(x_current, u_next, O):
+                q_dfs.put(x_next)
+                parent[x_next] = x_current
+                actions[x_next] = u_next
+    [path_dfs, actions_dfs] = extractpath(xI, xG, parent, actions)
+    [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_dfs, O)
+    return path_dfs, actions_dfs, len(parent), simple_cost, west_cost, east_cost

Search-based Planning/dijkstra.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: Huiming Zhou
+"""
+
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import colors
+
+from queue import *
+from mazemods import *
+
+
+def DijkstraSearch(xI, xG, n, m, O, cost_type):
+    q_dijk = QueuePrior()
+    q_dijk.put(xI, 0)
+    parent = {xI: xI}
+    actions = {xI: (0, 0)}
+    rec_cost = {xI: 0}
+    u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
+
+    while not q_dijk.empty():
+        x_current = q_dijk.get()
+        if x_current == xG:
+            break
+        for u_next in u_set:
+            x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+            if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
+                    and not collisionCheck(x_current, u_next, O):
+                cost_x = costfunc(x_current, x_next, O, cost_type)
+                new_cost = rec_cost[x_current] + cost_x
+                if x_next not in rec_cost or new_cost < rec_cost[x_next]:
+                    rec_cost[x_next] = new_cost
+                    priority = new_cost
+                    q_dijk.put(x_next, priority)
+                    parent[x_next] = x_current
+                    actions[x_next] = u_next
+    [path_dijk, actions_dijk] = extractpath(xI, xG, parent, actions)
+    [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_dijk, O)
+    return path_dijk, actions_dijk, len(parent), simple_cost, west_cost, east_cost
+
+
+# Cost function used in Dijkstra's algorithm
+def costfunc(x_current, x_next, O, function_type):
+    if function_type == "westcost":
+        return x_next[0] ** 2
+    elif function_type == "eastcost":
+        maxX = 0
+        for k in range(len(O)):
+            westxO = O[k][1]
+            if westxO > maxX:
+                maxX = westxO
+        return (maxX - x_next[0]) ** 2
+    else:
+        print("Please choose right cost function!")

Search-based Planning/environment.py

@@ -0,0 +1,16 @@
+n, m = 36, 18
+
+O=[[0,35,0,0],[0,0,0,17],[0,35,17,17],[35,35,0,17],[11,26,1,1],[1,9,2,2],
+[28,33,2,2],[9,14,3,3],[16,26,3,3],[28,29,3,3],[32,33,3,3],[2,7,4,4],
+[14,14,4,4],[22,26,4,4],[28,29,4,4],[7,12,5,5],[14,18,5,5],[20,20,5,5],
+[25,26,5,5],[28,29,5,5],[31,34,5,5],[1,5,6,6],[12,12,6,6],[20,23,6,6],
+[25,26,6,6],[28,28,6,6],[5,10,7,7],[12,18,7,7],[25,26,7,7],[28,33,7,7],
+[2,3,8,8],[5,5,8,8],[9,10,8,8],[18,26,8,8],[28,29,8,8],[2,3,9,9],[5,5,9,9],
+[7,7,9,9],[9,16,9,9],[18,19,9,9],[28,29,9,9],[31,34,9,9],[2,2,10,10],
+[5,5,10,10],[7,7,10,10],[9,9,10,10],[13,13,10,10],[18,19,10,10],[21,28,10,10],
+[2,3,11,11],[5,5,11,11],[7,7,11,11],[9,9,11,11],[11,11,11,11],[13,13,11,11],
+[15,18,11,11],[21,23,11,11],[28,33,11,11],[2,3,12,12],[5,5,12,12],[7,7,12,12],
+[9,9,12,12],[11,11,12,12],[25,26,12,12],[28,29,12,12],[2,3,13,13],[5,5,13,13],
+[7,7,13,13],[9,9,13,13],[11,11,13,13],[13,16,13,13],[18,26,13,13],
+[28,29,13,13],[31,34,13,13],[2,3,14,14],[7,7,14,14],[11,11,14,14],
+[18,24,14,14],[28,29,14,14],[2,24,15,15],[26,33,15,15]]

Search-based Planning/mazemods.py

@@ -0,0 +1,179 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: Huiming Zhou
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import colors
+
+
+# Makes the maze data with obstacles stored as black RGB values,
+# free space stored as white RGB values"
+def makeMaze(n, m, O):
+    # Initialize to lists of 1. for RGB color index = white
+    gridvals = [[[1. for i in range(3)] for col in range(n)] for row in range(m)]
+    # Iterate through each obstacle
+    for l in range(len(O)):
+        # Find boundaries of current obstacle
+        west, east = [O[l][0], O[l][1]]
+        south, north = [O[l][2], O[l][3]]
+        # Iterate through each cell of obstacle (clunky, but works)
+        for i in range(west, east + 1):
+            for j in range(south, north + 1):
+                gridvals[j][i] = [0., 0., 0.]  # Change entry to RGB black
+    return gridvals
+
+
+# Function to actually plot the maze
+def maze(n, m, O):
+    gridvals = makeMaze(n, m, O)
+    fig, ax = plt.subplots()  # make a figure + axes
+    ax.imshow(gridvals)  # Plot it
+    ax.invert_yaxis()  # Needed so that bottom left is (0,0)
+    # ax.axis('off')
+
+
+# Checks for collisions given position x, control u, obstacle list O
+def collisionCheck(x, u, O):
+    # Check input
+    if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
+        print('collision_check error: Invalid input u!')
+        return
+    nextx = [x[i] + u[i] for i in range(len(x))]
+    for l in range(len(O)):
+        # Find boundaries of current obstacle
+        west, east = [O[l][0], O[l][1]]
+        south, north = [O[l][2], O[l][3]]
+        # Check if nextx is contained in obstacle boundaries
+        if west <= nextx[0] <= east and south <= nextx[1] <= north:
+            return True
+    # If we iterate through whole list and don't trigger the "if", then no collisions
+    return False
+
+
+# Makes a piece of data with obstacles stored as black RGB values,
+# free space stored as white RGB values, and path stored as increasing hue of
+# yellow RGB values
+def makePath(xI, xG, path, n, m, O):
+    # Obtain the grid populated with obstacles and free space RGB values first
+    gridpath = makeMaze(n, m, O)
+    L = len(path)
+    # Iterate through the path to plot as increasing shades of yellow
+    for l in range(L - 1):
+        gridpath[path[l][1]][path[l][0]] = [1., 1., 1 - l / (L - 1)]  # white-->yellow
+    gridpath[xI[1]][xI[0]] = [0., 0., 1.]  # Initial node (plotted as blue)
+    gridpath[xG[1]][xG[0]] = [0., 1., 0.]  # Goal node (plotted as green)
+    return gridpath
+
+
+# Constructs path list from initial point and list of actions
+def getPathFromActions(xI, actions):
+    L = len(actions)
+    path = []
+    nextx = xI
+    for l in range(L):
+        u = actions[l]
+        if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
+            print('getPath error: Invalid input u!')
+            return
+        nextx = [nextx[i] + u[i] for i in range(len(nextx))]  # nextx = nextx + u
+        path.append(nextx)  # Builds the path
+    return path
+
+
+# If any collisions, cost is 999999, else cost is one for each action
+def getCostOfActions(xI, actions, O):
+    L = len(actions)
+    costsum = 0
+    nextx = xI
+    for l in range(L):
+        u = actions[l]
+        if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
+            print('getCostOfActions error: Invalid input u!')
+            return
+        collision = collisionCheck(nextx, u, O)
+        if collision: return 999999999
+        nextx = [nextx[i] + u[i] for i in range(len(nextx))]  # nextx = nextx + u
+        costsum = costsum + 1
+    return costsum
+
+
+# If any collisions, cost is 999999, else cost is 2^(x[0]) for each action
+def stayWestCost(xI, actions, O):
+    L = len(actions)
+    costsum = 0
+    nextx = xI
+    for l in range(L):
+        u = actions[l]
+        if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
+            print('stayWestCost error: Invalid input u!')
+            return
+        collision = collisionCheck(nextx, u, O)
+        if collision: return 999999999
+        nextx = [nextx[i] + u[i] for i in range(len(nextx))]  # nextx = nextx + u
+        costsum = costsum + nextx[0] ** 2
+    return costsum
+
+
+# If any collisions, cost is 999999, else cost is 2^(maxX - x[0]) for each action
+def stayEastCost(xI, actions, O):
+    # Determine maximum x coordinate of workspace from obstacle list
+    maxX = 0
+    for k in range(len(O)):
+        westxO = O[k][1]
+        if westxO > maxX:
+            maxX = westxO
+
+    L = len(actions)
+    costsum = 0
+    nextx = xI
+    for l in range(L):
+        u = actions[l]
+        if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
+            print('stayEastCost error: Invalid input u!')
+            return
+        collision = collisionCheck(nextx, u, O)
+        if collision: return 999999999
+        nextx = [nextx[i] + u[i] for i in range(len(nextx))]  # nextx = nextx + u
+        costsum = costsum + (maxX - nextx[0]) ** 2
+    return costsum
+
+
+# Calculate different types of cost for searching algorithm
+def cost_calculation(xI, actions, O):
+    simple_cost = getCostOfActions(xI, actions, O)
+    west_cost = stayWestCost(xI, actions, O)
+    east_cost = stayEastCost(xI, actions, O)
+    return simple_cost, west_cost, east_cost
+
+
+# Extract path from results of searching algorithm
+def extractpath(xI, xG, parent, actions):
+    pathback = [xG]
+    actionsback = []
+    actionsback.append(actions[xG])
+    x_current = xG
+    while parent[x_current] != x_current:
+        x_current = parent[x_current]
+        pathback.append(x_current)
+        actionsback.append(actions[x_current])
+    path_extract = list(reversed(pathback))
+    actions_extract = list(reversed(actionsback))
+    path_extract.pop(0)
+    actions_extract.pop(0)
+    return path_extract, actions_extract
+
+
+# Plots the path
+def showPath(xI, xG, path, n, m, O, name):
+    gridpath = makePath(xI, xG, path, n, m, O)
+    fig, ax = plt.subplots(1, 1)  # make a figure + axes
+    ax.imshow(gridpath)  # Plot it
+    ax.invert_yaxis()  # Needed so that bottom left is (0,0)
+    plt.title(name, fontdict=None)
+    plt.show()
+
+
+

Search-based Planning/queue.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+"""
+@author: Huiming Zhou
+@description: this file defines three kinds of queues that will be used in algorithms.
+"""
+
+import collections
+import heapq
+
+
+"""
+    Class: QueueFIFO
+    Description: QueueFIFO is designed for First-in-First-out rule.
+"""
+
+
+class QueueFIFO:
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)            # enter from back
+
+    def get(self):
+        return self.queue.popleft()        # leave from front
+
+
+"""
+    Class: QueueLIFO
+    Description: QueueLIFO is designed for Last-in-First-out rule.
+"""
+
+
+class QueueLIFO:
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)            # enter from back
+
+    def get(self):
+        return self.queue.pop()            # leave from back
+
+
+"""
+    Class: QueuePrior
+    Description: QueuePrior reorders elements using value [priority]
+"""
+
+
+class QueuePrior:
+    def __init__(self):
+        self.queue = []
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, item, priority):
+        heapq.heappush(self.queue, (priority, item))    # reorder x using priority
+
+    def get(self):
+        return heapq.heappop(self.queue)[1]             # pop out the smallest item

Search-based Planning/searching.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: Huiming Zhou
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import colors
+
+from queue import *
+from mazemods import *
+from environment import *
+
+from bfs import *
+from dijkstra import *
+from astar import *
+
+
+class Searching:
+    def __init__(self, Start_State, Goal_State, n, m, O):
+        self.xI = Start_State
+        self.xG = Goal_State
+        self.u_set = [(-1, 0), (1, 0), (0, 1), (0, -1)]
+        self.n = n
+        self.m = m
+        self.O = O
+
+    def depth_fist_search(self):
+        q_dfs = QueueLIFO()
+        q_dfs.put(self.xI)
+        parent = {self.xI: self.xI}
+        actions = {self.xI: (0, 0)}
+
+        while not q_dfs.empty():
+            x_current = q_dfs.get()
+            if x_current == xG:
+                break
+            for u_next in self.u_set:
+                x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+                if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
+                        and x_next not in parent \
+                        and not collisionCheck(x_current, u_next, O):
+                    q_dfs.put(x_next)
+                    parent[x_next] = x_current
+                    actions[x_next] = u_next
+        [path_dfs, actions_dfs] = extractpath(xI, xG, parent, actions)
+        [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_dfs, O)
+        return path_dfs, actions_dfs, len(parent), simple_cost, west_cost, east_cost
+
+
+if __name__ == '__main__':
+    xI = (1, 1)
+    xG = (23, 12)
+    searching = Searching(xI, xG, n, m, O)
+
+    [path_df, actions_df, num_visited_df, simple_cost_df, west_cost_df, east_cost_df] = searching.depth_fist_search()
+    print('1 - Depth_First_Searching algorithm: ')
+    print('Legal control actions: \n', actions_df)
+    print('Number of explored nodes was [%d], basic cost was [%d], stay west cost was [%d], stay east cost was [%d] \n'
+          % (num_visited_df, simple_cost_df, west_cost_df, east_cost_df))
+
+    showPath(xI, xG, path_df, n, m, O, 'Depth First Searching algorithm')