S65_driving_planning.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "UcRF7OziizF1",
   "metadata": {},
   "source": [
    "# Planning for Autonomous Driving."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "n3VgwxkoVsSX",
   "metadata": {
    "colab_type": "text",
    "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,
   "id": "tLBxSLGeWPV0",
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install -q -U gtbook\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 87,
   "id": "ewrl5k4_akQV",
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "# no imports (yet)\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "BMwycfX51DYm",
   "metadata": {},
   "source": [
    "```{index} planning; motion primitives\n",
    "```\n",
    "\n",
    "> Motion primitives provide a computationally efficient tool for fast, local motion planning."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5a9cfa79",
   "metadata": {
    "tags": [
     "no-pdf"
    ]
   },
   "source": [
    "<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",
   "id": "ilM9SxRJ2VT0",
   "metadata": {},
   "source": [
    "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. Even though this problem did not consider uncertainty,\n",
    "the computational complexity of the problem precludes exact, complete solutions for all\n",
    "but the simplest problems, leading to the introduction of sampling-based methods.\n",
    "\n",
    "A common characteristic of all planning methods described to this point\n",
    "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",
    "However, methods that address global problems in broad generality often require significant computational\n",
    "resources (both processing power and computation time).\n",
    "This can render such methods ineffective for situations in which real-time adaptivity\n",
    "is required over short time horizons.\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 motion primitives for motion planning."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "R0sQPSK681cf",
   "metadata": {},
   "source": [
    "```{index} motion primitives\n",
    "```\n",
    "## Motion Primitives\n",
    "\n",
    "Consider a car traveling in reverse that wishes to suddenly change its orientation\n",
    "by completing a rapid 180-degree turn (a favorite maneuver for drivers like James Bond and Steve McQueen).\n",
    "How would we go about implementing this type of maneuver in an autonomous vehicle?\n",
    "\n",
    "This two approaches we have considered before can be very inefficient for planning trajectories that have such well-defined\n",
    "characteristics.\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",
    "Instead, the U-turn maneuver could 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.  This is the basic idea of **motion primitives**."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "53y_6iTD1Ptz",
   "metadata": {},
   "source": [
    "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",
   "id": "y_GGvtQc94pI",
   "metadata": {},
   "source": [
    "```{index} polynomial trajectories, splines\n",
    "```\n",
    "\n",
    "<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>\n",
    "\n",
    "Motion primitives can be defined in numerous ways.\n",
    "The figure above illustrates four fixed motion primitives, but it would not be difficult to generalize each of these\n",
    "to a class of motions by using parametric descriptions. \n",
    "We could specify a geometric curve without consideration of time or dynamics.\n",
    "For example, 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 additionally specify\n",
    "a feedback control law to be executed from an initial state until some final state is achieved.\n",
    "We can parameterize these primitives by duration, by geometric properties (e.g., angle, distance),\n",
    "or by state feedback conditions.\n",
    "Below, we will see how to do this by using **polynomial trajectories**, which generalize to **splines**."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7GTgSAjpHFBn",
   "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 in rural Georgia is irrelevant when leaving downtown Atlanta on\n",
    "a trip to Boston.\n",
    "In this case, immediate driving decisions depend on the car just ahead, and the nearby\n",
    "cars in adjacent lanes.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ka--rdf1gpsg",
   "metadata": {},
   "source": [
    "##  Polynomial Trajectories\n",
    "\n",
    "Let’s begin with the simple problem of changing lanes along a straight stretch of highway. The situation is illustrated in the figure below.\n",
    "\n",
    "<figure id=\"fig:LaneChange\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/lane-change.png?raw=1\" style=\"width:18cm\" alt=\"\">\n",
    "<figcaption>Initial and final configuration for a lane change maneuver. </figcaption>\n",
    "</figure>\n",
    "\n",
    "Here, we have taken the $s$-axis to be longitudinal direction (parallel to the highway), and the $d$-axis is along the lateral direction (perpendicular to the highway).  This choice of coordinates will be convenient below, when we generalize trajectories to arbitrary curves. In addition, for now let us assume that the car speed satisfies $s=t$.\n",
    "Below, we will generalize further by defining $s$ to be the distance along the path\n",
    "(instead of a linear distance along a straight lane), and\n",
    "$s(t)$ to be the time parameterization of the path.\n",
    "\n",
    "For now, let us assume that the lane change maneuver begins at $s=0$, $d=0$, and that the desired ending configuration is $ s = s_\\mathrm{g}, d= d_\\mathrm{g}$, i.e., at the end of the maneuver (at $ s = s_\\mathrm{g}$), \n",
    "the car will have reached the center of the left lane (i.e., $ d = d_\\mathrm{g}$).\n",
    "\n",
    "At first glance, it might seem that this could be accomplished by a simple linear trajectory of the form\n",
    "\\begin{equation}\n",
    " d(x) = \\frac{ d_\\mathrm{g}}{s_\\mathrm{g} } s \n",
    "\\end{equation}\n",
    "At the start of the maneuver, $s=0$, which matches the initial condition $d(0)=0$, and at $ s = s_\\mathrm{g}$\n",
    "we match the end condition $d(s_\\mathrm{g}) = d_\\mathrm{g}$.\n",
    "This trajectory is illustrated in the figure below.\n",
    "\n",
    "<figure id=\"fig:LinearLaneChange\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/linear-lane-change.png?raw=1\" style=\"width:18cm\" alt=\"\">\n",
    "<figcaption>Linear trajectory that attempts to achieve a lane change. </figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7XqCDRxIfCpD",
   "metadata": {},
   "source": [
    "Note that this trajectory requires an instantaneous change in the car’s heading, from 0 at $s = 0^-$ to\n",
    "$\\frac{ d_\\mathrm{g}}{s_\\mathrm{g} }$ at $s=0^+$. Cars, of course, are not capable of executing maneuvers with velocity discontinuities (under normal driving conditions), so this trajectory could not be executed by a normal car.\n",
    "However, it is not difficult to generalize this approach to solve this problem.\n",
    "\n",
    "Consider the $m^{th}$ order polynomial \n",
    "\\begin{equation}\n",
    " d(s) = \\sum_{i=0}^m \\alpha_i s^i \n",
    "\\end{equation}\n",
    "For $m=1$, we obtain the linear trajectory above by setting\n",
    "$\\alpha_0 = 0, \\alpha_1 = \\frac{ d_\\mathrm{g}}{s_\\mathrm{g} }$.\n",
    "In other words, if we begin with the general linear trajectory\n",
    "$d(s) = \\alpha_0 + \\alpha_1 x$, we can solve for $\\alpha_0$ and $\\alpha_1$ to match the initial\n",
    "and final values for the lateral position $d$.\n",
    "This shouldn’t be surprising; the linear polynomial has two free parameters ($\\alpha_0$ and $\\alpha_1$),\n",
    "so it makes sense that we could satisfy two independent constraints (initial and final values for $d$) by\n",
    "an appropriate choice for these parameters.\n",
    "\n",
    "In general, the parameters of an $m^{th}$ order polynomial can be chosen to satisfy $m+1$ independent constraints.\n",
    "So, if we wish to match initial and final conditions on heading (which is defined by the first derivative of $d$),\n",
    "we would require a cubic polynomial, and if we wished to also satisfy lateral acceleration constraints, we would need a fifth order polynomial.\n",
    "The lateral velocity and acceleration for the trajectory are given by the first and second derivatives of $d$,\n",
    "which we denote by $d'$ and $d''$, respectively.\n",
    "For a fifth order polynomial, we have \n",
    "\n",
    "\\begin{equation}\n",
    "\\begin{aligned}\n",
    "d(s) &=& \\alpha_0 + \\alpha_1 s + \\alpha_2 s^2 + \\alpha_3 s^3 + \\alpha_4 s^4 + \\alpha_5 s^5\\\\\n",
    "d'(s) &=& \\alpha_1 + 2 \\alpha_2 s + 3 \\alpha_3 s^2 + 4 \\alpha_4 s^3 + 5 \\alpha_5 s^4\\\\\n",
    "d''(s) &=&  2 \\alpha_2  +6 \\alpha_3  s + 12 \\alpha_4 s^2 + 20 \\alpha_5 s^3 \n",
    "\\end{aligned}\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "rJycRB9LOIi3",
   "metadata": {},
   "source": [
    "For a simple lane change, it is typically desirable to have zero lateral velocity and zero lateral acceleration at the initial and final positions. This leads to the following system of equations:\n",
    "\\begin{equation}\n",
    "\\begin{aligned}\n",
    "y(0) = 0& =& \\alpha_0 \\\\\n",
    "y'(0) = 0 & =& \\alpha_1 \\\\\n",
    "y''(0) = 0 &=&   2 \\alpha_2  \\\\\n",
    "y(x_\\mathrm{g}) = y_\\mathrm{g} &=&\n",
    "     \\alpha_0 + \\alpha_1 x_\\mathrm{g} + \\alpha_2 x_\\mathrm{g}^2 + \\alpha_3 x_\\mathrm{g}^3 + \\alpha_4 x_\\mathrm{g}^4 + \\alpha_5 x_\\mathrm{g}^5 \\\\\n",
    "y'(x_\\mathrm{g}) = 0 &=&\n",
    "   \\alpha_1 + 2 \\alpha_2 x_\\mathrm{g} + 3 \\alpha_3 x_\\mathrm{g}^2 + 4 \\alpha_4 x_\\mathrm{g}^3 + 5 \\alpha_5 x_\\mathrm{g}^4 \\\\\n",
    "y''(x_\\mathrm{g}) =  0 &=&\n",
    "  2 \\alpha_2  +6 \\alpha_3  x_\\mathrm{g} + 12 \\alpha_4 x_\\mathrm{g}^2 + 20 \\alpha_5 x_\\mathrm{g}^3 \n",
    "\\end{aligned}\n",
    "\\end{equation}\n",
    "Note that these six equations are all linear in the parameters $\\alpha_i$, so it is a simple matter to solve\n",
    "these."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "x0LyznJsrzy4",
   "metadata": {},
   "source": [
    "```{index} splines\n",
    "```\n",
    "## Splines\n",
    "\n",
    "While the derivation above produced a single polynomial trajectory,\n",
    "it is a simple matter to extend this formalism to construct trajectories\n",
    "that are composed of multiple consecutive polynomial segments.\n",
    "Such trajectores belong to the more general class of **splines**.\n",
    "In general, a spline is a continuous, piecewise polynomial curve, and we are not\n",
    "necessarily given the specific values for the transition points between adjacent\n",
    "segments.\n",
    "\n",
    "In fact, we have actually done exactly this in the above derviation,\n",
    "if we consider that for $s < 0$ and for $s  > s_\\mathrm{g}$ the trajectory $d(s)$ is linear and pararallel to the $s$-axis,\n",
    "i.e., we have solved for a special case of three polynomial segments with two of those\n",
    "segments being linear and one quintic.\n",
    "\n",
    "Suppose that we wish to construct a piecewise polynomial trajectory that\n",
    "exactly reaches certain via points, $(s_i, d_i)$, i.e., we are given the\n",
    "sequence of couples $(s_0,d_0) \\dots (s_n, d_n)$,\n",
    "and we want to ensure that the lateral position of the car is given by $d_i$ at longitudinal\n",
    "position $s_i$.\n",
    "We can build the trajectory $d$ from a sequence of polynomials $P_k$ as follows\n",
    "\\begin{equation}\n",
    "\\begin{align}\n",
    "d(s) &= P_k(s)  \\;\\;\\;\\;\\;\\ s_k \\leq s < s_{k+1},\\;\\; 0 \\leq k < n\\\\\n",
    "P_k(s)  &= \\sum_{i=0}^{d} \\alpha_{ki} (s-s_k)^i \\\\\n",
    "\\end{align}\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "Cs4d7cLI1l05",
   "metadata": {
    "vscode": {
     "languageId": "plaintext"
    }
   },
   "source": [
    "The coefficients $\\alpha_{ki}$ are chosen to ensure the desired continuity and smoothness properties.\n",
    "For example, if we want to ensure that position, velocity, and accelerations are continuous\n",
    "throughout the trajectory, we must enforce\n",
    "\\begin{equation}\n",
    "\\begin{align}\n",
    "d_k = &P_{k-1}(s_k) = P_{k}(s_k), \\;\\;\\; k = 1, \\dots n-1 \\\\\n",
    "&P'_{k-1}(s_k) = P'_{k}(s_k), \\;\\;\\; k = 1, \\dots n-1 \\\\\n",
    "&P''_{k-1}(s_k) = P''_{k}(s_k), \\;\\;\\; k = 1, \\dots n-1 \n",
    "\\end{align}\n",
    "\\end{equation}\n",
    "Note that the first set of constraints also ensure that $d(s_k) = d_k$ for each transition point.\n",
    "In order to satisfy the boundary conditions for the start and end of the trajectory, we\n",
    "must enforce\n",
    "\\begin{equation}\n",
    "\\begin{align}\n",
    "d_0 = &P_{0}(s_0)  \\\\\n",
    "d_n = &P_{n}(s_n)  \\\\\n",
    "v_0 = &P'_{0}(s_0)  \\\\\n",
    "v_n = &P'_{n}(s_n)  \\\\\n",
    "a_0 = &P''_{0}(s_0)  \\\\\n",
    "a_n = &P''_{n}(s_n)  \\\\\n",
    "\\end{align}\n",
    "\\end{equation}\n",
    "where $v_k$ and $a_k$ denote the velocity and acceleration, respectively,\n",
    "at position $s_k$.\n",
    "\n",
    "Note that in this formulation, the accelerations and velocities at the junctions of two\n",
    "adjacent splines are not specified; the velocity and acceleration must match for the two\n",
    "polynomial segments, but the specific values of velocity and acceleration are not specified.\n",
    "Because of this, we have fewer constraints, and therefore require fewer parameters\n",
    "$\\alpha_{ki}$.\n",
    "In particular, if we merely want to ensure continuity of position, velocity, and acceleration\n",
    "at the junctions, cubic polynomials are sufficient.\n",
    "This is different from the situation above, in which we used a quintic polynomial to achieve\n",
    "specific values of velocity and acceleration (all of which were set to zero) at the junctions\n",
    "of the two linear segments with the lane-changing trajectory.\n",
    "You should count the constraints, and convince yourself that this is true."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "r5n8rrAFKYAd",
   "metadata": {},
   "source": [
    "## Following Nonlinear Trajectories\n",
    "\n",
    "All of the above can be generalized to the case of nonlinear trajectories, such\n",
    "as a car driving on a curvy road, or making a turn at an intersection.\n",
    "In these situations, the desired trajectory is often determined by a desired\n",
    "behavior. Possible behaviors might include lane following, merging, changing lanes,\n",
    "coming to a stop at an intersection, or making a turn at an intersection.\n",
    "In these cases, following the desired trajectory, rather than computing the desired\n",
    "trajectory, becomes an important problem. In this section, we address the problem\n",
    "of following such a trajectory.\n",
    "\n",
    "The figure below illustrates the situation.\n",
    "We denote by $\\gamma(s)$ the desired trjactory of the car, where $s$, an arc length\n",
    "parameter, is a function of time, and therefore the instantaneous desired speed of \n",
    "the car is $\\dot{s}(t)$.\n",
    "Since the goal is to keep the car on the deisred trajectory, it is convenient\n",
    "to represent the state of the car in a coordinate frame that is local to the trajectory.\n",
    "To do so, for each point along $\\gamma$, we define a frame with origin $\\gamma(s)$,\n",
    "with axes $t_\\gamma(s)$ and $n_\\gamma(s)$, the tangent and normal vectors\n",
    "to $\\gamma$ at $s$, respectively.\n",
    "This is essentially a Frenet frame for planar curves (the binormal being orthogonal to the plane).\n",
    "\n",
    "<figure id=\"fig:FrenetFrame\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures6/frenet-frame.png?raw=1\" style=\"width:18cm\" alt=\"\">\n",
    "<figcaption>Local coordinate frame relative to a given desired trajectory. </figcaption>\n",
    "</figure>\n",
    "\n",
    "Using this convention, at time $t$ we can denote the position of the car as\n",
    "\\begin{equation}\n",
    "x(t) = x(s,d) = \\gamma(s(t)) + d(t) n_\\gamma(s(t))\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "vfnGq-44krDf",
   "metadata": {},
   "source": [
    "A simple way to bring the car into coincidence with the desired trajectory is to choose a target point\n",
    "along the desired trajectory and plan a local trajectory for the car to reach this target\n",
    "point at some desired time, $T$, traveling at the desired velocity.\n",
    "We can do so by specifying the time $T$ at which the car should reach the target point,\n",
    "and planning a trajectory, $x(t)$ for the car such that $x(T) = \\gamma(s(T))$.\n",
    "\n",
    "For the lateral motion, this amounts to planning a trajectory for $d(t)$ with appropriate boundary conditions.\n",
    "Because human passengers are particularly sensitive to sudden motions in the lateral direction,\n",
    "we choose $d(0), \\dot{d}(0)$, and $\\ddot{d}(0)$ to match the state of the car at time $t = 0$.\n",
    "When the car reaches the point $\\gamma(s(T))$ on the desired trajectory,\n",
    "it is desirable that the lateral velocity and acceleration both be zero,\n",
    "so that the car moves along the desired direction of motion (i.e., no lateral motion relative\n",
    "to the desired trajectory).\n",
    "Thus, at time $T$, we need to satisfy $d(T) = \\dot{d}(T) = \\ddot{d}(T) = 0$.\n",
    "As we have seen above, these six constraints uniquely determine a quintic polynomial trajectory for $d$."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "KFU-kAtYhnrg",
   "metadata": {},
   "source": [
    "Given $d(t)$, the trajectory for the car is simply given by\n",
    "\\begin{equation}\n",
    "x(t) = \\gamma(s(t)) + d(t) n_\\gamma(s(t))\n",
    "\\end{equation}\n",
    "which brings the car onto the desired trajectory, with desired velocity, at time $t = T$."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "Lz7lVWZOmW8U",
   "metadata": {},
   "source": [
    "```{index} jerk\n",
    "```\n",
    "Obviously, the termination time $T$ plays a significant role in determining the quality of the solution.\n",
    "For example, if we choose $T$ to be too small, the car might need to accelerate quickly to an unsafe velocity\n",
    "to reach the target point at $t = T$.\n",
    "How, then, should we choose the value of $T$?\n",
    "A typical approach is to formulate the choice of $T$ as an optimization problem,\n",
    "with a cost functional that takes into account both the duration of the maneuver (we don't\n",
    "want the maneuver to take too long), and the comfort of the human passenger.\n",
    "As mentioned above, humans are sensitive to acceleration changes in the lateral direction,\n",
    "therefore, we might wish to minimize the overall effect of such changes.\n",
    "Mathematically, the instantaneous change in lateral cceleration is given by the third\n",
    "derivative of $d$, which is known as the **jerk**.\n",
    "For a given trajectory $d(t)$, defined on the interval $[0,T]$, the following\n",
    "cost functional penalizes aggregate jerk and total exectution time\n",
    "\\begin{equation}\n",
    "J(d) = \\int_0^T \\left(\\frac{d}{dt}\\ddot{d}(\\tau)\\right)^2  d\\tau + \\beta T\n",
    "\\end{equation}\n",
    "In general, it may not be possible to solve this optimization problem in real time.\n",
    "In such cases, rather than using $J$ to solve find the optimal $d$, we\n",
    "can use a generate-and-test approach.\n",
    "With such an approach, several values of $T$ are proposed, and the corresponding quintic trajectories\n",
    "are computed for each of these. It is then a simple matter to evaluate the cost of each of\n",
    "these proposed trajectories, ultimately choosing the value of $T$ with the smallest such cost."
   ]
  }
 ],
 "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": 5
}