S65_driving_planning.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "view-in-github",
    "tags": [
     "no-tex"
    ]
   },
   "source": [
    "<a href=\"https://colab.research.google.com/github/gtbook/robotics/blob/main/S65_driving_planning.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "id": "JoW4C_OkOMhe",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install -q -U gtbook\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 87,
   "metadata": {
    "id": "10-snNDwOSuC",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "# no imports (yet)\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "# Planning for Autonomous Driving.\n",
    "\n",
    "> Motion primitives provide a computationally efficient tool for fast, local motion planning.\n",
    "\n",
    "<img src=\"Figures6/S65-Autonomous_Vehicle_with_LIDAR_and_cameras-02.jpg\" alt=\"Splash image with steampunk autonomous car\" width=\"40%\" align=center style=\"vertical-align:middle;margin:10px 0px\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "In previous chapters, we have mainly considered two kinds of planning problems.\n",
    "For the trash sorting robot, vacuum cleaning robot, and warehouse robot, we focused\n",
    "on the problem of making the best decisions in the presence of uncertainty.\n",
    "In these problems, we used probability theory to quantify uncertainty,\n",
    "and developed policies to maximize the expected benefit (or to minimize the expected cost)\n",
    "of executing actions in a given state.\n",
    "In contrast, for the differential drive robot (DDR), we considered the purely geometric\n",
    "problem of planning collision-free paths.\n",
    "\n",
    "A common characteristic of these is that each addresses a global problem.\n",
    "For MDPs, we used value or policy iteration to establish a policy over the entire state space.\n",
    "For DDRs, we searched the entire configuration space for a collision-free path.\n",
    "Furthermore, the methods we developed for both problems were completely general.\n",
    "Our probabilistic approaches work for arbitrary probability distributions, reward functions,\n",
    "and system dynamics.\n",
    "Our geometric approaches to path planning work for arbitrary environments,\n",
    "and can easily be extended to robots with complex dynamics (e.g., we will extend RRTs to\n",
    "the case of drones in the next chapter).\n",
    "\n",
    "Methods that address global problems in broad generality often require significant computational\n",
    "resources and significant computation time.\n",
    "This can render such methods ineffective for situations in which real-time adaptivity\n",
    "is required over short time horizons, or in local regions of the state space.\n",
    "These conditions are exactly those confronted by self-driving cars,\n",
    "and for this reason, in this chapter we introduce a new approach,\n",
    "one that exploits precomputed motion primitives,\n",
    "for motion planning."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Motion Primitives\n",
    "\n",
    "To this point, we have considered two approaches for quantifying motions.\n",
    "For all of our probabilistic methods, we used a discrete time formulation and considered\n",
    "the effects of executing an action (e.g., move forward, move left) for a small duration of time, $\\Delta t$.\n",
    "To plan collision-free paths, we considered artificial potential fields and RRTs, both of which\n",
    "use short straight-line paths in the configuration space to connect configurations (small gradient descent\n",
    "steps for potential fields, and steering toward $q_\\mathrm{rand}$ for RRTs).\n",
    "In each case, the language of path segments is very simple, and in each case,\n",
    "a full plan will consist of many sequential steps.\n",
    "\n",
    "This approach can be very inefficient for planning long trajectories that have well-defined\n",
    "properties.\n",
    "For example, consider a car traveling in reverse that wishes to suddenly change it's orientation\n",
    "by completing a rapid 180-degree turn (a favorite maneuver for drivers like James Bond and Steve McQueen).\n",
    "This maneuver can be achieved by a predefined\n",
    "sequence of steps: after achieving a reasonable speed, remove your foot from the gas pedal;\n",
    "turn left sharply and hit the breaks; at the perfect moment, release the breaks\n",
    "and straighten the wheel.\n",
    "When stunt drivers execute this maneuver, they do not plan step-by-step what to do.\n",
    "Rather, they have pre-compiled this sequence of steps into a basic action that can be executed\n",
    "with little reasoning.\n",
    "This is the basic idea of **motion primitives**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Motion primitives can be defined in numerous ways.\n",
    "We could specify a geometric curve without consideration of time or dynamics\n",
    "(e.g., for a parallel parking robot, we might define an\n",
    "initial curve to move the car from the street into an empty parking spot).\n",
    "In cases where dynamics are significant (e.g., in drone flight), we might specify\n",
    "a feedback control law to be executed from an initial state until some final state is achieved.\n",
    "We might parameterize these primitives by duration, by geometric properties (e.g., angle, distance),\n",
    "or by state feedback conditions.\n",
    "This idea is illustrated in the figure below, which shows four motion primitives\n",
    "for a car.\n",
    "The primitive $P_1$ corresponds to driving forward, while motion primitives $P_2$, $P_3$, and $P_4$ correspond to veering\n",
    "to the left at increasingly sharp angles."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<figure id=\"fig:MotionPrimitives\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/motion-primitives.png?raw=1\" style=\"width:18cm\" alt=\"\">\n",
    "<figcaption>Four motion primitives for a car veering to its left. </figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Planning using Motion Primitives\n",
    "\n",
    "The use of motion primitives can greatly reduce the cost of planning, since the set \n",
    "of actions available at any moment in time is small and easily computed.\n",
    "For the car example above, if we assume a symmetric set of motion primitives for veering to the right,\n",
    "motion planning can be reduced to choosing from this set of seven possible actions at each moment in time.\n",
    "If, for example, there is a slow moving car just ahead, it might be advantageous to change lanes using one of\n",
    "$P_2$, $P_3$, or $P_4$.\n",
    "If there is a rapidly approaching oncoming car, it might be best to use $P_2$, to delay changing lanes\n",
    "until that car has passed by.\n",
    "\n",
    "More generally, a motion primitive typically includes a set of conditions that define when\n",
    "the primitive is applicable, and a set of possible transitions to other motion primitives.\n",
    "For example, it would be reasonable to veer left slightly and then drive straight, but it would\n",
    "not be reasonable to transition from forward motion to reverse motion without some intermediate\n",
    "maneuvering.\n",
    "\n",
    "Under these conditions, planning can be effected by a generate-and-test approach.\n",
    "At each moment in time, the planner considers the current situation, enumerates the valid\n",
    "motion primitives (using preconditions for execution and set of valid transitions), and evaluates\n",
    "the benefit of each admissible candidate motion primitive. This approach can be effective for\n",
    "problems such as highway driving, where local context is all that is necessary for making decisions.\n",
    "For example, the traffic outside the Atlanta perimeter is irrelevant when leaving the downtown on\n",
    "a trip to Chicago.\n",
    "In this case, immediate driving decisions depend on the car just ahead, and the nearby\n",
    "cars in adjacent lanes."
   ]
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "include_colab_link": true,
   "name": "S65_driving_decision_theory.ipynb",
   "provenance": []
  },
  "interpreter": {
   "hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d"
  },
  "kernelspec": {
   "display_name": "Python 3.9.6 64-bit ('base': conda)",
   "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.8.12"
  },
  "latex_metadata": {
   "affiliation": "Georgia Institute of Technology",
   "author": "Frank Dellaert and Seth Hutchinson",
   "title": "Introduction to Robotics"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}