# A* GridWorld Search import matplotlib.pyplot as plt from matplotlib import colors import numpy as np import copy def plotGrid(data): # Plot a 10x10 grid of squares # Create a colormap cmap = colors.ListedColormap(['yellow','green','black','blue','red']) # Black=obstacles, blue=open, red=explored, green=goal, yellow=current position bounds = [-2,-1,0,1,2,3] norm = colors.BoundaryNorm(bounds, cmap.N) fig, ax = plt.subplots() ax.imshow(data, cmap=cmap, norm=norm) # draw gridlines ax.grid(which='major', axis='both', linestyle='-', color='k', linewidth=2) ax.set_xticks(np.arange(-.5,10,1)); ax.set_yticks(np.arange(-0.5,10,1)); plt.show() # Node Class class Node: def __init__(self): # Constructs a Node self.position = None self.parent = None self.children = None self.numChildren = 0 self.g_cost = None # exact cost self.h_cost = None # heuristic self.cost = None # total cost # Grow Tree in the Direction of the Lowest Cost def GrowTree(X, data, goalpos): # Needs the current node and the map pos = X.position explore_points, num = FindPointsToExplore(data, pos) X.numChildren = num X.children = [] for i in range(0,num): newChild = Node() newChild.position = explore_points[i] newChild.g_cost = X.g_cost+1 newChild.h_cost = ComputeHeuristicCost(newChild.position, goalpos) newChild.cost = newChild.g_cost+newChild.h_cost newChild.parent = X X.children.append(newChild) data[newChild.position[0], newChild.position[1]] = 3 if num == 0: X.Children = None X.cost = 1000 # hack return data # Find Child with Lowest Cost def LowestCost(X): if X.numChildren == 0: return X, X.cost else: for i in range(0,X.numChildren): if i == 0: new_child, new_cost = LowestCost(X.children[i]) lowest_cost = new_cost bestest_child = new_child else: new_child, new_cost = LowestCost(X.children[i]) if new_cost < lowest_cost: lowest_cost = new_cost bestest_child = new_child return bestest_child, lowest_cost # ComputeHuristicCost function def ComputeHeuristicCost(cpos, gpos): # Heuristic cost # Manhattan Distance return np.abs(cpos[0]-gpos[0])+np.abs(cpos[1]-gpos[1]) # We will define grid points we have explored/added to the # tree as "red" = 2 def FindPointsToExplore(data, cpos): Nrows, Ncols = data.shape list_of_spaces = [] num = 0 # Look up down left and right from this position lpos = copy.deepcopy(cpos) lpos[0] = lpos[0]-1 # Look up one row if lpos[0] >= 0: if data[lpos[0],lpos[1]] == 1 or data[lpos[0],lpos[1]] == -11: # can be explored # add lpos to a list of explorable space list_of_spaces.append(lpos) num = num + 1 # Looking down lpos = copy.deepcopy(cpos) lpos[0] = lpos[0]+1 # Look down one row if lpos[0] <= Nrows-1: if data[lpos[0],lpos[1]] == 1 or data[lpos[0],lpos[1]] == -1: # can be explored # add lpos to a list list_of_spaces.append(lpos) num = num + 1 # Looking left lpos = copy.deepcopy(cpos) lpos[1] = lpos[1]-1 # Look left one column if lpos[1] >= 0: if data[lpos[0],lpos[1]] == 1 or data[lpos[0],lpos[1]] == -1: # can be explored # add lpos to a list list_of_spaces.append(lpos) num = num + 1 # Looking right lpos = copy.deepcopy(cpos) lpos[1] = lpos[1]+1 # Look right one columns if lpos[1] <= Ncols-1: if data[lpos[0],lpos[1]] == 1 or data[lpos[0],lpos[1]] == -1: # can be explored # add lpos to a list list_of_spaces.append(lpos) num = num + 1 return list_of_spaces, num # Rank all searchable nodes in terms of cost # Search in the direction of lowest cost # Make a grid and plot it data = np.ones((10,10)) # Make obstacles (setting data to zero) data[np.arange(1,7),1] = 0 data[7,np.arange(1,7)] = 0 data[np.arange(6,8),7] = 0 goalpos = np.array([6, 3]) startpos = np.array([9, 7]) data[goalpos[0],goalpos[1]] = -1 data[startpos[0],startpos[1]] = -2 plotGrid(data) # ComputeCost(np.array([0,0]), np.array([2,2])) FindPointsToExplore(data, np.array([9,6])) start = Node() start.position = np.array(startpos) start.g_cost = 0; start.h_cost = ComputeHeuristicCost(start.position, goalpos) start.cost = start.g_cost+start.h_cost start.numChildren = 0 #*************** # Run A* Search currentNode = start done = False i = 0 while done == False and i < 100: data = GrowTree(currentNode,data,goalpos) currentNode, lowest_cost = LowestCost(start) i = i + 1 # End if at the goal if currentNode.position[0] == goalpos[0] and currentNode.position[1] == goalpos[1]: done = True #***************** # Reconstruct the best path from tree you just built done = False i = 0 while done == False and i < 100: data[currentNode.position[0],currentNode.position[1]] = -2 # Highlight path in yellow if currentNode.parent == None: done = True else: currentNode = currentNode.parent i = i+1 # Plot Grid of Path plotGrid(data)