A-Star-City-Solution.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Finding Your Way In The City\n",
    "\n",
    "In this notebook you'll combine the work of previous exercises to calculate a minimal series of waypoints in order to get from a start location to a goal location.\n",
    "\n",
    "You'll reuse and modify your algorithms from:\n",
    "\n",
    "- A*\n",
    "- Configuration Space\n",
    "- Collinearity and/or Bresenham"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from grid import create_grid\n",
    "from planning import a_star\n",
    "\n",
    "%matplotlib inline \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.rcParams['figure.figsize'] = 12, 12"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You'll notice we've imported `create_grid`, and `a_star`. These are functions you've implemented in previous exercises, you'll use them to create a map and find a path from a starting position to a goal position.\n",
    "\n",
    "To read the function signature and documentation execute `?` followed by the function name in a cell. In the example below we'll check the documentation for `create_grid`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "?create_grid"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you would like to alter these implementations, you can modify [grid.py](/edit/grid.py) and [planning.py](/edit/planning.py) in the current directory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-310.2389   -439.2315     85.5         5.          5.         85.5     ]\n",
      " [-300.2389   -439.2315     85.5         5.          5.         85.5     ]\n",
      " [-290.2389   -439.2315     85.5         5.          5.         85.5     ]\n",
      " ...\n",
      " [ 257.8061    425.1645      1.75852     1.292725    1.292725    1.944791]\n",
      " [ 293.9967    368.3391      3.557666    1.129456    1.129456    3.667319]\n",
      " [ 281.5162    354.4156      4.999351    1.053772    1.053772    4.950246]]\n"
     ]
    }
   ],
   "source": [
    "# This is the same obstacle data from the previous lesson.\n",
    "filename = 'colliders.csv'\n",
    "data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)\n",
    "print(data)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Static drone altitude (meters)\n",
    "drone_altitude = 5\n",
    "\n",
    "# Minimum distance stay away from obstacle (meters)\n",
    "safe_distance = 3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# TODO: Use `create_grid` to create a grid configuration space of\n",
    "# the obstacle data.\n",
    "grid = create_grid(data, drone_altitude, safe_distance)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "Image data cannot be converted to float",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-7-365d656c8a6b>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[0;32m      1\u001b[0m \u001b[1;31m# equivalent to\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      2\u001b[0m \u001b[1;31m# plt.imshow(np.flip(grid, 0))\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 3\u001b[1;33m \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mimshow\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mgrid\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0morigin\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;34m'lower'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      4\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mxlabel\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'EAST'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\pyplot.py\u001b[0m in \u001b[0;36mimshow\u001b[1;34m(X, cmap, norm, aspect, interpolation, alpha, vmin, vmax, origin, extent, shape, filternorm, filterrad, imlim, resample, url, hold, data, **kwargs)\u001b[0m\n\u001b[0;32m   3099\u001b[0m                         \u001b[0mfilternorm\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mfilternorm\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mfilterrad\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mfilterrad\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   3100\u001b[0m                         \u001b[0mimlim\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mimlim\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mresample\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mresample\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0murl\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0murl\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mdata\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mdata\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 3101\u001b[1;33m                         **kwargs)\n\u001b[0m\u001b[0;32m   3102\u001b[0m     \u001b[1;32mfinally\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   3103\u001b[0m         \u001b[0max\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m_hold\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mwashold\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\__init__.py\u001b[0m in \u001b[0;36minner\u001b[1;34m(ax, *args, **kwargs)\u001b[0m\n\u001b[0;32m   1715\u001b[0m                     warnings.warn(msg % (label_namer, func.__name__),\n\u001b[0;32m   1716\u001b[0m                                   RuntimeWarning, stacklevel=2)\n\u001b[1;32m-> 1717\u001b[1;33m             \u001b[1;32mreturn\u001b[0m \u001b[0mfunc\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0max\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m*\u001b[0m\u001b[0margs\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0mkwargs\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   1718\u001b[0m         \u001b[0mpre_doc\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0minner\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m__doc__\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   1719\u001b[0m         \u001b[1;32mif\u001b[0m \u001b[0mpre_doc\u001b[0m \u001b[1;32mis\u001b[0m \u001b[1;32mNone\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\axes\\_axes.py\u001b[0m in \u001b[0;36mimshow\u001b[1;34m(self, X, cmap, norm, aspect, interpolation, alpha, vmin, vmax, origin, extent, shape, filternorm, filterrad, imlim, resample, url, **kwargs)\u001b[0m\n\u001b[0;32m   5125\u001b[0m                               resample=resample, **kwargs)\n\u001b[0;32m   5126\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 5127\u001b[1;33m         \u001b[0mim\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_data\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mX\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   5128\u001b[0m         \u001b[0mim\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_alpha\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0malpha\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   5129\u001b[0m         \u001b[1;32mif\u001b[0m \u001b[0mim\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mget_clip_path\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m \u001b[1;32mis\u001b[0m \u001b[1;32mNone\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\image.py\u001b[0m in \u001b[0;36mset_data\u001b[1;34m(self, A)\u001b[0m\n\u001b[0;32m    605\u001b[0m         if (self._A.dtype != np.uint8 and\n\u001b[0;32m    606\u001b[0m                 not np.can_cast(self._A.dtype, float, \"same_kind\")):\n\u001b[1;32m--> 607\u001b[1;33m             \u001b[1;32mraise\u001b[0m \u001b[0mTypeError\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m\"Image data cannot be converted to float\"\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    608\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    609\u001b[0m         if not (self._A.ndim == 2\n",
      "\u001b[1;31mTypeError\u001b[0m: Image data cannot be converted to float"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x2e018a3f7b8>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# equivalent to\n",
    "# plt.imshow(np.flip(grid, 0))\n",
    "plt.imshow(grid, origin='lower') \n",
    "\n",
    "plt.xlabel('EAST')\n",
    "plt.ylabel('NORTH')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next you'll compute the path from a start location to a goal location using A*."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Start and goal coordinates in *(north, east)*."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "start_ne = (25,  100)\n",
    "goal_ne = (750., 370.)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Write a heuristic function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def heuristic_func(position, goal_position):\n",
    "    return np.abs(position[0] - goal_position[0]) + np.abs(position[1] - goal_position[1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compute the lowest cost path with `a_star`. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "ename": "AttributeError",
     "evalue": "'tuple' object has no attribute 'shape'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mAttributeError\u001b[0m                            Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-10-ff589b8e9b35>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[0mpath\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mcost\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0ma_star\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mgrid\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mheuristic_func\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mstart_ne\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mgoal_ne\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      2\u001b[0m \u001b[0mprint\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mlen\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mpath\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mcost\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\FCND\\First Term - Aerial Robotics\\FCND-Motion-Planning\\planning.py\u001b[0m in \u001b[0;36ma_star\u001b[1;34m(grid, h, start, goal)\u001b[0m\n\u001b[0;32m    169\u001b[0m             \u001b[1;32mbreak\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    170\u001b[0m         \u001b[1;32melse\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 171\u001b[1;33m             \u001b[1;32mfor\u001b[0m \u001b[0maction\u001b[0m \u001b[1;32min\u001b[0m \u001b[0mvalid_actions\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mgrid\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mcurrent_node\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    172\u001b[0m                 \u001b[1;31m# get the tuple representation\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    173\u001b[0m                 \u001b[0mda\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0maction\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mdelta\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\FCND\\First Term - Aerial Robotics\\FCND-Motion-Planning\\planning.py\u001b[0m in \u001b[0;36mvalid_actions\u001b[1;34m(grid, current_node)\u001b[0m\n\u001b[0;32m     62\u001b[0m     \"\"\"\n\u001b[0;32m     63\u001b[0m     \u001b[0mvalid\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mlist\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mAction\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 64\u001b[1;33m     \u001b[0mn\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mm\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mgrid\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mshape\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m \u001b[1;33m-\u001b[0m \u001b[1;36m1\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mgrid\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mshape\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m1\u001b[0m\u001b[1;33m]\u001b[0m \u001b[1;33m-\u001b[0m \u001b[1;36m1\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     65\u001b[0m     \u001b[0mx\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0my\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mcurrent_node\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     66\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mAttributeError\u001b[0m: 'tuple' object has no attribute 'shape'"
     ]
    }
   ],
   "source": [
    "path, cost = a_star(grid, heuristic_func, start_ne, goal_ne)\n",
    "print(len(path), cost)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's plot the path!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "Image data cannot be converted to float",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-11-1733def4d5ac>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mimshow\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mgrid\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mcmap\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;34m'Greys'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0morigin\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;34m'lower'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      2\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      3\u001b[0m \u001b[1;31m# For the purposes of the visual the east coordinate lay along\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      4\u001b[0m \u001b[1;31m# the x-axis and the north coordinates long the y-axis.\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mplot\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mstart_ne\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m1\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mstart_ne\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;34m'x'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\pyplot.py\u001b[0m in \u001b[0;36mimshow\u001b[1;34m(X, cmap, norm, aspect, interpolation, alpha, vmin, vmax, origin, extent, shape, filternorm, filterrad, imlim, resample, url, hold, data, **kwargs)\u001b[0m\n\u001b[0;32m   3099\u001b[0m                         \u001b[0mfilternorm\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mfilternorm\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mfilterrad\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mfilterrad\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   3100\u001b[0m                         \u001b[0mimlim\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mimlim\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mresample\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mresample\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0murl\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0murl\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mdata\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mdata\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 3101\u001b[1;33m                         **kwargs)\n\u001b[0m\u001b[0;32m   3102\u001b[0m     \u001b[1;32mfinally\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   3103\u001b[0m         \u001b[0max\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m_hold\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mwashold\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\__init__.py\u001b[0m in \u001b[0;36minner\u001b[1;34m(ax, *args, **kwargs)\u001b[0m\n\u001b[0;32m   1715\u001b[0m                     warnings.warn(msg % (label_namer, func.__name__),\n\u001b[0;32m   1716\u001b[0m                                   RuntimeWarning, stacklevel=2)\n\u001b[1;32m-> 1717\u001b[1;33m             \u001b[1;32mreturn\u001b[0m \u001b[0mfunc\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0max\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m*\u001b[0m\u001b[0margs\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0mkwargs\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   1718\u001b[0m         \u001b[0mpre_doc\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0minner\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m__doc__\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   1719\u001b[0m         \u001b[1;32mif\u001b[0m \u001b[0mpre_doc\u001b[0m \u001b[1;32mis\u001b[0m \u001b[1;32mNone\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\axes\\_axes.py\u001b[0m in \u001b[0;36mimshow\u001b[1;34m(self, X, cmap, norm, aspect, interpolation, alpha, vmin, vmax, origin, extent, shape, filternorm, filterrad, imlim, resample, url, **kwargs)\u001b[0m\n\u001b[0;32m   5125\u001b[0m                               resample=resample, **kwargs)\n\u001b[0;32m   5126\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 5127\u001b[1;33m         \u001b[0mim\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_data\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mX\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   5128\u001b[0m         \u001b[0mim\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_alpha\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0malpha\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   5129\u001b[0m         \u001b[1;32mif\u001b[0m \u001b[0mim\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mget_clip_path\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m \u001b[1;32mis\u001b[0m \u001b[1;32mNone\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Anaconda\\envs\\fcnd\\lib\\site-packages\\matplotlib\\image.py\u001b[0m in \u001b[0;36mset_data\u001b[1;34m(self, A)\u001b[0m\n\u001b[0;32m    605\u001b[0m         if (self._A.dtype != np.uint8 and\n\u001b[0;32m    606\u001b[0m                 not np.can_cast(self._A.dtype, float, \"same_kind\")):\n\u001b[1;32m--> 607\u001b[1;33m             \u001b[1;32mraise\u001b[0m \u001b[0mTypeError\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m\"Image data cannot be converted to float\"\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    608\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    609\u001b[0m         if not (self._A.ndim == 2\n",
      "\u001b[1;31mTypeError\u001b[0m: Image data cannot be converted to float"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x2e019a6c748>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.imshow(grid, cmap='Greys', origin='lower')\n",
    "\n",
    "# For the purposes of the visual the east coordinate lay along\n",
    "# the x-axis and the north coordinates long the y-axis.\n",
    "plt.plot(start_ne[1], start_ne[0], 'x')\n",
    "plt.plot(goal_ne[1], goal_ne[0], 'x')\n",
    "\n",
    "pp = np.array(path)\n",
    "plt.plot(pp[:, 1], pp[:, 0], 'g')\n",
    "\n",
    "plt.xlabel('EAST')\n",
    "plt.ylabel('NORTH')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pretty neat! Unfortunately this path is impractical, the drone would be constantly stopping and going since each grid cell visited is synonymous with a waypoint. Ideally, we'd like to only consider a new waypoint when the drone's direction changes. Path pruning is one way to achieve this result."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Path Pruning"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def point(p):\n",
    "    return np.array([p[0], p[1], 1.]).reshape(1, -1)\n",
    "\n",
    "def collinearity_check(p1, p2, p3, epsilon=1e-6):   \n",
    "    m = np.concatenate((p1, p2, p3), 0)\n",
    "    det = np.linalg.det(m)\n",
    "    return abs(det) < epsilon"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Complete the `prune_path` function below. It should return a new path much shorter than the original."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We're using collinearity here, but you could use Bresenham as well!\n",
    "def prune_path(path):\n",
    "    pruned_path = [p for p in path]\n",
    "    # TODO: prune the path!\n",
    "    \n",
    "    i = 0\n",
    "    while i < len(pruned_path) - 2:\n",
    "        p1 = point(pruned_path[i])\n",
    "        p2 = point(pruned_path[i+1])\n",
    "        p3 = point(pruned_path[i+2])\n",
    "        \n",
    "        # If the 3 points are in a line remove\n",
    "        # the 2nd point.\n",
    "        # The 3rd point now becomes and 2nd point\n",
    "        # and the check is redone with a new third point\n",
    "        # on the next iteration.\n",
    "        if collinearity_check(p1, p2, p3):\n",
    "            # Something subtle here but we can mutate\n",
    "            # `pruned_path` freely because the length\n",
    "            # of the list is check on every iteration.\n",
    "            pruned_path.remove(pruned_path[i+1])\n",
    "        else:\n",
    "            i += 1\n",
    "    return pruned_path"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Prune the path."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pruned_path = prune_path(path)\n",
    "print(len(pruned_path))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pruned_path"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Replot the path, it will be the same as before but the drone flight will be much smoother."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.imshow(grid, cmap='Greys', origin='lower')\n",
    "\n",
    "plt.plot(start_ne[1], start_ne[0], 'x')\n",
    "plt.plot(goal_ne[1], goal_ne[0], 'x')\n",
    "\n",
    "pp = np.array(pruned_path)\n",
    "plt.plot(pp[:, 1], pp[:, 0], 'g')\n",
    "plt.scatter(pp[:, 1], pp[:, 0])\n",
    "\n",
    "plt.xlabel('EAST')\n",
    "plt.ylabel('NORTH')\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now the waypoints symbolize a change in direction, much better!"
   ]
  },
  {
   "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.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}