A* Search

def aStarSearch(problem, heuristic=nullHeuristic):
  "Search the node that has the lowest combined cost and heuristic first."

  solution = []

  if problem.isGoalState(problem.getStartState()):
    return solution

  #initialize frontier and explored set
  frontier = PriorityQueue()
  for (loc, action, cost) in problem.getSuccessors(problem.getStartState()):
    newNode = Node(loc, action, cost+heuristic(loc, problem), cost, None)
    frontier.push(newNode, cost+heuristic(loc, problem))
  explored = []

  #while the frontier is not empty, search
  while (not frontier.isEmpty()):
    curNode = frontier.pop()
    if(curNode.getLoc() not in explored):
      #if current node contains a goal state
      if problem.isGoalState(curNode.getLoc()):
        preNode = curNode
        solution.append(preNode.getAction())
        #while predecessor exists
        while (preNode.getPred()):
          preNode = preNode.getPred()
          solution.append(preNode.getAction())
        solution.reverse()
        return solution
      explored.append(curNode.getLoc())
      for (loc, action, cost) in problem.getSuccessors(curNode.getLoc()):
        #if location is not already explored, add to frontier
        if(loc not in explored):
          newNode = Node(loc, action, curNode.getPathCost()+cost+heuristic(loc, problem), curNode.getPathCost()+cost, curNode)
          frontier.push(newNode, curNode.getPathCost()+cost+heuristic(loc, problem))

  #all nodes searched, frontier empty
  return [] #failure