S57_diffdrive_summary.ipynb

{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "# Chapter Summary\n",
    "\n",
    "<img src=\"Figures5/S50-Two-wheeled_Toy_Robot-02.jpg\" alt=\"Splash image with steampunk differential drive robot\" width=\"60%\" align=center style=\"vertical-align:middle;margin:10px 0px\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the first four chapters of this book, we have considered motion and sensor models, but we haven't\n",
    "really discussed the mechanics of motion or the implementation of sensors.\n",
    "Instead, we proposed abstract models (e.g., sensor models of the form $z = h(x)$)\n",
    "and various probability distributions to characterize uncertainty (e.g., conditional Gaussians),\n",
    "but we have not bothered to relate these models to the actual devices that effect motion and sensing.\n",
    "In this chapter, we began to remedy this deficiency, for motion models by studying the kinematics of differential\n",
    "drive robots, and for sensor models by studying cameras.\n",
    "We then developed appropriate planning, perception, and learning approaches for these models.\n",
    "For both modeling and reasoning, the methods we introduced in this chapter were quite different from\n",
    "what we have seen in previous chapters."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Models\n",
    "\n",
    "In previous chapters, our models have often been probabilistic in nature. Motion models have been expressed as conditional\n",
    "probability distributions that depend on current state and applied action,\n",
    "and sensor models have been expressed as conditional probability distributions that depend on state. \n",
    "In this chapter, we develop geometric models without consideration of uncertainty. In general, geometric\n",
    "models together with models for how uncertainty enters the system can be used to derive the kinds of conditional\n",
    "probability models we have seen in previous chapters. \n",
    "\n",
    "We began the chapter with a formal definition of a *configuration* and of the *configuration space* for robotic systems.\n",
    "For the simple differential drive robot of this chapter, we rigidly attach a coordinate frame to the robot,\n",
    "with origin at the midpoint of the axle and $x$-axis parallel to the direction of motion.\n",
    "The pose of this frame (position of its origin, and orientation of its axes) defines a configuration\n",
    "of the robot, and the set of all configurations, ${\\cal Q} = \\mathbb{R}^2 \\times [0, 2\\pi),$ defines\n",
    "the configuration space of the robot.\n",
    "We then showed how it is possible to determine the location of any point on the robot using the\n",
    "robot's configuration.\n",
    "\n",
    "The motion model for the differential drive robot relates the angular velocities of the two\n",
    "actuated wheels to the linear and angular velocity of the body-attached coordinate frame.\n",
    "The *forward velocity kinematics* in the body-attached frame are given by\n",
    "\n",
    "$$\n",
    "v_x = \\frac{r}{2} (\\dot{\\phi}_R + \\dot{\\phi}_L)\n",
    "~~~~~~~~\n",
    "\\omega = \\frac{r}{L} (\\dot{\\phi}_R - \\dot{\\phi}_L)\n",
    "$$\n",
    "\n",
    "which can be expressed with respect to the world coordinate frame, as a function\n",
    "of the configuration of the robot, as\n",
    "\n",
    "$$\n",
    "v^{\\mathrm{world}}=\n",
    "\\begin{bmatrix} v_x \\cos \\theta \\\\ v_x \\sin \\theta \\\\ \\dot{\\theta} \\end{bmatrix}\n",
    "=\\begin{bmatrix} \\frac{r}{2} (\\dot{\\phi}_R + \\dot{\\phi}_L) \\cos\\theta  \\\\  \\frac{r}{2} (\\dot{\\phi}_R + \\dot{\\phi}_L) \\sin\\theta  \\\\ \\frac{r}{L} (\\dot{\\phi}_R - \\dot{\\phi}_L)\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Using the differential kinematic equations, we can easily calculate either (a) the linear and angular velocity that will result from applied\n",
    "wheel angular velocities, or (b) the wheel angular velocities that are required to achieve the desired angular and linear\n",
    "velocities of the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Cameras are sensors that compute a two-dimensional, discrete representation of a three-dimensional scene.\n",
    "In this chapter, we studied the geometry of the image formation process.\n",
    "There are essentially two aspects to image formation: projection and discretization.\n",
    "Points in the world are projected onto the image plane, which contains a discrete array of sensing elements,\n",
    "each of which corresponds to one pixel.\n",
    "\n",
    "For most cameras, the imaging geometry can be modeled using perspective projection, which corresponds\n",
    "to a pinhole camera model. In such systems, the physical lens apparatus implements an effective pinhole,\n",
    "through which each light ray passes. The pinhole is located at the lens focal center,\n",
    "at distance $F$ in front of the image plane. For a 3D point\n",
    "with coordinates $(X,Y,Z)$, the corresponding image plane coordinates are given by\n",
    "the perspective projection equations:\n",
    "\n",
    "$$\n",
    "X_I = - F \\frac{X}{Z} ~~~~~~\n",
    "Y_I = - F \\frac{Y}{Z} \n",
    "$$\n",
    "\n",
    "In the discretization stage, image plane coordinates are mapped to pixel coordinates by\n",
    "incorporating the pixel size and the location of the image center (also called the *principal point*),\n",
    "whose image coordinates are given as $(u_0, v_0)$.\n",
    "For the case of square pixels, such that there are $k$ pixels per unit length, we combine\n",
    "pixel size with focal length into a single constant, $f = Fk$, \n",
    "and obtain the projection equations for pixel coordinates $(r,c)$ as\n",
    "\n",
    "$$\n",
    "r = \\left\\lfloor v_0 + f \\frac{Y}{Z} \\right\\rfloor ~~~~~~ c = \\left\\lfloor u_0 + f \\frac{X}{Z} \\right\\rfloor\n",
    "$$\n",
    "\n",
    "These equations describe the *forward* imaging geometry, i.e., how points in the world project onto the imaging sensor.\n",
    "In robotics, we often need to solve the inverse problem: computing the 3D coordinates of a point in the world\n",
    "that corresponds to a certain pixel in the image.\n",
    "While this inverse problem cannot be solved using only the information in a single image,\n",
    "it is easily solved using a stereo pair of cameras, provided (a) the two cameras both view the relevant portion of the scene,\n",
    "(b) the geometric relationship between the two cameras is known.\n",
    "In the simplest stereo rig, the geometric relationship is restricted to a pure translation along the $x$-axis of the image\n",
    "frame.\n",
    "In this case, the $Z$ coordinate of an image point can be computed using the *fundamental stereo equation*:\n",
    "\n",
    "$$\n",
    "Z = B \\frac{f}{d}.\n",
    "$$\n",
    "\n",
    "in which $B$ is the distance between the origins of the two camera coordinate frames,\n",
    "and $d$ is the disparity for the projected point in left and right images."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Reasoning\n",
    "\n",
    "In previous chapters, we have developed several methods to reason with uncertainty.\n",
    "We frequently relied on the concept of expectation to optimize with respect\n",
    "to average performance over long time horizons.\n",
    "In this chapter, we focused on the deterministic aspects of computer vision and robot motion.\n",
    "Rather than finding solutions that work well on average, we presented algorithms\n",
    "that consistently find good solutions, whether for problems related to scene understanding using\n",
    "computer vision or path planning for mobile robots.\n",
    "A motion planning system that only avoids collision *on average* is not acceptable in many applications.\n",
    "Likewise, computer vision systems that recognize objects well *on average* can fail\n",
    "spectacularly and catastrophically when used to guide robots operating in the real world,\n",
    "such as for self-driving cars (which we will see in Chapter 6).\n",
    "\n",
    "Computer vision involves reasoning about images, and images are collections of pixels,\n",
    "therefore, the first computer vision algorithms that we described were simple\n",
    "image processing algorithms that operated on collections of pixels in an input image\n",
    "to produce and output image.\n",
    "Many of these image processing algorithms were merely variations on\n",
    "convolution (e.g., smoothing, edge detection)\n",
    "This motivated the introduction of convolutional neural networks (CNNs),\n",
    "and led naturally to the introduction of *deep neural networks* (or *deep nets*),\n",
    "which can be constructed using sequential layers of CNNs connected by *activation layers*.\n",
    "We then saw how deep nets can be used to solve classical computer vision\n",
    "problems such as semantic segmentation and depth estimation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Our introduction of deep nets focused on their use as simple computational units. \n",
    "We provided the specific weights in the network that were required to implement specific, and known, operations.\n",
    "However, the real power of deep nets is that they can be used trained to implement operations\n",
    "that are not well understood, and that have not been successfully implemented using traditional\n",
    "computer algorithms.\n",
    "The secret to this power is that it is possible to *learn* the parameters of the network!\n",
    "In this chapter, we saw how to do this using *supervised learning*.\n",
    "With supervised learning, we are provided with a training set\n",
    "and a loss function.\n",
    "We train the network by choosing parameters that minimize this loss function with respect to the training set.\n",
    "While there are numerous candidate loss functions, we confined our attention to *mean squared error* and\n",
    "*cross entropy* loss functions.\n",
    "The usual way to train the network is to compute a gradient of the loss function with respect\n",
    "to the network's parameters, and use some variation of gradient descent to find the optimal parameters.\n",
    "In cases for which the loss function includes additive terms for each element of the training data,\n",
    "the gradient computations can be extremely expensive, \n",
    "but we showed that we can reduce the required computation by computing the gradient only at randomly\n",
    "selected terms in the loss function, an approach known as *stochastic gradient descent*.\n",
    "It is worth noting that the method of randomization used in stochastic gradient descent\n",
    "is fundamentally different from our previous use of random variables to represent uncertain quantities.\n",
    "Until this point, we have used probability theory to characterize uncertainty in the robot's understanding\n",
    "of the world, of its actions in the world, and of the data provided by its sensors.\n",
    "When using randomized algorithms, the stochastic behavior is a property of the *algorithm*, and *not* of the\n",
    "robot or its environment."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In previous chapters, we framed the planning problem in terms of maximizing an expected reward function.\n",
    "In this chapter, we looked at a more specific problem: planning a collision-free path for the robot\n",
    "from its initial configuration to a specified goal configuration.\n",
    "We began by showing how this problem could be solved by using a special case of value iteration\n",
    "(no stochastic component, negative reward for collision, high reward for reaching the goal),\n",
    "but it was immediately clear that this approach would not scale to more complex robotic systems,\n",
    "since the computational complexity of a dynamic programming approach scales exponentially with the\n",
    "dimension of the configuration space.\n",
    "\n",
    "Even though value iteration doesn't scale to higher dimensions, the idea of following the gradient\n",
    "of the reward function would be useful if there were a way to access this gradient without\n",
    "incurring the cost of computing the value function.\n",
    "Path planners that use artificial potential fields attempt to achieve this by constructing a function that behaves similarly\n",
    "to the value function, but that is specified in closed form for each configuration,\n",
    "thus eliminating the computational cost of dynamic programming.\n",
    "These algorithms work by evaluating the gradient of the potential function at the current configuration,\n",
    "and using gradient descent to make progress toward the goal, essentially focusing attention on computing\n",
    "the potential function along a single path to the goal, rather than computing a global representation of the value function.\n",
    "The problem with these methods is that the potential functions are almost never equivalent to the value function.\n",
    "In particular, the value function has a single optimum at the goal,\n",
    "while an artificial potential function typically has many local minima, which will trap gradient descent methods.\n",
    "Thus, with artificial potential field methods, we trade computational complexity for completeness, i.e., we lose the\n",
    "guarantee that a solution will be found when a solution exists, but we gain the possibility of finding solutions quickly.\n",
    "\n",
    "In the same way that it was used to deal with the computational burden of computing gradients of\n",
    "complex loss functions, randomization has also been used to solve complex path planning problems.\n",
    "The basic idea is straightforward: generate random sample configurations,\n",
    "and then use fast and efficient motion planning algorithms to find local paths\n",
    "between these samples.\n",
    "In order for these methods to be effective, two conditions must hold:\n",
    "the random selection of configurations should ensure good coverage of the entire configuration\n",
    "space (as the number of samples increases), and the local path planning problems should be easy to solve\n",
    "(typically this is the case when the distance between two configurations is small).\n",
    "\n",
    "Probabilistic road maps (PRMs) are graphs whose vertices correspond to sample configurations,\n",
    "and whose edges correspond to local paths between sample configurations.\n",
    "They are constructed by iteratively generating random samples and connecting each new sample\n",
    "to nearby samples that are already represented in the graph. This is an offline process, intended to build an\n",
    "approximate, global representation of the free configuration space.\n",
    "Equipped with a PRM, path planning is reduced to (a) finding a path from the initial configuration to some\n",
    "vertex in the graph, (b) finding a path from the goal configuration to a vertex in the graph, and (c) searching\n",
    "the graph for a path that connects these two vertices.\n",
    "\n",
    "Rapidly-Exploring Random Trees (RRTs) are constructed by iteratively adding vertices to a tree whose root\n",
    "corresponds to the initial configuration, until a leaf is added that can be easily connected to the goal configuration.\n",
    "The trick that makes RRTs work is that the randomly generated samples are not themselves\n",
    "added to the tree; instead, they are used to determine which vertex in the existing tree should be expanded.\n",
    "Specifically, the vertex $q_\\mathrm{near}$ in the existing tree that is nearest to the\n",
    "randomly generated configuration $q_\\mathrm{rand}$ is selected for expansion,\n",
    "and the expansion is achieved by constructing a local path from $q_\\mathrm{near}$ in the\n",
    "direction of $q_\\mathrm{rand}$, terminating at the configuration $q_\\mathrm{new}$, which is added to the tree.\n",
    "If $q_\\mathrm{rand}$ is generated by sampling from a uniform distribution, the RRT will tend to explore\n",
    "the entire configuration space, ensuring (in an asymptotic, probabilistic sense) that a path will be found\n",
    "if one exists.\n",
    "\n",
    "Finally, it is worth noting that, apart from value iteration, all of the planning methods introduced \n",
    "in this chapter can be considered as sampling-based algorithms for path planning.\n",
    "For the case of PRMs and RRTs, the samples are randomly generated, while for potential fields methods,\n",
    "we can consider each step in the gradient descent algorithm as a sample along the path\n",
    "currently under exploration.\n",
    "While the sampling strategies for these two approaches (random generation vs. gradient descent) are quite\n",
    "different, both share the property that paths are constructed by generating sample configurations and building\n",
    "a graph (a general graph for PRMs, a tree for RRTs, and a simple linear graph for potential fields).\n",
    "The resulting graphs essentially represent one-dimensional subsets of the configuration space,\n",
    "and the goal is to construct these sets so that their connectivity matches the connectivity of\n",
    "the free configuration space with respect to the problem at hand."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Background and History"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The kinematics of differential drive robots are described in detail in [Introduction to Autonomous Mobile Robots](https://mitpress.mit.edu/9780262015356/introduction-to-autonomous-mobile-robots/) by Siegwart, Nourbakhsh, Scaramuzza {cite:p}`Siegwart11book_robots`\n",
    "\n",
    "The first mathematically rigorous book on robot motion planning was written by Latombe\n",
    "in the early nineties {cite:p}`Latombe91book`.\n",
    "Brian Eno once remarked\n",
    "that only about 1,000 people bought the first Velvet Underground album, but every one of them formed a rock 'n' roll band. Latombe's book held this status in robotics; if you owned it, likely as not, you went on to become a researcher in robot motion planning.\n",
    "In subsequent years, [Principles of Robot Motion](https://mitpress.mit.edu/9780262033275/principles-of-robot-motion/) by Choset, et al. {cite:p}`Choset05book_motion`\n",
    "and [Planning Algorithms](https://lavalle.pl/planning/) by LaValle {cite:p}`LaValle06book_planning`\n",
    "provided updated treatments of the rapidly expanding field.\n",
    "\n",
    "\n",
    "Excellent introductions to the material on machine learning can be found in\n",
    "[Deep Learning](https://www.deeplearningbook.org/) by Goodfellow, Bengio, and Courville {cite:p}`Goodfellow16book_dl`\n",
    "and\n",
    "[Dive into Deep Learning](https://d2l.ai/) by Zhang et al. {cite:p}`Zhang20book_d2l`."
   ]
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "include_colab_link": true,
   "name": "S51_diffdrive_intro.ipynb",
   "provenance": []
  },
  "interpreter": {
   "hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d"
  },
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.8.12"
  },
  "latex_metadata": {
   "affiliation": "Georgia Institute of Technology",
   "author": "Frank Dellaert and Seth Hutchinson",
   "title": "Introduction to Robotics"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}