diff --git a/src/pgraph/PGraph.py b/src/pgraph/PGraph.py index fd4fb7c7..382f02a3 100644 --- a/src/pgraph/PGraph.py +++ b/src/pgraph/PGraph.py @@ -10,6 +10,8 @@ from matplotlib.transforms import Affine2D, ScaledTranslation from mpl_toolkits.mplot3d import proj3d import copy +import heapq +from collections import deque from collections.abc import Iterable, Iterator import tempfile import webbrowser @@ -3074,7 +3076,10 @@ def path_BFS( # we use lists not sets since the order is instructive in verbose # mode, really need ordered sets... - frontier: list[Vertex] = [S] + # the frontier is a queue; ``seen`` is everything that has been on it, + # so that "frontier or explored" is a set lookup, not a list scan + frontier: deque[Vertex] = deque([S]) + seen: set[Vertex] = {S} explored: list[Vertex] = [] parent: dict[Vertex, Vertex] = {} done = False @@ -3085,7 +3090,7 @@ def path_BFS( print("FRONTIER:", ", ".join([str(v.name) for v in frontier])) print("EXPLORED:", ", ".join([str(v.name) for v in explored])) - x = frontier.pop(0) + x = frontier.popleft() if verbose: print(" expand", x.name) @@ -3099,8 +3104,9 @@ def path_BFS( parent[n] = x done = True break - if n not in frontier and n not in explored: + if n not in seen: # add it to the frontier + seen.add(n) frontier.append(n) if verbose: print(" add", n.name, "to the frontier") @@ -3122,8 +3128,9 @@ def path_BFS( while x is not S: p = parent[x] length += self._require_cost(p.edgeto(x)) - path.insert(0, p) + path.append(p) x = p + path.reverse() if summary or verbose: print( @@ -3184,8 +3191,14 @@ def path_UCS( S = self._resolve_vertex(S, "start") G = self._resolve_vertex(G, "goal") - frontier: list[Vertex] = [S] - explored: list[Vertex] = [] + # The frontier is a dict, used as an ordered set: its order is the order + # vertices first joined it, which breaks ties between equal ``f``, and + # its value is that position. ``heap`` finds the minimum ``f``; a + # vertex that is reparented is pushed again, and the older entry is + # recognised as stale when it is popped. + frontier: dict[Vertex, int] = {S: 0} + heap: list[tuple[float, int, Vertex]] = [(0, 0, S)] + explored: dict[Vertex, None] = {} parent: dict[Vertex, Vertex] = {} f: dict[Vertex, float] = {S: 0} # evaluation function @@ -3197,8 +3210,11 @@ def path_UCS( ) print("EXPLORED:", ", ".join([str(v.name) for v in explored])) - i = np.argmin([f[n] for n in frontier]) # minimum f in frontier - x = frontier.pop(i) + while True: # minimum f in frontier, skipping stale heap entries + fx, _, x = heapq.heappop(heap) + if x in frontier and f[x] == fx: + break + del frontier[x] if verbose: print(" expand", x.name) if x is G: @@ -3212,7 +3228,8 @@ def path_UCS( # add it to the frontier parent[n] = x f[n] = fnew - frontier.append(n) + frontier[n] = len(parent) # one entry per vertex added so far + heapq.heappush(heap, (fnew, frontier[n], n)) if verbose: print(" add", n.name, "to the frontier") @@ -3226,8 +3243,9 @@ def path_UCS( ) f[n] = fnew parent[n] = x + heapq.heappush(heap, (fnew, frontier[n], n)) - explored.append(x) + explored[x] = None if verbose: print(" move", x.name, "to the explored list") else: @@ -3242,8 +3260,9 @@ def path_UCS( while x is not S: p = parent[x] length += self._require_cost(p.edgeto(x)) - path.insert(0, p) + path.append(p) x = p + path.reverse() parent_names: dict[str, str] = {} for v, p in parent.items(): @@ -3310,8 +3329,10 @@ def path_Astar( S = self._resolve_vertex(S, "start") G = self._resolve_vertex(G, "goal") - frontier: list[Vertex] = [S] - explored: list[Vertex] = [] + # see path_UCS for the frontier and heap + frontier: dict[Vertex, int] = {S: 0} + heap: list[tuple[float, int, Vertex]] = [(0, 0, S)] + explored: dict[Vertex, None] = {} parent: dict[Vertex, Vertex] = {} g: dict[Vertex, float] = {S: 0} # cost to come f: dict[Vertex, float] = {S: 0} # evaluation function @@ -3324,8 +3345,11 @@ def path_Astar( ) print("EXPLORED:", ", ".join([str(v.name) for v in explored])) - i = np.argmin([f[n] for n in frontier]) # minimum f in frontier - x = frontier.pop(i) + while True: # minimum f in frontier, skipping stale heap entries + fx, _, x = heapq.heappop(heap) + if x in frontier and f[x] == fx: + break + del frontier[x] if verbose: print(" expand", x.name) if x is G: @@ -3336,10 +3360,11 @@ def path_Astar( continue # self-loop, never part of a shortest path if n not in frontier and n not in explored: # add it to the frontier - frontier.append(n) parent[n] = x + frontier[n] = len(parent) # one entry per vertex added so far g[n] = g[x] + self._require_cost(e) # update cost to come f[n] = g[n] + n.heuristic_distance(G) # heuristic + heapq.heappush(heap, (f[n], frontier[n], n)) if verbose: print(" add", n.name, "to the frontier") elif n in frontier: @@ -3355,8 +3380,9 @@ def path_Astar( f[n] = g[n] + n.heuristic_distance(G) # heuristic parent[n] = x # reparent + heapq.heappush(heap, (f[n], frontier[n], n)) - explored.append(x) + explored[x] = None if verbose: print(" move", x.name, "to the explored list") @@ -3372,8 +3398,9 @@ def path_Astar( while x is not S: p = parent[x] length += self._require_cost(p.edgeto(x)) - path.insert(0, p) + path.append(p) x = p + path.reverse() parent_names: dict[str, str] = {} for v, p in parent.items(): diff --git a/tests/test_planner_choices.py b/tests/test_planner_choices.py new file mode 100644 index 00000000..79f86ff0 --- /dev/null +++ b/tests/test_planner_choices.py @@ -0,0 +1,231 @@ +""" +The path planners are searches with an explicit frontier whose order matters: +ties between equal costs are broken by the order vertices joined the frontier, +and ``verbose=True`` prints that order. Implemented naively (lists searched +with ``in``, ``np.argmin`` over the frontier) they were O(V^2) or worse, and +the frontier is now a queue or heap with set lookups. These tests pin the +results *and the verbose transcripts* that the original implementation gave, +so the faster one must make exactly the same choices. + +The expectations were generated by the list-based planners that preceded it. +""" + +import contextlib +import io +import unittest + +from pgraph import DGraph, UGraph + + +def grid(): + # a 3 x 2 grid of unit squares plus the diagonal a-e: many equal costs + g = UGraph(dim=2) + coords = dict(a=(0, 0), b=(1, 0), c=(2, 0), d=(0, 1), e=(1, 1), f=(2, 1)) + v = {n: g.add_vertex(list(xy), name=n) for n, xy in coords.items()} + for p, q in ["ab", "bc", "de", "ef", "ad", "be", "cf", "ae"]: + g.add_edge(v[p], v[q], cost=1) + return g, "a", "f" + + +def reparent(): + # x is first reached directly (cost 5), then more cheaply via y (1 + 1) + g = DGraph(dim=2) + v = {n: g.add_vertex([x, 0], name=n) for n, x in dict(s=0, y=1, x=2, t=3).items()} + for p, q, c in [("s", "x", 5), ("s", "y", 1), ("y", "x", 1), ("x", "t", 1)]: + g.add_edge(v[p], v[q], cost=c) + return g, "s", "t" + + +# (graph, planner) -> (path, length, verbose transcript) +EXPECTED = {('grid', 'Astar'): (['a', 'e', 'f'], + 2.0, + '\n' + 'FRONTIER: a(0)\n' + 'EXPLORED: \n' + ' expand a\n' + ' add b to the frontier\n' + ' add d to the frontier\n' + ' add e to the frontier\n' + ' move a to the explored list\n' + '\n' + 'FRONTIER: b(2), d(3), e(2)\n' + 'EXPLORED: a\n' + ' expand e\n' + ' add f to the frontier\n' + ' move e to the explored list\n' + '\n' + 'FRONTIER: b(2), d(3), f(2)\n' + 'EXPLORED: a, e\n' + ' expand f\n' + '2 vertices explored, 2 remaining on the frontier\n'), + ('grid', 'BFS'): (['a', 'e', 'f'], + 2.0, + '\n' + 'FRONTIER: a\n' + 'EXPLORED: \n' + ' expand a\n' + ' add b to the frontier\n' + ' add d to the frontier\n' + ' add e to the frontier\n' + ' move a to the explored list\n' + '\n' + 'FRONTIER: b, d, e\n' + 'EXPLORED: a\n' + ' expand b\n' + ' add c to the frontier\n' + ' move b to the explored list\n' + '\n' + 'FRONTIER: d, e, c\n' + 'EXPLORED: a, b\n' + ' expand d\n' + ' move d to the explored list\n' + '\n' + 'FRONTIER: e, c\n' + 'EXPLORED: a, b, d\n' + ' expand e\n' + ' goal f reached\n' + '3 vertices explored, 1 remaining on the frontier\n'), + ('grid', 'UCS'): (['a', 'e', 'f'], + 2.0, + '\n' + 'FRONTIER: a(0)\n' + 'EXPLORED: \n' + ' expand a\n' + ' add b to the frontier\n' + ' add d to the frontier\n' + ' add e to the frontier\n' + ' move a to the explored list\n' + '\n' + 'FRONTIER: b(1), d(1), e(1)\n' + 'EXPLORED: a\n' + ' expand b\n' + ' add c to the frontier\n' + ' move b to the explored list\n' + '\n' + 'FRONTIER: d(1), e(1), c(2)\n' + 'EXPLORED: a, b\n' + ' expand d\n' + ' move d to the explored list\n' + '\n' + 'FRONTIER: e(1), c(2)\n' + 'EXPLORED: a, b, d\n' + ' expand e\n' + ' add f to the frontier\n' + ' move e to the explored list\n' + '\n' + 'FRONTIER: c(2), f(2)\n' + 'EXPLORED: a, b, d, e\n' + ' expand c\n' + ' move c to the explored list\n' + '\n' + 'FRONTIER: f(2)\n' + 'EXPLORED: a, b, d, e, c\n' + ' expand f\n' + '5 vertices explored, 0 remaining on the frontier\n'), + ('reparent', 'Astar'): (['s', 'y', 'x', 't'], + 3.0, + '\n' + 'FRONTIER: s(0)\n' + 'EXPLORED: \n' + ' expand s\n' + ' add x to the frontier\n' + ' add y to the frontier\n' + ' move s to the explored list\n' + '\n' + 'FRONTIER: x(6), y(3)\n' + 'EXPLORED: s\n' + ' expand y\n' + ' reparent x: cost 2 via y is less than cost 5 via s, change parent from ' + 's to y \n' + ' move y to the explored list\n' + '\n' + 'FRONTIER: x(3)\n' + 'EXPLORED: s, y\n' + ' expand x\n' + ' add t to the frontier\n' + ' move x to the explored list\n' + '\n' + 'FRONTIER: t(3)\n' + 'EXPLORED: s, y, x\n' + ' expand t\n' + '3 vertices explored, 0 remaining on the frontier\n'), + ('reparent', 'BFS'): (['s', 'x', 't'], + 6.0, + '\n' + 'FRONTIER: s\n' + 'EXPLORED: \n' + ' expand s\n' + ' add x to the frontier\n' + ' add y to the frontier\n' + ' move s to the explored list\n' + '\n' + 'FRONTIER: x, y\n' + 'EXPLORED: s\n' + ' expand x\n' + ' goal t reached\n' + '1 vertices explored, 1 remaining on the frontier\n'), + ('reparent', 'UCS'): (['s', 'y', 'x', 't'], + 3.0, + '\n' + 'FRONTIER: s(0)\n' + 'EXPLORED: \n' + ' expand s\n' + ' add x to the frontier\n' + ' add y to the frontier\n' + ' move s to the explored list\n' + '\n' + 'FRONTIER: x(5), y(1)\n' + 'EXPLORED: s\n' + ' expand y\n' + ' reparent x: cost 2 via y is less than cost 5 via s, change parent from s ' + 'to y \n' + ' move y to the explored list\n' + '\n' + 'FRONTIER: x(2)\n' + 'EXPLORED: s, y\n' + ' expand x\n' + ' add t to the frontier\n' + ' move x to the explored list\n' + '\n' + 'FRONTIER: t(3)\n' + 'EXPLORED: s, y, x\n' + ' expand t\n' + '3 vertices explored, 0 remaining on the frontier\n')} + + +class TestPlannerChoices(unittest.TestCase): + def check(self, make, planner): + g, s, t = make() + buf = io.StringIO() + with contextlib.redirect_stdout(buf): + result = getattr(g, f"path_{planner}")(s, t, verbose=True, summary=True) + path, length, transcript = EXPECTED[(make.__name__, planner)] + self.assertEqual([v.name for v in result[0]], path) + self.assertEqual(result[1], length) + self.assertEqual(buf.getvalue(), transcript) + + def test_grid_BFS(self): + self.check(grid, "BFS") + + def test_grid_UCS(self): + self.check(grid, "UCS") + + def test_grid_Astar(self): + self.check(grid, "Astar") + + def test_reparent_BFS(self): + self.check(reparent, "BFS") + + def test_reparent_UCS(self): + self.check(reparent, "UCS") + + def test_reparent_Astar(self): + self.check(reparent, "Astar") + + def test_reparenting_happens(self): + # guard the guard: the transcript really does contain a reparent + self.assertIn("reparent x", EXPECTED[("reparent", "UCS")][2]) + + +if __name__ == "__main__": + unittest.main()