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