def UCSRoute(graph, startVert, goalVert):
"""This algorithm search a graph using Uniform Cost Search algorithm"""
if startVert == goalVert:
return []
q = PriorityQueue()
q.insert(0, (startVert, None))
visited = set()
pred = {}
while not q.isEmpty():
weight, (nextVert, predNextvert) = q.firstElement()
q.delete()
if nextVert in visited:
pass
else:
visited.add(nextVert)
pred[nextVert] = predNextvert
if nextVert == goalVert:
print("UCS number of visited: ", len(visited))
return reconstructPath(startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
if type(n) != int:
# NOTICE: From getNeighbors, the order is (vert, weight)
weight_n = n[1]
n = n[0]
if n not in visited:
q.insert(weight + weight_n, (n, nextVert))
return "NO PATH"
def AStarRoute(graph, startVert, goalVert):
"""This algorithm search a graph using A star Search algorithm"""
if startVert == goalVert:
return []
q = PriorityQueue()
q.insert(0, (startVert, None))
visited = set()
pred = {}
while not q.isEmpty():
weight, (nextVert, predNextvert) = q.firstElement()
# The weight in PriorityQueue also contains the heuristicDist, not the actual weight
# Calculate the actual weight
weight = weight - graph.heuristicDist(nextVert, goalVert)
q.delete()
if nextVert in visited:
pass
else:
visited.add(nextVert)
pred[nextVert] = predNextvert
if nextVert == goalVert:
print("Astar number of visited: ", len(visited))
return reconstructPath(startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
if type(n) != int:
# NOTICE: From getNeighbors, the order is (vert, weight)
weight_n = n[1]
n = n[0]
if n not in visited:
q.insert(weight + weight_n + graph.heuristicDist(n, goalVert), (n, nextVert))
return "NO PATH"
def UCSRoute(graph, startVert, goalVert):
if startVert == goalVert:
return []
minHeap = PriorityQueue()
minHeap.insert(0, startVert)
pred_cost = {}
visited = set()
pred = {}
while not minHeap.isEmpty():
nextVert = minHeap.firstElement() #nextVert = [cost, vert]
minHeap.delete()
print("--------------")
print("Popping", nextVert)
if nextVert[1] in visited:
continue
else:
visited.add(nextVert[1])
if (nextVert[0], nextVert[1]) in pred_cost.keys():
pred[nextVert[1]] = pred_cost[(nextVert[0], nextVert[1])]
else:
pred[nextVert[1]] = None
if goalVert == nextVert[1]:
return reconstructPath(startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert[1])
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited:
minHeap.insert(edgeCost + nextVert[0], neighNode)
pred_cost[(edgeCost + nextVert[0], neighNode)] = nextVert[1]
return "NO PATH"
def initialize(self):
"""The Initialize algorithm from the pseudocode."""
self.u = PriorityQueue()
vertices = self.graph.getVertices()
for vert in vertices:
self.rhs[vert] = self.maxVal
self.g[vert] = self.maxVal
self.rhs[self.startVert] = 0
self.u.insert(priority=self.calculateKey(self.startVert),
val=self.startVert)
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 AStarRoute(graph, startVert, goalVert):
nodesRemoved = 0
maxSize = 0
if startVert == goalVert:
return []
minHeap = PriorityQueue()
minHeap.insert(0, startVert)
pred_cost = {}
visited = set()
pred = {}
while not minHeap.isEmpty():
if minHeap.size > maxSize:
maxSize = minHeap.size
nextVert = minHeap.firstElement() # nextVert = [cost, vert]
# print("--------------")
# print("Popping", nextVert)
minHeap.delete()
nodesRemoved = nodesRemoved + 1
if nextVert[1] in visited:
continue
else:
visited.add(nextVert[1])
if (nextVert[0], nextVert[1]) in pred_cost.keys():
pred[nextVert[1]] = pred_cost[(nextVert[0], nextVert[1])]
else:
pred[nextVert[1]] = None
if goalVert == nextVert[1]:
return nodesRemoved, maxSize, reconstructPath(
startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert[1])
# print('Adding neighbors to the queue: ')
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited:
Gcost = edgeCost + nextVert[0]
Hcost = graph.heuristicDist(neighNode, goalVert)
if startVert != nextVert[1]:
Gcost = Gcost - graph.heuristicDist(nextVert[1], goalVert)
Fcost = Gcost + Hcost
# print('Node ' + str(neighNode) + ' from ' + str(nextVert[1]))
# print('G cost = ' + str(Gcost) + ' H cost = ' + str(Hcost) + ' F cost = ' + str(Fcost))
minHeap.insert(Fcost, neighNode)
pred_cost[(Fcost, neighNode)] = nextVert[1]
return "NO PATH"
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 dijkstras(graph, startVert, goalVert):
""" This algorithm searches a graph using Dijkstras algorithm to find
the shortest path from every point to a goal point (actually
searches from goal to every point, but it's the same thing.
It uses a priority queue to store the indices of vertices that it still
needs to examine.
It returns the best path frmo startVert to goalVert, but otherwise
startVert does not play into the search."""
if startVert == goalVert:
return []
q = PriorityQueue()
visited = set()
pred = {}
cost = {}
for vert in graph.getVertices():
cost[vert] = 1000.0
pred[vert] = None
q.insert(cost[vert], vert)
visited.add(goalVert)
cost[goalVert] = 0
q.update(cost[goalVert], goalVert)
while not q.isEmpty():
(nextCTG, nextVert) = q.firstElement()
q.delete()
visited.add(nextVert)
print("--------------")
print("Popping", nextVert, nextCTG)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited and\
cost[neighNode] > nextCTG + edgeCost:
print("Node", neighNode, "From", nextVert)
print("New cost =", nextCTG + edgeCost)
cost[neighNode] = nextCTG + edgeCost
pred[neighNode] = nextVert
q.update( cost[neighNode], neighNode )
for vert in graph.getVertices():
bestPath = reconstructPath(goalVert, vert, pred)
bestPath.reverse()
print("Best path from ", vert, "to", goalVert, "is", bestPath)
finalPath = reconstructPath(goalVert, startVert, pred)
finalPath.reverse()
return finalPath
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
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())
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 = 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
def dijkstras(graph, startVert, goalVert):
""" This algorithm searches a graph using Dijkstras algorithm to find
the shortest path from every point to a goal point (actually
searches from goal to every point, but it's the same thing.
It uses a priority queue to store the indices of vertices that it still
needs to examine.
It returns the best path frmo startVert to goalVert, but otherwise
startVert does not play into the search."""
num_q_nodes_removed = 0
max_queue_size = -1
if startVert == goalVert:
return []
q = PriorityQueue()
visited = set()
pred = {}
cost = {}
for vert in graph.getVertices():
cost[vert] = 1000.0
pred[vert] = None
q.insert(cost[vert], vert)
visited.add(goalVert)
cost[goalVert] = 0
q.update(cost[goalVert], goalVert)
while not q.isEmpty():
(nextCTG, nextVert) = q.firstElement()
if q.getSize() > max_queue_size:
max_queue_size = q.getSize()
q.delete()
num_q_nodes_removed += 1
visited.add(nextVert)
# print("--------------")
# print("Popping", nextVert, nextCTG)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited and\
cost[neighNode] > nextCTG + edgeCost:
# print("Node", neighNode, "From", nextVert)
# print("New cost =", nextCTG + edgeCost)
cost[neighNode] = nextCTG + edgeCost
pred[neighNode] = nextVert
q.update(cost[neighNode], neighNode)
# This part is finding the best path from all nodes to the goal. Not necessary
# for vert in graph.getVertices():
# bestPath = reconstructPath(goalVert, vert, pred)
# bestPath.reverse()
# print("Best path from ", vert, "to", goalVert, "is", bestPath)
print("====> Dijkstra number of nodes visited: ", len(visited))
print("====> Dijkstra number of nodes removed: ", num_q_nodes_removed)
print("====> Dijkstra max size of queue: ", max_queue_size)
finalPath = reconstructPath(goalVert, startVert, pred)
finalPath.reverse()
return finalPath
def AStarRoute(graph, startVert, goalVert):
""" This algorithm searches a graph using Uniform Cost Search
looking for a path from some start vertex to some goal vertex using
lowest cost. It uses a PriorityQueue to store the indices of
vertices that it still needs to examine."""
maxQueueSize = 0
nodesVisited = 0
if startVert == goalVert:
return []
q = PriorityQueue()
q.insert(graph.heuristicDist(startVert, goalVert), startVert)
visited = {startVert}
pred = {startVert: None}
totalCost = {startVert: 0}
while not q.isEmpty():
if q.getSize() > maxQueueSize:
maxQueueSize = q.getSize()
nextCost, nextVert = q.firstElement()
nodesVisited += 1
nextCost = nextCost - graph.heuristicDist(nextVert, goalVert)
if nextVert == goalVert:
print("Total Cost is : " + str(nextCost))
print("Total Nodes Visited : " + str(nodesVisited))
print("Max queue size: " + str(maxQueueSize))
return reconstructPath(startVert, goalVert, pred)
q.delete()
neighbors = graph.getNeighbors(nextVert)
for (node, edgeCost) in neighbors:
cost = nextCost + edgeCost
# cost = nextCost + edgeCost + graph.heuristicDist(node, goalVert)
if node not in visited:
visited.add(node)
pred[node] = nextVert
totalCost[node] = cost
q.insert(totalCost[node] + graph.heuristicDist(node, goalVert),
node)
else:
if cost < totalCost[node]:
pred[node] = nextVert
totalCost[node] = cost
q.insert(
totalCost[node] + graph.heuristicDist(node, goalVert),
node)
return "NO PATH"
