S52_diffdrive_actions.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/S52_diffdrive_actions.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": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install -q -U gtbook\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "id": "10-snNDwOSuC",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import plotly.express as px\n",
    "import gtsam\n",
    "\n",
    "from gtbook.discrete import Variables\n",
    "from gtbook.display import show, pretty\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "# Motion Model for the Differential Drive Robot\n",
    "\n",
    "> By rotating the two wheels independently, we can control both the linear and angular velocity of the DDR.\n",
    "\n",
    "<img src=\"Figures5/S52-Two-wheeled_Toy_Robot-00.jpg\" alt=\"Splash image with steampunk differential drive robot\" width=\"40%\" align=center style=\"vertical-align:middle;margin:10px 0px\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The motion model for the logistics robot of the previous chapter was fairly simple;\n",
    "we assumed that the robot moved with constant linear velocity $v$ for a time interval $\\Delta T$,\n",
    "and therefore we expressed the motion model as $x_{k+1} = x_k + v \\Delta T$.\n",
    "Further, by assigning the robot's body-attached frame to be parallel to the world frame,\n",
    "we were able to simply ignore the body-attached frame, and reason directly in the world frame without difficulty.\n",
    "Things are more complex for our DDR, due to the role of orientation.\n",
    "\n",
    "When describing the motion of our DDR, the orientation of the robot enters in two ways.\n",
    "First, because the robot wheels roll without slipping,\n",
    "the linear velocity of the robot is always instantaneously in the steering direction.\n",
    "Second, because the robot can rotate, we must take account of its angular velocity, in addition to the linear velocity.\n",
    "This is illustrated in the figure below.\n",
    "Suppose the robot is following a path $\\gamma(s)$ (where $s$ parameterizes the path).\n",
    "The instantaneous linear velocity expressed with respect to the body frame is given by:\n",
    "\n",
    "$$\n",
    "v^{\\mathrm{body,linear}}\n",
    "\\begin{bmatrix} v_x \\\\ 0 \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Note that the velocity is tangent to the curve $\\gamma$ at $s$, and that in the body-attached frame the y-component of the velocity\n",
    "is zero (i.e., in the body-attached frame, $v_y = 0$).\n",
    "The steering direction is determined by the angle $\\theta$ as $[\\cos \\theta, \\sin \\theta]^T$,\n",
    "so that the linear velocity with respect to the world frame is given by\n",
    "\n",
    "$$\n",
    "v^{\\mathrm{world,linear}}\n",
    "\\begin{bmatrix} v_x \\cos \\theta \\\\ v_x \\sin \\theta \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Because our robot moves in the plane, the z-axis of the body-attached frame is always parallel to the z-axis of the world frame.\n",
    "This greatly simplifies the description of angular velocity, which in this case we may define as $\\omega = \\dot{\\theta}$,\n",
    "the instantaneous rate of change of the robot's orientation.\n",
    "\n",
    "It is common to combine the angular and linear velocity into a single vector, \n",
    "\n",
    "$$\n",
    "v^{\\mathrm{body}}=\n",
    "\\begin{bmatrix} v_x   \\\\ 0 \\\\ \\dot{\\theta} \\end{bmatrix}\n",
    "~~~~~~~\n",
    "v^{\\mathrm{world}}=\n",
    "\\begin{bmatrix} v_x \\cos \\theta \\\\ v_x \\sin \\theta \\\\ \\dot{\\theta} \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "\n",
    "<figure id=\"fig:DDR-velocity\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures5/DDR-velocity.png?raw=1\" style=\"width:9cm\" alt=\"\">\n",
    "<figcaption>The linear velocity is always in the steering direction.</figcaption>\n",
    "</figure>\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The Relationship Between Wheel Rotation and Robot Velocity\n",
    "\n",
    "Because the wheels of the DDR are independently actuated, it is possible to achieve a large range of robot velocities.\n",
    "We can derive the relationship between wheel rotation and robot velocity by considering first the motion of a single\n",
    "wheel, and then considering the effect of coupling the two wheels along a single axis of rotation.\n",
    "\n",
    "The figure below shows a side-view of the right wheel.\n",
    "We denote by $\\phi_R$ the instantaneous orientation of the right wheel with respect to the world z-axis (Note\n",
    "that we measure the angle $\\phi_R$ by attaching a distinguished point to the wheel, so that we can uniquely identify\n",
    "its orientation. In the figure, a red star is used to denote this point.)\n",
    "As we have seen in the previous chapter when modeling the forward motion of an omni-wheel,\n",
    "the relationship between the forward linear speed of the wheel and the rotation speed is\n",
    "given by\n",
    "\n",
    "$$\n",
    "v_\\mathrm{right} = r \\dot{\\phi}_R\n",
    "$$\n",
    "\n",
    "The same reasoning can be applied to the left wheel  to obtain\n",
    "\n",
    "$$\n",
    "v_\\mathrm{left} = r \\dot{\\phi}_L\n",
    "$$\n",
    "\n",
    "\n",
    "<figure id=\"fig:DDR-one-wheel\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures5/DDR-one-wheel.png?raw=1\" style=\"width:9cm\" alt=\"\">\n",
    "<figcaption>The linear velocity is always in the steering direction.</figcaption>\n",
    "</figure>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Suppose now that both wheels spin at the same speed, $\\dot{\\phi}_R = \\dot{\\phi}_L$.\n",
    "In this case, the forward speed of the wheels will also be equal, $v_\\mathrm{left} = v_\\mathrm{right}$,\n",
    "and the robot will move in purely translational motion (i.e., $\\omega = 0$), with\n",
    "$v_x = v_\\mathrm{left} = v_\\mathrm{right}$, since all points on the robot move with exactly the\n",
    "same velocity for pure translational motion.\n",
    "Using the results above, we obtain\n",
    "\n",
    "$$\n",
    "\\dot{\\phi}_L = \\dot{\\phi}_R = \\frac{v_x}{r}\n",
    "$$\n",
    "\n",
    "\n",
    "<figure id=\"fig:DDR-pure-translation\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures5/DDR-pure-translation.png?raw=1\" style=\"width:9cm\" alt=\"\">\n",
    "<figcaption>When the wheels spin in the same direction with the same speed, the robot moves with pure translation.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Suppose instead that the two wheels spin in opposite directions,\n",
    "so that $\\dot{\\phi}_R = -\\dot{\\phi}_L$.\n",
    "In this case, $v_\\mathrm{left} = -r\\dot{\\phi}_L $ and $v_\\mathrm{right} = r\\dot{\\phi}_R$.\n",
    "Because the two wheels are constrained by the physical mechanism to remain in a fixed geometric relationship\n",
    "to one another, these opposite but equal forward wheel speeds cause the robot to rotate,\n",
    "with both $v_\\mathrm{left}$ and $v_\\mathrm{right}$ tangent to a circle of diameter $L$ centered at the origin of the body-attached frame.\n",
    "Note that the linear velocity of the robot, $v_x$, is zero in this case,\n",
    "since $v_\\mathrm{left}$ and $v_\\mathrm{right}$ \"cancel one another out\" with respect to the linear velocity of the robot.\n",
    "Applying the equation of circular motion yields\n",
    "\n",
    "$$\n",
    "\\frac{L}{2} \\omega = -v_\\mathrm{left} = -r\\dot{\\phi}_R\n",
    "~~~~~~~\n",
    "\\frac{L}{2} \\omega =  v_\\mathrm{right} = r\\dot{\\phi}_R\n",
    "$$\n",
    "\n",
    "which leads to\n",
    "\n",
    "$$\n",
    "\\dot{\\phi}_L= -\\frac{L}{2} \\frac{\\omega}{r}\n",
    "~~~~~~~\n",
    "\\dot{\\phi}_R= \\frac{L}{2} \\frac{\\omega}{r}\n",
    "$$\n",
    "\n",
    "<figure id=\"fig:DDR-pure-rotation\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures5/DDR-pure-rotation.png?raw=1\" style=\"width:9cm\" alt=\"\">\n",
    "<figcaption>When the wheels spin in the opposite direction with the same speed, the robot moves with pure rotation..</figcaption>\n",
    "</figure>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have now considered the two special cases of pure translation and pure rotation.\n",
    "Because instantaneous velocities lie in a vector space, we can add these in the same\n",
    "way that we would add any vectors.\n",
    "Therefore, by spinning the wheels at different rates, we can obtain various linear combinations\n",
    "of the above pure translations and rotations.\n",
    "Adding the equations for linear and angular velocities of the two wheels, we obtain\n",
    "\n",
    "$$\n",
    "\\dot{\\phi}_L = \\frac{v_x}{r} -\\frac{L}{2} \\frac{\\omega}{r}\n",
    "~~~~~~~\n",
    "\\dot{\\phi}_R = \\frac{v_x}{r} + \\frac{L}{2} \\frac{\\omega}{r}\n",
    "$$\n",
    "\n",
    "These two equations define the **inverse velocity kinematics** for our DDR:\n",
    "Given a desired *output* specified by $v$ and $\\omega$,\n",
    "determine the required $input$ specified as $\\dot{\\phi}_R$ and $\\dot{\\phi}_L$.\n",
    "These equations can be used to determine the required wheel actuation to achieve\n",
    "the desired linear and angular velocities of the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **forward velocity kinematics**\n",
    "are easily obtained from the above equations via simple algebra:\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",
    "This leads immediately to the system equations with respect to the body-attached frame\n",
    "and with respect to the world frame\n",
    "\n",
    "$$\n",
    "v^{\\mathrm{body}}=\n",
    "\\begin{bmatrix} v_x   \\\\ 0 \\\\ \\dot{\\theta} \\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix} \\frac{r}{2} (\\dot{\\phi}_R + \\dot{\\phi}_L)   \\\\ 0 \\\\ \\frac{r}{L} (\\dot{\\phi}_R - \\dot{\\phi}_L)\\end{bmatrix}\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"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kinematics in Code\n",
    "\n",
    "The equations above can be easily implemented in code. We do so below:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ddr_ik(v_x, omega, L=0.5, r=0.1):\n",
    "    \"\"\"DDR inverse kinematics: calculate wheels speeds from desired velocity.\"\"\"\n",
    "    return (v_x - (L/2)*omega)/r, (v_x + (L/2)*omega)/r\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ddr_fk(phidot_L, phidot_R, L=0.5, r=0.1):\n",
    "    \"\"\"DDR inverse kinematics: calculate wheels speeds from desired velocity.\"\"\"\n",
    "    return gtsam.Point3((phidot_R+phidot_L)*r/2, 0, (phidot_R-phidot_L)*r/L)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As an example, let us try to move forward with a velocity of 20 cm/s, while turning counterclockwise at 0.3 rad/s:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1.25 2.75\n"
     ]
    }
   ],
   "source": [
    "phidot_L, phidot_R = ddr_ik(v_x=0.2, omega=0.3)\n",
    "print(phidot_L, phidot_R)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As expected, the left wheel rotates less quickly, making us turn counter-clockwise. To sanity-check, let us put these same wheel speeds through the *forward* kinematics:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.2 0.  0.3]\n"
     ]
    }
   ],
   "source": [
    "print(ddr_fk(phidot_L, phidot_R))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The velocities are as desired, validating both the equations and their implementation. Feel free to experiment with other values using the code above!"
   ]
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "include_colab_link": true,
   "name": "S52_diffdrive_actions.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
}