GraphSearch.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Appendix C. COMPUTATIONAL METHODS\n",
    "\n",
    "# C.2. Graph Search"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One primitive operation that will be used over and over in motion\n",
    "planning is known as *graph search*. In this context, the vertices of\n",
    "the graph are usually points in C-space and edges indicate some notion\n",
    "of local connectivity between points (such as \"a straight line exists\n",
    "from point A to point B\"). In other contexts, the vertices denote a\n",
    "region of C-space, and edges indicate connectivity between regions.\n",
    "\n",
    "Observe that a graph only encodes a set of alternatives (edges) at each\n",
    "vertex, but does not contain any information about which edge to take.\n",
    "The purpose of graph search is to determine, exactly and efficiently,\n",
    "the path --- a sequence of edges --- in the graph that connects a start\n",
    "vertex to a goal vertex. Search is a process of *systematically*\n",
    "exploring a sequence of alternatives, and we will use it to find paths\n",
    "in discretized versions of continuous free spaces.\n",
    "\n",
    "\n",
    "## Big-O notation\n",
    "\n",
    "Throughout this book we use *Big-O notation* to describe the performance\n",
    "of various quantities, including the size of data structures and running\n",
    "times of calculations. This notation allows us to specify the most\n",
    "dominant factors affecting performance while ignoring constant terms.\n",
    "Specifically, the statement: $$g(n) \\text{ is } O(f(n))$$ means that\n",
    "above a certain value of $n$, $g(n)$ will be less than some constant\n",
    "times $f(n)$. More specifically, it is defined as the following\n",
    "statement: there exists a value $N$ and a constant $c$ such that for all\n",
    "$n > N$, $g(n) < c f(n)$. In other words, $g$ is asymptotically bounded\n",
    "by $f$\n",
    "\n",
    "Even if $g(n)$ is a complex expression, it is possible to determine a\n",
    "simple expression for $f(n)$. For example, if\n",
    "$g(n) = 30 n^4 - 12 n^2  + 1000 n$, the dominant term as $n$ grows is\n",
    "the $n^4$ term, and we can say $g(n)$ is $O(n^4)$. We will seek the\n",
    "simplest expression for $f$ that fulfills the big-O requirement while\n",
    "also remaining a tight bound.\n",
    "\n",
    "Some common Big-O expressions include:\n",
    "\n",
    "- $O(1)$: constant.\n",
    "- $O(n)$: upper bounded by a linear function.\n",
    "- $O(n^2)$: upper bounded by a quadratic function.\n",
    "\n",
    "Big-O notation also generalizes to functions of multiple variables. For\n",
    "example, the statement \"$4 mn - 30 m^{1.5} + 200n$ is $O(mn + m^{1.5})$\"\n",
    "holds because for any fixed value of $m > 1$, the big-O expression holds\n",
    "for $n$, and likewise for any fixed $n > 1$ the expression holds for\n",
    "$m$.\n",
    "\n",
    "\n",
    "## Graph search\n",
    "\n",
    "Given start and goal vertices, respectively $s,g \\in V$, the goal of\n",
    "graph search is to find a sequence of vertices:\n",
    "$$v_0 = s, v_1, \\ldots, v_k = g \\text{ such that }(v_{i-1},v_{i}) \\in E\\text{ for all }i=1,...k.$$\n",
    "The number of steps $k$ in the path is not fixed. If there does not\n",
    "exist a path (that is, $s$ and $g$ are disconnected), then search should\n",
    "return \"no path.\"\n",
    "\n",
    "We may also ascribe a notion of cost $c(u,v) > 0$ to each edge, in which\n",
    "case our goal is to find the *optimal path*, that is, the sequence of\n",
    "vertices such that the total path cost $$\\sum_{i=1}^{k} c(v_{i-1},v_i)$$\n",
    "is minimized among all paths connecting the start and goal. If no cost\n",
    "is given, then all edges are assumed to have uniform cost and we wish to\n",
    "optimize the total number of edges in a path.\n",
    "\n",
    "### Dijkstra's algorithm\n",
    "\n",
    "The most famous graph search method is *Dijkstra's algorithm*. It\n",
    "calculates an optimal path when one exists, and works in\n",
    "$O(|E| + |V| \\log |V|$) time and $O(|V|$) space, when implemented with\n",
    "an appropriate priority queue data structure. The general idea is to\n",
    "iterate through all unexplored vertices, ordered in increasing cost from\n",
    "the start (cost-to-come) $d[v]$ . All vertices have estimated\n",
    "cost-to-come set to $d[v] = \\infty$ at the beginning, except for the\n",
    "start vertex which has cost-to-come 0. At each iteration, the vertex $v$\n",
    "with the lowest cost-to-come is marked as explored, and the costs of all\n",
    "of $v$'s unexplored neighbors $w$ in the graph are updated if the path\n",
    "to $w$ through $(v,w)$ has a lower cost than the previous value $d[w]$.\n",
    "Pseudocode is given in\n",
    "[Algorithm Dijkstra's](#alg:Dijkstras).\n",
    "\n",
    "******************\n",
    "\n",
    "<a name=\"alg:Dijkstras\"></a>\n",
    "**Algorithm Dijkstras**$(G=(V,E),s,g)$\n",
    "\n",
    "1. $d[v] \\gets \\infty$ for all $v\\in V$\n",
    "* $d[s] \\gets 0$\n",
    "* $p[v] \\gets nil$ for all $v\\in V$\n",
    "* $Q \\gets \\{ s \\}$\n",
    "* **while** $Q$ is not empty **do**\n",
    "* &nbsp;&nbsp;&nbsp;$v \\gets \\text{vertex in } Q \\text{ that minimizes }d[v]$\n",
    "* &nbsp;&nbsp;&nbsp;$Q \\gets Q \\setminus \\{ v \\}$\n",
    "* &nbsp;&nbsp;&nbsp;**if** $v = g$\n",
    "* &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp;**return** the path leading to $g$ via the predecessor list $p$\n",
    "* &nbsp;&nbsp;&nbsp;**for** all $w$ such that $(v,w) \\in E$ **do**\n",
    "* &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp;$d_{cand} \\gets d[v] + d(v,w)$\n",
    "* &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp;**if** $d_{cand} < d[w]$\n",
    "* &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp;$d[w] \\gets d_{cand}$\n",
    "* &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp;$p[w] \\gets v$\n",
    "* &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp; &nbsp;&nbsp;&nbsp;$Q \\gets Q \\cup \\{w\\}$\n",
    "* **return** \"no path\"\n",
    "\n",
    "****************\n",
    "\n",
    "Here the *predecessor list* $p$ stores the previous vertex on the\n",
    "optimal path to a vertex, and at the end of the algorithm, it is\n",
    "traversed to provide the optimal path from $s$ to $g$. This traversal is\n",
    "given in\n",
    "[Algorithm Predecessor-Traversal](#alg:PredecessorTraversal):\n",
    "\n",
    "******************\n",
    "\n",
    "<a name=\"alg:PredecessorTraversal\"></a>\n",
    "**Algorithm PredecessorTraversal**$(p,s,g)$\n",
    "\n",
    "1. $L \\gets$ empty list\n",
    "* $v \\gets g$\n",
    "* **while** $v \\neq nil$ **do**\n",
    "* &nbsp;&nbsp;&nbsp;Prepend $v$ to $L$\n",
    "* &nbsp;&nbsp;&nbsp;$v \\gets p[v]$\n",
    "* **return** $L$\n",
    "\n",
    "******************\n",
    "\n",
    "It can be proven that Dijkstra's algorithm satisfies the invariant that any\n",
    "time a vertex is removed from $Q$, then its cost-to-come is the optimal\n",
    "amongst all paths to it. Furthermore, since costs are never negative,\n",
    "then the cost of the vertex in $Q$ with minimum cost always increases.\n",
    "In this way, Dijkstra's algorithm can be likened to a \"brush fire\" that\n",
    "fills in correct costs in a progressively expanding ring surrounding the\n",
    "start vertex. It is also guaranteed to terminate in finite time, since\n",
    "it will never expand the same vertex more than once."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Simple graph adj list: {'C': ['B', 'D'], 'D': [], 'A': ['B', 'C'], 'E': ['B', 'D'], 'B': ['E']}\n",
      "Running dijkstra's...\n",
      "Dijkstra's succeeded in 4 iterations\n",
      "Path in simple graph: ['A', 'C', 'D']\n"
     ]
    }
   ],
   "source": [
    "#Demonstration of Dijkstra's algorithm. Note that this code can be found in rsbook_code.utilities.search\n",
    "from __future__ import print_function,division\n",
    "\n",
    "#dijkstra's algorithm\n",
    "import heapq  #for a fast priority queue implementation\n",
    "\n",
    "class AdjListGraph:\n",
    "    \"\"\"A very simple adjacency list graph structure.  For higher performance use\n",
    "    in Python, you will probably want to learn a library like networkx, which will\n",
    "    have graph search algorithms built in.\"\"\"\n",
    "    def __init__(self,vertices,edges):\n",
    "        self.vertices = vertices\n",
    "        self.edges = dict((v,[]) for v in vertices)\n",
    "        for (v,w) in edges:\n",
    "            self.edges[v].append(w)\n",
    "    def neighbors(self,v):\n",
    "        return self.edges[v]\n",
    "\n",
    "def predecessor_traverse(p,s,g):\n",
    "    \"\"\"Used by dijkstra's algorithm to traverse a predecessor dictionary\"\"\"\n",
    "    L = []\n",
    "    v = g\n",
    "    while v is not None:\n",
    "        L.append(v)\n",
    "        v = p.get(v,None)\n",
    "    #rather than prepending, we appended and now we'll reverse.  This is a more efficient than prepending\n",
    "    return L[::-1]\n",
    "    \n",
    "def dijkstras(G,s,g,cost=(lambda v,w:1),verbose=1):\n",
    "    \"\"\"Completes a shortest-path search on graph G.\n",
    "    \n",
    "    Args:\n",
    "        G (AdjListGraph): the graph, given by an AdjListGraph\n",
    "        s: the start node\n",
    "        g: the goal node\n",
    "        cost (optional): a callback function c(v,w) that returns the edge cost\n",
    "        verbose (optional): if nonzero, will print information about search\n",
    "            progress.\n",
    "    \n",
    "    Returns:\n",
    "        list or None: either the path of nodes from s to g with minimum cost,\n",
    "        or None if no path exists.\n",
    "    \"\"\"\n",
    "    d = dict((v,float('inf')) for v in G.vertices)\n",
    "    p = dict((v,None) for v in G.vertices)\n",
    "    d[s] = 0\n",
    "    Q = [(0,s)]   #each element is a tuple (c,v) with c=cost from start, v=vertex\n",
    "    nnodes = 0\n",
    "    while len(Q) > 0:\n",
    "        c,v = heapq.heappop(Q)  #get the element in the queue with the least value of c\n",
    "        nnodes += 1\n",
    "        if v == g:\n",
    "            #found a path\n",
    "            if verbose: print(\"Dijkstra's succeeded in\",nnodes,\"iterations\")\n",
    "            return predecessor_traverse(p,s,g)\n",
    "        for w in G.neighbors(v):\n",
    "            dcand = d[v] + cost(v,w)   #this is the cost of going through v to w\n",
    "            if dcand < d[w]:\n",
    "                #going through v is optimal\n",
    "                #if the predecessor of w is not None, then we'll have to adjust the heap\n",
    "                if p[w] is not None:\n",
    "                    Q = [(c,x) for (c,x) in Q if x is not w]\n",
    "                    heapq.heapify(Q)\n",
    "                d[w] = dcand\n",
    "                p[w] = v\n",
    "                #put w on the queue\n",
    "                heapq.heappush(Q,(dcand,w))\n",
    "    #no path found\n",
    "    if verbose: print(\"Dijkstra's failed in\",nnodes,\"iterations\")\n",
    "    return None\n",
    "\n",
    "G = AdjListGraph(['A','B','C','D','E'],[('A','B'),('A','C'),('C','B'),('C','D'),('B','E'),('E','B'),('E','D')])\n",
    "print(\"Simple graph adj list:\",G.edges)\n",
    "print(\"Running dijkstra's...\")\n",
    "path = dijkstras(G,'A','D')\n",
    "print(\"Path in simple graph:\",path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Dijkstra's succeeded in 60 iterations\n",
      "Path on grid from (0,0) to (5,5): [(0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (0, 5), (1, 5), (2, 5), (3, 5), (4, 5), (5, 5)]\n"
     ]
    }
   ],
   "source": [
    "def make_grid_graph(M,N):\n",
    "    \"\"\"Makes a grid graph\"\"\"\n",
    "    G = AdjListGraph([],[])\n",
    "    for i in range(M):\n",
    "        for j in range(N):\n",
    "            n = (i,j)\n",
    "            G.vertices.append(n)\n",
    "            G.edges[n] = []\n",
    "            if i > 0:\n",
    "                G.edges[n].append((i-1,j))\n",
    "            if j > 0:\n",
    "                G.edges[n].append((i,j-1))\n",
    "            if i+1 < M:\n",
    "                G.edges[n].append((i+1,j))\n",
    "            if j+1 < N:\n",
    "                G.edges[n].append((i,j+1))\n",
    "    return G\n",
    "\n",
    "G = make_grid_graph(10,10)\n",
    "path = dijkstras(G,(0,0),(5,5))\n",
    "print(\"Path on grid from (0,0) to (5,5):\",path)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Heuristic search\n",
    "\n",
    "In many cases, the uniformly expanding strategy of Dijkstra's algorithm\n",
    "is a waste because it is known that the goal lies in a particular\n",
    "direction. To make search faster, it is possible to bias the search\n",
    "ordering using a *heuristic* that encodes an estimated distance to the\n",
    "goal. In particular, suppose we develop a *heuristic function* $h(v)$\n",
    "that evaluates an approximation of the cost from $v$ to $g$ (e.g., the\n",
    "length of the line segment from a configuration to the goal). Then, by\n",
    "replacing line 6 in Dijkstra's algorithm with the line:\n",
    "\n",
    "6. $v \\gets$ vertex in $Q$ that minimizes $d[v] + h(v)$\n",
    "\n",
    "we obtain a method called *$A^* search$*. This method is proven to\n",
    "calculate optimal paths under the conditions that $h(v)$ is *admissible*\n",
    "and *consistent* . Admissibility means that $h(v)$ is a lower bound on\n",
    "the true cost from $v$ to $g$ in the graph, and consistency means that\n",
    "$h(v)$ becomes more accurate as $v$ approaches $g$. (Specifically,\n",
    "$h(u) \\leq h(v) + c(u,w)$ for all edges $(u,v) \\in E$.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/markdown": [
       "### Euclidean distance heuristic"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 27 iterations\n",
      "Path, heuristic 1: [(0, 0), (0, 1), (1, 1), (1, 2), (2, 2), (2, 3), (3, 3), (3, 4), (4, 4), (4, 5), (5, 5)]\n"
     ]
    },
    {
     "data": {
      "text/markdown": [
       "### Manhattan distance heuristic"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 11 iterations\n",
      "Path, heuristic 2: [(0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (0, 5), (1, 5), (2, 5), (3, 5), (4, 5), (5, 5)]\n"
     ]
    }
   ],
   "source": [
    "#Below is code for the A* algorithm. Note that this code can be found in rsbook_code.utilities.search\n",
    "\n",
    "from IPython.display import display,Markdown\n",
    "\n",
    "def astar(G,s,g,cost=(lambda v,w:1),heuristic=(lambda v:0),verbose=1):\n",
    "    \"\"\"Completes an A* search on graph G.\n",
    "    \n",
    "    Args:\n",
    "        G (AdjListGraph): the graph, given by an AdjListGraph\n",
    "        s: the start node\n",
    "        g: the goal node\n",
    "        cost (optional): a callback function c(v,w) that returns the edge cost\n",
    "        heuristic (optional): a callback function h(v) that returns the \n",
    "            heuristic cost-to-go between v and g\n",
    "        verbose (optional): if nonzero, will print information about search\n",
    "            progress.\n",
    "    \n",
    "    Returns:\n",
    "        list or None: either the path of nodes from s to g with minimum cost,\n",
    "        or None if no path exists.\n",
    "    \"\"\"\n",
    "    d = dict((v,float('inf')) for v in G.vertices)\n",
    "    p = dict((v,None) for v in G.vertices)\n",
    "    d[s] = 0\n",
    "    Q = [(0,0,s)]   #each element is a tuple (f,-c,v) with f=c + heuristic(v), c=cost from start, v=vertex\n",
    "    nnodes = 0\n",
    "    while len(Q) > 0:\n",
    "        f,minus_c,v = heapq.heappop(Q)  #get the element in the queue with the least value of c\n",
    "        nnodes += 1\n",
    "        if v == g:\n",
    "            #found a path\n",
    "            if verbose: print(\"A* succeeded in\",nnodes,\"iterations\")\n",
    "            return predecessor_traverse(p,s,g)\n",
    "        for w in G.neighbors(v):\n",
    "            dcand = d[v] + cost(v,w)   #this is the cost of going through v to w\n",
    "            if dcand < d[w]:\n",
    "                #going through v is optimal\n",
    "                #if the predecessor of w is not None, then we'll have to adjust the heap\n",
    "                if p[w] is not None:\n",
    "                    Q = [(f,c,x) for (f,c,x) in Q if x is not w]\n",
    "                    heapq.heapify(Q)\n",
    "                d[w] = dcand\n",
    "                p[w] = v\n",
    "                #put w back on the queue, with the heuristic value as its priority\n",
    "                heapq.heappush(Q,(dcand+heuristic(w),-dcand,w))\n",
    "    #no path found\n",
    "    if verbose: print(\"A* failed in\",nnodes,\"iterations\")\n",
    "    return None\n",
    "\n",
    "import math\n",
    "G = make_grid_graph(10,10)\n",
    "start = (0,0)\n",
    "goal = (5,5)\n",
    "\n",
    "display(Markdown(\"### Euclidean distance heuristic\"))\n",
    "def h1(x):\n",
    "    \"\"\"Euclidean distance heuristic\"\"\"\n",
    "    return math.sqrt((x[0]-goal[0])**2+(x[1]-goal[1])**2)\n",
    "path = astar(G,start,goal,heuristic=h1)\n",
    "print(\"Path, heuristic 1:\",path)\n",
    "\n",
    "display(Markdown(\"### Manhattan distance heuristic\"))\n",
    "def h2(x):\n",
    "    \"\"\"Manhattan distance heuristic\"\"\"\n",
    "    return abs(x[0]-goal[0])+abs(x[1]-goal[1])\n",
    "path = astar(G,start,goal,heuristic=h2)\n",
    "print(\"Path, heuristic 2:\",path)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Search on infinite or large graphs\n",
    "\n",
    "Search can also be performed on an *implicit* graph that may be infinite\n",
    "or impractically large to construct in advance. The hope is that only a\n",
    "small portion of the graph needs to be explored in order to find a\n",
    "solution (or to prove that no path exists). We can do this by\n",
    "dynamically generating portions of the graph using the *successor\n",
    "function* $N(v)$ and using sparse data structures for $p$ and $d$ that\n",
    "do not store $nil$ and $\\infty$ values.\n",
    "\n",
    "In this way we only need to provide the start, goal, successor function\n",
    "$N$, and costs $c$. The algorithm will generate as little of the graph\n",
    "as necessary to find a solution path. However, if the graph is infinite\n",
    "and the goal cannot be reached, then the search may not terminate.\n",
    "\n",
    "Alternatively, one can construct a *search tree* that reconstructs a\n",
    "small part of $G$. Each node of the tree stores its successors, parent,\n",
    "and depth. However, to construct a search tree properly it is important\n",
    "to detect when the same vertex can be reached with multiple paths so as\n",
    "to keep only the node whose ancestors trace out the shortest path. This\n",
    "requires auxiliary techniques for *revisited state detection*.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/markdown": [
       "### Dijkstra, implicit graph"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 116171 iterations\n",
      "Path length 101 cost 9.99999999999998\n"
     ]
    },
    {
     "data": {
      "text/markdown": [
       "### Euclidean heuristic, implicit graph"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 6886 iterations\n",
      "Path length 101 cost 9.99999999999998\n"
     ]
    },
    {
     "data": {
      "text/markdown": [
       "### Manhattan heuristic, implicit graph"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 108 iterations\n",
      "Path length 101 cost 9.99999999999998\n"
     ]
    },
    {
     "data": {
      "text/markdown": [
       "### Dijkstra, 8-connected implicit graph"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 68441 iterations\n",
      "Path length 51 cost 7.071067811865471\n"
     ]
    },
    {
     "data": {
      "text/markdown": [
       "### Euclidean heuristic, 8-connected implicit graph"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 51 iterations\n",
      "Path length 51 cost 7.071067811865471\n"
     ]
    },
    {
     "data": {
      "text/markdown": [
       "### Manhattan heuristic, 8-connected implicit graph"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* succeeded in 51 iterations\n",
      "Path length 51 cost 7.071067811865471\n"
     ]
    }
   ],
   "source": [
    "#Below is code for the implicit A* algorithm. Note that this code can be found in rsbook_code.utilities.search\n",
    "\n",
    "def astar_implicit(successors,s,g,cost=(lambda v,w:1),heuristic=(lambda v:0),verbose=1):\n",
    "    \"\"\"Completes an A* search on a large/infinite implicit graph.\n",
    "    \n",
    "    Args:\n",
    "        successors: a callback function s(v) that returns a list of neighbors \n",
    "            of a node v.\n",
    "        s: the start node\n",
    "        g: the goal node or goal test\n",
    "        cost (optional): a callback function c(v,w) that returns the edge cost\n",
    "        heuristic (optional): a callback function h(v) that returns the \n",
    "            heuristic cost-to-go between v and g\n",
    "        verbose (optional): if nonzero, will print information about search\n",
    "            progress.\n",
    "    \n",
    "    Returns:\n",
    "        list or None: either the path of nodes from s to g with minimum cost,\n",
    "        or None if no path exists.\n",
    "    \"\"\"\n",
    "    if not callable(g):\n",
    "        gtest = lambda x,goal=g: x==g\n",
    "    else:\n",
    "        gtest = g\n",
    "    inf = float('inf')\n",
    "    d = dict()\n",
    "    p = dict()\n",
    "    d[s] = 0\n",
    "    Q = [(0,0,s)]   #each element is a tuple (f,-c,v) with f=c + heuristic(v), c=cost from start, v=vertex\n",
    "    nnodes = 0\n",
    "    while len(Q) > 0:\n",
    "        f,minus_c,v = heapq.heappop(Q)  #get the element in the queue with the least value of c\n",
    "        nnodes += 1\n",
    "        if gtest(v):\n",
    "            #found a path\n",
    "            if verbose: print(\"A* succeeded in\",nnodes,\"iterations\")\n",
    "            return predecessor_traverse(p,s,v)\n",
    "        for w in successors(v):\n",
    "            dcand = d[v] + cost(v,w)   #this is the cost of going through v to w\n",
    "            if dcand < d.get(w,float('inf')):\n",
    "                #going through v is optimal\n",
    "                #if the predecessor of w is not None, then we'll have to adjust the heap\n",
    "                if w in p:\n",
    "                    Q = [(f,c,x) for (f,c,x) in Q if x is not w]\n",
    "                    heapq.heapify(Q)\n",
    "                d[w] = dcand\n",
    "                p[w] = v\n",
    "                #put w back on the queue, with the heuristic value as its priority\n",
    "                heapq.heappush(Q,(dcand+heuristic(w),-dcand,w))\n",
    "    #no path found\n",
    "    if verbose: print(\"A* failed in\",nnodes,\"iterations\")\n",
    "    return None\n",
    "\n",
    "import math\n",
    "\n",
    "#defines a fairly dense graph, with step 0.1\n",
    "delta = 0.1\n",
    "#defines a coarse graph, with step 1\n",
    "#delta = 1\n",
    "\n",
    "start = (0,0)\n",
    "goal = (5,5)\n",
    "\n",
    "def length_cost(v,w):\n",
    "    \"\"\"Euclidean length\"\"\"\n",
    "    return math.sqrt(sum((a-b)**2 for (a,b) in zip(v,w)))\n",
    "\n",
    "def successors(n):\n",
    "    c1 = (n[0]+delta,n[1])\n",
    "    c2 = (n[0]-delta,n[1])\n",
    "    c3 = (n[0],n[1]+delta)\n",
    "    c4 = (n[0],n[1]-delta)\n",
    "    return [c1,c2,c3,c4]\n",
    "\n",
    "def successors_8connected(n):\n",
    "    c5 = (n[0]+delta,n[1]+delta)\n",
    "    c6 = (n[0]-delta,n[1]+delta)\n",
    "    c7 = (n[0]-delta,n[1]+delta)\n",
    "    c8 = (n[0]-delta,n[1]-delta)\n",
    "    return successors(n) + [c5,c6,c7,c8]\n",
    "\n",
    "#may be needed for rounding errors\n",
    "def goaltest(n):\n",
    "    return all(abs(xn-xg) < delta*0.5 for (xn,xg) in zip(n,goal))\n",
    "\n",
    "\n",
    "display(Markdown(\"### Dijkstra, implicit graph\"))\n",
    "path = astar_implicit(successors,start,goaltest,cost=length_cost)\n",
    "print(\"Path length\",len(path),\"cost\",sum(length_cost(a,b) for (a,b) in zip(path[:-1],path[1:])))\n",
    "\n",
    "display(Markdown(\"### Euclidean heuristic, implicit graph\"))\n",
    "def h1(x):\n",
    "    \"\"\"Euclidean distance heuristic\"\"\"\n",
    "    return math.sqrt((x[0]-goal[0])**2+(x[1]-goal[1])**2)\n",
    "path = astar_implicit(successors,start,goaltest,cost=length_cost,heuristic=h1)\n",
    "print(\"Path length\",len(path),\"cost\",sum(length_cost(a,b) for (a,b) in zip(path[:-1],path[1:])))\n",
    "\n",
    "display(Markdown(\"### Manhattan heuristic, implicit graph\"))\n",
    "def h2(x):\n",
    "    \"\"\"Manhattan distance heuristic\"\"\"\n",
    "    return abs(x[0]-goal[0])+abs(x[1]-goal[1])\n",
    "path = astar_implicit(successors,start,goaltest,cost=length_cost,heuristic=h2)\n",
    "print(\"Path length\",len(path),\"cost\",sum(length_cost(a,b) for (a,b) in zip(path[:-1],path[1:])))\n",
    "\n",
    "display(Markdown(\"### Dijkstra, 8-connected implicit graph\"))\n",
    "path = astar_implicit(successors_8connected,start,goaltest,cost=length_cost)\n",
    "print(\"Path length\",len(path),\"cost\",sum(length_cost(a,b) for (a,b) in zip(path[:-1],path[1:])))\n",
    "\n",
    "display(Markdown(\"### Euclidean heuristic, 8-connected implicit graph\"))\n",
    "def h1(x):\n",
    "    \"\"\"Euclidean distance heuristic\"\"\"\n",
    "    return math.sqrt((x[0]-goal[0])**2+(x[1]-goal[1])**2)\n",
    "path = astar_implicit(successors_8connected,start,goaltest,cost=length_cost,heuristic=h1)\n",
    "print(\"Path length\",len(path),\"cost\",sum(length_cost(a,b) for (a,b) in zip(path[:-1],path[1:])))\n",
    "\n",
    "display(Markdown(\"### Manhattan heuristic, 8-connected implicit graph\"))\n",
    "def h2(x):\n",
    "    \"\"\"Manhattan distance heuristic\"\"\"\n",
    "    return abs(x[0]-goal[0])+abs(x[1]-goal[1])\n",
    "path = astar_implicit(successors_8connected,start,goaltest,cost=length_cost,heuristic=h2)\n",
    "print(\"Path length\",len(path),\"cost\",sum(length_cost(a,b) for (a,b) in zip(path[:-1],path[1:])))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Multi-source / multi-goal search\n",
    "\n",
    "It is also easy to perform a multi-source and/or multi-goal graph\n",
    "search, that is, to find the shortest path between any source and goal\n",
    "in some designated sets. Given graph $G$, a set of possible start\n",
    "vertices $S \\subset V$, and a set of goal vertices $T \\subset V$, we can\n",
    "simply construct a new graph $G^\\prime= (V^\\prime, E^\\prime)$ augmented\n",
    "with a virtual start vertex $s$ and a virtual goal vertex $g$ that are\n",
    "connected to $S$ and $T$ respectively.\n",
    "\n",
    "More precisely, let $V^\\prime = V \\cup \\{s,g\\}$ and\n",
    "$E^\\prime = E \\cup \\{ (s,v)\\quad | \\quad v \\in S \\} \\cup \\{ (v,g) \\quad |\\quad v\\in T\\}$.\n",
    "A search on $G^\\prime$ will yield a path that passes through the optimal\n",
    "path amongst all pairs of vertices in $S$ and $T$.\n",
    "\n",
    "Another way of implementing this is to replace in Dijkstra's algorithm the goal test in Line 8 with the condition **if** $v \\in T$, and then set $g=v$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": true,
   "autoclose": false,
   "autocomplete": true,
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 1,
   "hotkeys": {
    "equation": "Ctrl-E",
    "itemize": "Ctrl-I"
   },
   "labels_anchors": false,
   "latex_user_defs": true,
   "report_style_numbering": false,
   "user_envs_cfg": false
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": true,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": true
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}