class BestFirstSearchSolver(AbstractSearchSolver):
"""This class contains a priority-queue based search algorithm. The Priority-Queue Search can act like
any best-first search, including UCS and A*, depending on how the "cost" is calculated. This class contains
stubs for the helper methods that those algorithms need. Only the isGoal and generateNeighbors methods
should be overridden by the subclass.
These algorithms assume that the qData stored in the states implement the equality operators properly!"""
def __init__(self, taskAdvisor):
"""Creates a Best-First search solver, with the given task advisor."""
AbstractSearchSolver.__init__(self, taskAdvisor)
def _setupFringe(self, startState):
"""This method sets up the proper kind of fringe set for this particular search.
In this case, it creates a priority queue and adds the start state to it."""
self.fringe = PriorityQueue()
self.fringe.insert(startState, startState.getCost())
def searchStep(self):
"""This method performs one step of a priority-queue search. It finds the next node in
the priority queue, generates its children, and adds the appropriate ones to the priority queue
It returns three values: the current state, the neighbors of the current state, and a status
message. The message is either "Done", "Fail", or "Step" for a normal step."""
newNeighbors = []
if self.fringe.isEmpty():
return (False, False, "Fail")
nextState, priority = self.fringe.delete()
if self.taskAdvisor.isGoal(nextState):
return (nextState, [], "Done"
) # when hit goal, neighbors are irrelevant
# Otherwise, go on
if verbose:
print("----------------------")
print("Current state:", nextState)
neighbors = self.taskAdvisor.generateNeighbors(nextState)
self.visited.add(nextState)
self.nodesVisited += 1
for n in neighbors:
visitedMatch = self._hasBeenVisited(n)
fringeMatch = self._hasBeenFringed(n)
if (not visitedMatch) and (not fringeMatch):
if verbose:
print(" Neighbor never seen before:", n)
# this node has not been generated before, add it to the fringe
self.fringe.insert(n, n.getCost())
newNeighbors.append(n)
self.nodesCreated += 1
elif visitedMatch and visitedMatch.getCost() > n.getCost():
# if state was visited before but this one is better, add this one to fringe set
if verbose:
print(
" Neighbor was already in explored, cost is lower now",
n, visitedMatch.getCost(), n.getCost())
self.fringe.insert(n, n.getCost())
newNeighbors.append(n)
self.nodesCreated += 1
elif fringeMatch and fringeMatch.getCost() > n.getCost():
# if state is in fringe but this one is better, add this one to fringe AND
if verbose:
print(
" Neighbor state was already in fringe, cost is lower now",
n, fringeMatch.getCost(), n.getCost())
# remove the old one from the fringe
self.fringe.removeValue(fringeMatch)
self.fringe.insert(n, n.getCost())
newNeighbors.append(n)
self.nodesCreated += 1
elif visitedMatch:
if verbose:
print(" Neighbor was already in explored, skipping", n)
elif fringeMatch:
if verbose:
print(" Neighbor was already in fringe, skipping", n)
# end for
return nextState, newNeighbors, "Not Done"
class DStarAlgorithm:
def __init__(self, graph, startVert, goalVert):
"""Takes in a graph, start vertex and goal vertex, and sets up the D* Lite
search, initializing all the data structures and the priority queue."""
self.graph = graph
self.startVert = startVert
self.goalVert = goalVert
self.maxVal = math.inf
self.initialize()
def initialize(self):
"""The Initialize algorithm from the pseudocode."""
self.U = PriorityQueue()
self.nodesRemoved = 0
self.maxSize = 0
self.rhs = {}
self.g = {}
for node in self.graph.getVertices():
self.rhs[node] = self.maxVal
self.g[node] = self.maxVal
self.rhs[self.startVert] = 0
self.U.insert(
self.calculateKey(self.startVert), self.startVert
) # The priority queue stores the priority first, then the vertex
def computeShortestPath(self):
"""The ComputeShortestPath algorithm from the pseudocode."""
while (not self.U.isEmpty()) and (self.compareKeys(
self.U.firstElement()[0], self.calculateKey(
self.goalVert))) or (self.rhs[self.goalVert] !=
self.g[self.goalVert]):
if self.U.size > self.maxSize:
self.maxSize = self.U.size
u = self.U.firstElement()[1]
self.U.delete()
self.nodesRemoved = self.nodesRemoved + 1
if self.g[u] > self.rhs[u]:
self.g[u] = self.rhs[u]
else:
self.g[u] = self.maxVal
self.updateVertex(u)
successors = self.graph.getNeighbors(u)
for s in successors:
self.updateVertex(s[0])
if self.U.isEmpty():
return [] # So that it doesn't crash
return self.reconstructPath()
def updateVertex(self, vert):
"""The UpdateVertex algorithm from the pseudocode."""
if vert != self.startVert:
minVal = self.maxVal
for s in self.graph.getNeighbors(vert):
if self.g[s[0]] + s[1] < minVal:
minVal = self.g[s[0]] + s[1]
self.rhs[vert] = minVal
if self.U.contains(vert):
self.U.removeValue(vert)
if self.g[vert] != self.rhs[vert]:
self.U.insert(self.calculateKey(vert), vert)
def minNeighCost(self, vert):
"""A helper to compute the new rhs value, by finding the minimum cost among
all the neighbors of a vertex. The cost is computed as the g cost of the
neighbor plus the edge cost between the neighbor and the vertex."""
minNCost = self.maxVal
minVert = -1
for neighInfo in self.graph.getNeighbors(vert):
neigh = neighInfo[0]
edgeCost = neighInfo[1]
newCost = self.g[neigh] + edgeCost
if newCost < minNCost:
minNCost = newCost
minVert = neigh
return minNCost
def calculateKey(self, vert):
"""Calculates the current priority for a given vertex"""
minG = min(self.g[vert], self.rhs[vert])
heurCost = self.graph.heuristicDist(vert, self.goalVert)
return [minG + heurCost, minG]
def compareKeys(self, key1, key2):
"""Takes in two keys, each of which is a list containing f cost
and g cost. It prefers the lower f cost, but for equal f costs
it chooses the lower g cost."""
[f1, g1] = key1
[f2, g2] = key2
return (f1 < f2) or ((f1 == f2) and (g1 < g2))
def correctInformation(self, newInfo):
"""Takes in a dictionary whose keys are (r, c) tuples, and the value
is the newly corrected cell weight. Updates the graph, and then updates
the search information appropriately."""
for (r, c) in newInfo:
self.graph.setCellValue(r, c, newInfo[r, c])
self.graph.graphFromGrid()
for (r, c) in newInfo:
nodeNum = r * self.graph.getWidth() + c
# print("(", r, c, ")", nodeNum)
self.updateVertex(nodeNum)
neighs = self.graph.getNeighbors(nodeNum)
for (nextNeigh, wgt) in neighs:
self.updateVertex(nextNeigh)
def reconstructPath(self):
""" Given the start vertex and goal vertex, and the table of
predecessors found during the search, this will reconstruct the path
from start to goal"""
path = [self.goalVert]
currVert = self.goalVert
while currVert != self.startVert:
currVert = self._pickMinNeighbor(currVert)
path.insert(0, currVert)
print(self.nodesRemoved)
print(self.maxSize)
return path
def _pickMinNeighbor(self, vert):
"""A helper to path-reconstruction that finds the neighbor of a vertex
that has the minimum g cost."""
neighs = self.graph.getNeighbors(vert)
minNeigh = None
minVal = self.maxVal
for [neigh, cost] in neighs:
if self.g[neigh] < minVal:
minVal = self.g[neigh]
minNeigh = neigh
return minNeigh
class DStarAlgorithm:
def __init__(self, graph, startVert, goalVert):
"""Takes in a graph, start vertex and goal vertex, and sets up the D* Lite
search, initializing all the data structures and the priority queue."""
self.graph = graph
self.startVert = startVert
self.goalVert = goalVert
self.maxVal = 1000
# PriorityQueue with values [fEst, gEst]
self.U = PriorityQueue()
# estimated cost from g and c for a vertex
self.rhs = {}
# estimated cost from start vertex to other vertex
self.g = {}
# print(self.startVert, self.goalVert)
self.initialize()
def initialize(self):
"""The Initialize algorithm from the pseudocode."""
for s in range(0, self.graph._numVerts):
# s = self.graph.getData(i)
self.rhs.update({s: self.maxVal})
self.g.update({s: self.maxVal})
self.rhs[self.startVert] = 0
# print(self.g, self.rhs)
# print(self.g.values(),self.rhs.values())
self.U.insert(self.calculateKey(self.startVert), self.startVert)
def computeShortestPath(self):
"""The ComputeShortestPath algorithm from the pseudocode."""
# print(self.rhs[self.goalVert])
# print(self.g[self.goalVert])
# print((self.rhs[self.goalVert] != self.g[self.goalVert]))
while (self.U.firstElement() is not None) and (
(self.U.firstElement()[0][0] < self.calculateKey(self.goalVert)[0])
or (self.rhs[self.goalVert] != self.g[self.goalVert])):
u = self.U.firstElement()[1]
# print(u)
self.U.delete()
if self.g.get(u) > self.rhs.get(u):
self.g[u] = self.rhs[u]
else:
self.g[u] = np.inf
self.updateVertex(u)
for [s, cost] in self.graph.getNeighbors(u):
self.updateVertex(s)
return self.reconstructPath()
# if (self.U.firstElement() is not None)
# break
def updateVertex(self, vert):
"""The UpdateVertex algorithm from the pseudocode."""
if vert != self.startVert:
self.rhs[vert] = self.minNeighCost(vert)
if self.U.contains(vert):
self.U.removeValue(vert)
if self.g[vert] != self.rhs[vert]:
self.U.insert(self.calculateKey(vert), vert)
def minNeighCost(self, vert):
"""A helper to compute the new rhs value, by finding the minimum cost among
all the neighbors of a vertex. The cost is computed as the g cost of the
neighbor plus the edge cost between the neighbor and the vertex."""
minNCost = self.maxVal
minVert = -1
for neighInfo in self.graph.getNeighbors(vert):
neigh = neighInfo[0]
edgeCost = neighInfo[1]
newCost = self.g[neigh] + edgeCost
if newCost < minNCost:
minNCost = newCost
minVert = neigh
return minNCost
def calculateKey(self, vert):
"""The CalculateKey algorithm from the pseudocode."""
minG = min(self.g.get(vert), self.rhs.get(vert))
heurCost = self.graph.heuristicDist(vert, self.goalVert)
return [minG + heurCost, minG]
def compareKeys(self, key1, key2):
"""Takes in two keys, each of which is a list containing f cost
and g cost. It prefers the lower f cost, but for equal f costs
it chooses the lower g cost."""
[f1, g1] = key1
[f2, g2] = key2
return (f1 < f2) or ((f1 == f2) and (g1 < g2))
def correctInformation(self, newInfo):
"""Takes in a dictionary whose keys are (r, c) tuples, and the value
is the newly corrected cell weight. Updates the graph, and then updates
the search information appropriately."""
for (r, c) in newInfo:
self.graph.setCellValue(r, c, newInfo[r, c])
self.graph.graphFromGrid()
for (r, c) in newInfo:
nodeNum = r * self.graph.getWidth() + c
#print("(", r, c, ")", nodeNum)
self.updateVertex(nodeNum)
neighs = self.graph.getNeighbors(nodeNum)
for (nextNeigh, wgt) in neighs:
self.updateVertex(nextNeigh)
def reconstructPath(self):
""" Given the start vertex and goal vertex, and the table of
predecessors found during the search, this will reconstruct the path
from start to goal"""
path = [self.goalVert]
currVert = self.goalVert
while currVert != self.startVert:
currVert = self._pickMinNeighbor(currVert)
path.insert(0, currVert)
return path
def _pickMinNeighbor(self, vert):
"""A helper to path-reconstruction that finds the neighbor of a vertex
that has the minimum g cost."""
neighs = self.graph.getNeighbors(vert)
minVal = self.maxVal
minNeigh = None
for [neigh, cost] in neighs:
if self.g[neigh] < minVal:
minVal = self.g[neigh]
minNeigh = neigh
return minNeigh