Esempio n. 1
0
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"
Esempio n. 2
0
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
Esempio n. 3
0
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