3-Robot motion.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3. Robot motion\n",
    "\n",
    "Index:\n",
    "- [3.1 Pose composition](#pose_composition)\n",
    "- [3.2 Movement of a robot using velocity command](#velocity_commands)\n",
    "- [3.3 Movement of a robot using odometry commands](#odometry_commands)\n",
    "\n",
    "\n",
    "## Introduction\n",
    "\n",
    "A fundamental aspect of the development of mobile robots is the motion itself. This is not a trivial matter as it is one of the main sources of uncertainty and other constraints to the movement difficult its implementation.\n",
    "This particular lesson introduces the concept of a robot's pose and how we deal with it in a probabilistic context.\n",
    "\n",
    "The pose itself can take multiple forms depending on the problems context:\n",
    "\n",
    "- **2D location**: In a planar context we only need to a 2d vector $[x, y]^T$ to locate a robot against a point of reference, the origin $(0, 0).$\n",
    "- **2D pose**: In most cases involving mobile robots, the location alone is insufficient. We need an aditional paramater known as orientation or *bearing*. Therefore, a robot's pose is usually expressed as $[x, y, \\theta]^T$. *In the rest of the course, we mostly refer to this one.*\n",
    "- **3D pose**: Although we will only mentioned in passing, for robotics applications in the 3D space, *i.e.* UAV or drones, not only a third axis $z$ is added, but to handle the orientation in a 3D environment we need 3 components, *i.e.* roll, pitch and yaw. This course is centered around planar movile robots so we will not use this one, nevertheless most methods could be adapted to 3D environments."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "\n",
    "# IMPORTS\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy import stats\n",
    "\n",
    "from utils.DrawRobot import DrawRobot\n",
    "from utils.tcomp import tcomp"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <span style=\"color:green\">OPTIONAL</span>\n",
    "\n",
    "<span style=\"color:green\">In the Robot motion lecture, we started talking about *Differential drive* motion systems. Include as many cells as needed to introduce the background that you find interesting about it and some code illustrating some related aspect, for example, a code computing and plotting the *Instantaneus Center of Rotation (ICR)* according to a number of given parameters.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:green\">***END OF OPTIONAL PART***</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.1 Pose composition <a id=\"pose_composition\"></a>\n",
    "\n",
    "Given an initial pose $p_1$ and a pose differential $\\Delta p$, *i.e.* how much the robot has moved during an interval of time, we compute the final pose $p$ using the **composition of poses** function:\n",
    "\n",
    "$$\n",
    "    p_1 = \n",
    "        \\begin{bmatrix}\n",
    "            x_1 \\\\ y_1 \\\\ \\theta_1\n",
    "        \\end{bmatrix},\n",
    "    \\Delta p = \n",
    "        \\begin{bmatrix}\n",
    "            \\Delta x \\\\ \\Delta y \\\\ \\Delta \\theta\n",
    "        \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "$$\n",
    "    \\begin{equation}\n",
    "    p = \\begin{bmatrix}\n",
    "            x \\\\ y \\\\ \\theta\n",
    "        \\end{bmatrix}\n",
    "        = p_1 \\oplus \\Delta p\n",
    "        = \\begin{bmatrix}\n",
    "            x_1 + \\Delta x \\cos \\theta_1 - \\Delta y \\sin \\theta_1 \\\\ \n",
    "            y_1 + \\Delta x \\sin \\theta_1 - \\Delta y \\cos \\theta_1 \\\\\n",
    "            \\theta_1 + \\Delta \\theta\n",
    "          \\end{bmatrix}\n",
    "    \\end{equation}\n",
    "$$\n",
    "\n",
    "\n",
    "The differential $\\Delta p$, although we are using it as control in this exercise, normally is calculated given the robot's locomotion or sensed by the wheel encoders."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <span style=\"color:green\">OPTIONAL</span>\n",
    "\n",
    "<span style=\"color:green\">Implement your own methods to compute the composition of two poses, as well as the inverse composition. Include some examples of their utilization, also incorporating plots.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:green\">***END OF OPTIONAL PART***</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Assignment**\n",
    "\n",
    "Take a look at the `Robot()` class provided and its methods. Then, modify the main function in the next cell for the robot to describe a $8m \\times 8m$ square path as seen in the figure below.\n",
    "\n",
    "The robot starts in the bottom-left corner $(0, 0)$ heading north and moves at increments of $2 m$ each step. Each 4 steps it will turn right.\n",
    "\n",
    "**Example**\n",
    "\n",
    "\n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/fig3-1-1.png\" alt=\"\">\n",
    "  <figcaption>Fig. 1: Route of our robot.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Robot():\n",
    "    '''Mobile robot implementation\n",
    "    \n",
    "        Attr:\n",
    "            pose: Expected position of the robot\n",
    "    '''\n",
    "    def __init__(self, mean):\n",
    "        self.pose = mean\n",
    "\n",
    "    def step(self, u):\n",
    "        self.pose = tcomp(self.pose, u)\n",
    "    \n",
    "    def draw(self, fig, ax):\n",
    "        DrawRobot(fig, ax, self.pose)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def main(robot):\n",
    "    \n",
    "    # PARAMETERS INITIALIZATION\n",
    "    num_steps = 15 # Number of robot motions\n",
    "    turning = 4  # Number of steps for turning\n",
    "    u = np.vstack([2., 0., 0.]) # Motion command (pose increment)\n",
    "    angle_inc = -np.pi/2 # Angle increment\n",
    "    \n",
    "    # VISUALIZATION\n",
    "    fig, ax = plt.subplots()\n",
    "    plt.ion()\n",
    "    plt.draw()\n",
    "    plt.xlim((-2, 10))\n",
    "    plt.ylim((-2, 10))\n",
    "    \n",
    "    plt.grid()\n",
    "    robot.draw(fig, ax)\n",
    "    \n",
    "    # MAIN LOOP\n",
    "    for step in range(1,num_steps+1):\n",
    "        \n",
    "        # Check if the robot has to move in straight line or also has to turn\n",
    "        # and accordingly set the third component (rotation) of the motion command \n",
    "        if not None:\n",
    "            u[2] = None \n",
    "        else:\n",
    "            u[2] = None\n",
    "             \n",
    "        # Execute the motion command        \n",
    "        robot.step(u)\n",
    "        \n",
    "        # VISUALIZATION\n",
    "        robot.draw(fig, ax)\n",
    "        fig.canvas.draw()\n",
    "        plt.pause(0.1)   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# RUN \n",
    "initial_pose = np.vstack([0., 0., np.pi/2])\n",
    "robot = Robot(initial_pose)\n",
    "main(robot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.1.1 Considering noise\n",
    "\n",
    "In the previous case, the robot motion was error-free, this is overly optimistic as in a real use case the conditions of the environment are a huge source of uncertainty.\n",
    "\n",
    "Therefore, we have to transform the movement of the robot into a (multidimensional) gaussian distribution.\n",
    "\n",
    "- The mean is still the pose differential in the previous exercise.\n",
    "- The covariance is a $3 \\times 3$ matrix, which defines the amount of error at each step (time interval) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Assignment**\n",
    "\n",
    "Now, we are goint to add a Gaussian noise to the motion, assuming that the incremental motion now follows the probability distribution:\n",
    "\n",
    "$$\n",
    "    \\Delta p = N(\\Delta p_{given}, \\Sigma_{\\Delta p})\n",
    "    \\textit{ with } \n",
    "    \\Sigma_{\\Delta p}  =\n",
    "        \\begin{bmatrix}\n",
    "            0.04 & 0 & 0 \\\\\n",
    "            0 & 0.04 & 0 \\\\\n",
    "            0 & 0 & 0.01\n",
    "        \\end{bmatrix}\n",
    "    (\\text{ units in }m^2 \\text{ and } rad^2)\n",
    "$$\n",
    "\n",
    "For doing that, complete the `NosyRobot()`, which is a child class of the previous `Robot()` one. Concretely, you have to:\n",
    "\n",
    "- Complete this new class by adding some amount of noise to the movement (take a look at the `step()` method. *Hints: [`np.vstack()`](https://docs.scipy.org/doc/numpy/reference/generated/numpy.vstack.html), `multivariate_normal.rvs()`*\n",
    "- Along with the expected pose drawn in red (`self.pose`), in the `draw()` method plot the real pose of the robot (`self.true_pose`) in blue, which is affected by noise. \n",
    "\n",
    "Run the cell several times to see that the motion (and the path) is different each time. Try also with different values of the covariance matrix.\n",
    "\n",
    "**Example**\n",
    "\n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/fig3-1-2.png\" alt=\"\">\n",
    "  <figcaption>Fig. 2: Movement of our robot using pose compositions. <br/>\n",
    "      Containing the expected poses (in red) and the true pose <br/> affected by noise (in blue)</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class NoisyRobot(Robot):\n",
    "    \"\"\"Mobile robot implementation. It's motion has a set ammount of noise.\n",
    "    \n",
    "        Attr:\n",
    "            pose: Inherited from Robot\n",
    "            true_pose: Real robot pose, which has been affected by some ammount of noise.\n",
    "            covariance: Amount of error of each step.\n",
    "    \"\"\"\n",
    "    def __init__(self, mean, covariance):\n",
    "        super().__init__(mean)\n",
    "        self.true_pose = mean\n",
    "        self.covariance = covariance\n",
    "        \n",
    "    def step(self, step_increment):\n",
    "        \"\"\"Computes a single step of our noisy robot.\n",
    "        \n",
    "            super().step(...) updates the expected pose (without noise)\n",
    "            Generate a noisy increment based on step_increment and self.covariance.\n",
    "            Then this noisy increment is applied to self.true_pose\n",
    "        \"\"\"\n",
    "        super().step(step_increment)\n",
    "        true_step = None\n",
    "        self.true_pose = tcomp(None, None)\n",
    "        \n",
    "    def draw(self, fig, ax):\n",
    "        super().draw(fig, ax)\n",
    "        DrawRobot(fig, ax, None, None)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# RUN \n",
    "initial_pose = np.vstack([0., 0., np.pi/2])\n",
    "cov = np.diag([0.04, 0.04, 0.01])  \n",
    "\n",
    "robot = NoisyRobot(initial_pose, cov)\n",
    "main(robot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3.2 Movement of a robot using velocity commands <a id='velocity_commands'></a>\n",
    "\n",
    "In the remainder of this chapter we will describe two probabilistic motion models for planar movement: the **velocity motion model** and the **odometry motion model**, the former being the main topic of this section.\n",
    "\n",
    "The *velocity motion model* is mainly used for motion planning, where the details of the robot's movement are of importance and odometry information is not available (it is computed after the movement).\n",
    "\n",
    "This motion model is characterized by the use of two velocities to control the robot's movement: **linear velocity** $v$ and **angular velocity** $w$. Therefore, during the following exercises, the movement commands will be of the form: $$u = \\begin{bmatrix} v \\\\ w \\end{bmatrix}$$\n",
    "\n",
    "Concretely, this motion model is characterized by the following equations:\n",
    "\n",
    "<table>\n",
    "    <tr>\n",
    "        <td>\n",
    "\n",
    "- If $w \\neq 0$:\n",
    "\n",
    "    $$\n",
    "        x_t = x_{t-1} + \n",
    "        \\begin{bmatrix}\n",
    "            -R \\sin \\theta_{t-1} + R \\sin(\\theta_{t-1} + \\Delta \\theta) \\\\ \n",
    "            R \\cos \\theta_{t-1} - R \\cos(\\theta_{t-1} + \\Delta \\theta)\\\\\n",
    "            \\Delta \\theta\n",
    "        \\end{bmatrix}\n",
    "    $$\n",
    "\n",
    "- If $w = 0$:\n",
    "\n",
    "    $$\n",
    "        x_t = x_{t-1} + v \\cdot \\partial t\n",
    "        \\begin{bmatrix}\n",
    "            \\cos \\theta_{t-1} \\\\ \\sin \\theta_{t-1} \\\\ 0\n",
    "        \\end{bmatrix}\n",
    "    $$\n",
    "    <td/>       \n",
    "    <td>\n",
    "        $$ \n",
    "        \\begin{aligned}\n",
    "        v &= w \\cdot R \\\\\n",
    "        \\Delta \\theta &= w \\cdot  \\partial t\n",
    "        \\end{aligned}\n",
    "        $$\n",
    "    <td/>\n",
    "  <tr/>\n",
    "<table/>\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# IMPORTS\n",
    "from numpy import random\n",
    "\n",
    "from utils.PlotEllipse import PlotEllipse"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2.1 The model in action   \n",
    "\n",
    "\n",
    "**Assignment**\n",
    "\n",
    "Modify the following `next_pose()` function, used in the `VelocityRobot` class below, which computes the next pose of a robot given its previous pose $x$, the velocity movement command $u=[v,w]'$, and a lapse of time $dt$. Concretly you have to complete the if-else statement that takes into account when the robot moves in an straight line ($w = 0$). *Note: you don't have to modify the `None` in the function header nor in the `if cov is not None:` condition.*\n",
    "\n",
    "At this point we don't take into account uncertainty in the system: neither from the initial pose (matrix $P_{3\\times3}$) nor the movement $(v, w)$ (matrix $Q_{2\\times2}$).\n",
    "\n",
    "**Example**\n",
    "\n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/fig3-2-1.png\" alt=\"\">\n",
    "  <figcaption>Fig. 1: Route of our robot.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def next_pose(x, u, dt, cov=None):\n",
    "    ''' This function takes pose x and transform it according to the motion u=[v,w]'\n",
    "        applying the differential drive model.\n",
    "\n",
    "        Args:\n",
    "            x: current pose\n",
    "            u: differential command as a vector [v, w]'\n",
    "            dt: Time interval in which the movement occurs\n",
    "            cov: covariance of our movement. If not None, then add gaussian noise\n",
    "    '''\n",
    "    if cov is not None:\n",
    "        u += cov @ random.randn(2, 1)\n",
    "\n",
    "    if u[1] == 0: #linear motion w=0\n",
    "        next_x = np.vstack([None,\n",
    "                       None,\n",
    "                       None])\n",
    "    else: #Non-linear motion w=!0\n",
    "        R = u[0]/u[1] #v/w=r is the curvature radius\n",
    "        next_x = np.vstack([None,\n",
    "                       None,\n",
    "                       None])\n",
    "\n",
    "    return next_x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class VelocityRobot(object):\n",
    "    \"\"\" Mobile robot implementation that uses velocity commands.\n",
    "    \n",
    "        Attr:\n",
    "            pose: expected pose of the robot in the real world (without taking account noise)\n",
    "            dt: Duration of each step in seconds\n",
    "    \"\"\"    \n",
    "    def __init__(self, mean, dt):\n",
    "        self.pose = mean\n",
    "        self.dt = dt\n",
    "        \n",
    "    def step(self, u):\n",
    "        self.pose = next_pose(self.pose, u, self.dt)\n",
    "        \n",
    "    def draw(self, fig, ax):\n",
    "        DrawRobot(fig, ax, self.pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test the movement of your robot using the demo below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def main(robot, nSteps):\n",
    "          \n",
    "    v = 1 # Linear Velocity \n",
    "    l = 0.5 #Half the width of the robot\n",
    "        \n",
    "    # MATPLOTLIB\n",
    "    fig, ax = plt.subplots()\n",
    "    plt.ion()\n",
    "    fig.canvas.draw()\n",
    "    plt.xlim((-2, 20))\n",
    "    plt.ylim((-2, 30))\n",
    "    \n",
    "    plt.grid()\n",
    "        \n",
    "    # MAIN LOOP\n",
    "    for k in range(1, nSteps + 1):\n",
    "        #control is a wiggle with constant linear velocity\n",
    "        u = np.vstack((v, np.pi / 10 * np.sin(4 * np.pi * k/nSteps)))\n",
    "        \n",
    "        robot.step(u)   \n",
    "        \n",
    "        #draw occasionally\n",
    "        if (k-1)%20 == 0:\n",
    "            robot.draw(fig, ax)\n",
    "            fig.canvas.draw()\n",
    "            plt.pause(0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# RUN \n",
    "dT = 0.1 # time steps size\n",
    "pose = np.vstack([0., 0., 0.])\n",
    "\n",
    "robot = VelocityRobot(pose, dT)\n",
    "main(robot, nSteps=400)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2.2 Adding uncertainty\n",
    "\n",
    "Now we will include uncertainty to the previous exercise, changing the behaviour of the robot class you have implemented.\n",
    "\n",
    "In contrast to the noisy robot in practice 3.1, we will use the equations of the velocity motion model and their respective Jacobians to keep track of how confident we are of the robot's pose (i.e. the robot's pose now is also a gaussian distribution).\n",
    "\n",
    "Therefore, we have to deal with two Gaussian distributions: \n",
    "- the **pose** $\\sim(x_t, \\Sigma_{x_t})$ at time $t$,\n",
    "- and the **movement command** $\\sim(u_t, \\Sigma_{u_t})$, being applied during an interval of time $\\partial t$.\n",
    "\n",
    "The covariance of this movement ($\\Sigma_{u_t}$,`Q` in the code) is defined as seen below. It is constant throughout the execution of our code:\n",
    "\n",
    "$$\n",
    "    \\Sigma_{u_t} = \\begin{bmatrix}\n",
    "            \\sigma_v^2 & 0 \\\\\n",
    "            0 &  \\sigma_w^2\n",
    "        \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Whereas, the pose's covariance $\\Sigma_{x_t}$ (`P_t` in the code) has to be updated at every step of the execution. To achieve this you'll have to use:\n",
    "\n",
    "$$\\Sigma_{x_t} =  \\frac{\\partial g}{\\partial x_{t-1}} \\cdot \\Sigma_{x_{t-1}} \\cdot {\\frac{\\partial g}{\\partial x_{t-1}}}^T + \\frac{\\partial g}{\\partial u_{t}} \\cdot \\Sigma_{u_t} \\cdot {\\frac{\\partial g}{\\partial u_{t}}}^T$$\n",
    " \n",
    "Where $\\partial g / \\partial x_{t-1}$ and $\\partial g / \\partial u_{t}$ (`JacF_x` and `JacF_u` in the code) are the jacobians of our motion model evaluated at the previous pose $x_{t-1}$ and the current command $u_t$.\n",
    "\n",
    "As a practical exercise, you can derivate the ecuations of these jacobians and check them against the solution in the apendix of the slides."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <span style=\"color:green\">OPTIONAL</span>\n",
    "\n",
    "<span style=\"color:green\">Write a Markdown cell containing the Jacobians ecuations aforementioned.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:green\">***END OF OPTIONAL PART***</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Assignment**\n",
    "      \n",
    "1. Complete the following code calculating the covariance matrix $\\Sigma_{x_t}$ (`$P_k$` in the code)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def next_covariance(x, P, Q, u, dt):\n",
    "    ''' Compute the covariance of a robot following the velocity motion model\n",
    "\n",
    "        Args:\n",
    "            x: current pose (before movement)\n",
    "            u: differential command as a vector [v, w]''\n",
    "            dt: Time interval in which the movement occurs\n",
    "            P: current covariance of the pose\n",
    "            Q: covariance of our movement.\n",
    "    '''\n",
    "    # Aliases\n",
    "    v = u[0, 0]\n",
    "    w = u[1, 0]\n",
    "\n",
    "    sx, cx = np.sin(x[2, 0]), np.cos(x[2, 0]) #sin and cos for the previous robot heading\n",
    "    si, ci = np.sin(u[1, 0]*dt), np.cos(u[1, 0]*dt) #sin and cos for the heading increment\n",
    "    R = u[0, 0]/u[1, 0] #v/w Curvature radius\n",
    "\n",
    "    if u[1, 0] == 0:  #linear motion w=0 --> R = infinite\n",
    "        #TODO JACOBIAN HERE\n",
    "        JacF_x = np.array([\n",
    "            None,\n",
    "            None,\n",
    "            None\n",
    "        ])\n",
    "        JacF_u = np.array([\n",
    "            None,\n",
    "            None\n",
    "        ])\n",
    "    else: #Non-linear motion w=!0\n",
    "        # TODO JACOBIAN HERE\n",
    "        JacF_x = np.array([\n",
    "            None,\n",
    "            None,\n",
    "            None\n",
    "        ])\n",
    "\n",
    "        JacF_u = (\n",
    "            np.array([\n",
    "                None,\n",
    "                None,\n",
    "                None\n",
    "            ])@\n",
    "            np.array([\n",
    "                [1/w, -v/w**2],\n",
    "                [0, dt]\n",
    "            ])\n",
    "        )\n",
    "    #prediction steps\n",
    "    Pt = ( None @ None @ None ) + ( None @ None @ None )\n",
    "    \n",
    "    return Pt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2. Complete the methods:\n",
    "\n",
    "  - `step` to get the true robot pose (ground-truth) using the $Q$ matrix, \n",
    "  - and the `draw()` one to plot an ellipse representing the uncertainty about the robot pose centered at the expected robot pose (`self.pose`) as well as marks representing the ground truth poses.\n",
    "  \n",
    "\n",
    " \n",
    "**Example**\n",
    " \n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/fig3-2-2.png\" alt=\"\">\n",
    "  <figcaption>Fig. 2: Movement of a robot using velocity commands. <br/> Representing the expected pose (in red), the true pose (as dots) <br/> and the confidence ellipse.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class NoisyVelocityRobot(VelocityRobot):\n",
    "    \"\"\" Mobile robot implementation that uses velocity commands.\n",
    "       \n",
    "        Attr:\n",
    "            [...]: Inherited from VelocityRobot\n",
    "            true_pose: expected pose of the robot in the real world (noisy)\n",
    "            cov_pose: Covariance of the pose at each step\n",
    "            cov_move: Covariance of each movement. It is a constant\n",
    "\n",
    "    \"\"\"\n",
    "    \n",
    "    def __init__(self, mean, cov_pose, cov_move, dt):\n",
    "        super().__init__(mean, dt)\n",
    "        self.true_pose = mean\n",
    "        self.cov_pose = cov_pose\n",
    "        self.cov_move = cov_move\n",
    "        \n",
    "    def step(self, u):\n",
    "        self.cov_pose = next_covariance(None, None, None, None, None)\n",
    "        \n",
    "        super().step(u)\n",
    "        self.true_pose = next_pose(None, None, None, cov=None)\n",
    "        \n",
    "    def draw(self, fig, ax):\n",
    "        super().draw(fig, ax)\n",
    "        el = PlotEllipse(fig, ax, None, None)\n",
    "        ax.plot(None, None, 'o', color=el[0].get_color())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# RUN\n",
    "dT = 0.1 # time steps size\n",
    "\n",
    "SigmaV = 0.1 #Standard deviation of the linear velocity. \n",
    "SigmaW = 0.1 #Standard deviation of the angular velocity\n",
    "nSteps = 400 #Number of motions\n",
    "\n",
    "P = np.diag([0.2, 0.4, 0.]) #pose covariance matrix 3x3\n",
    "Q = np.diag([SigmaV**2, SigmaW**2]) #motion covariance matrix 2x2\n",
    "\n",
    "robot = NoisyVelocityRobot(np.vstack([0., 0., 0.]), P, Q, dT)\n",
    "main(robot, nSteps=nSteps)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.3. Movement of a robot using odometry commands <a id='odometry_commands'></a>\n",
    "\n",
    "The *odometry motion model* is more suitable to keep track and estimate the robot pose in contrast to the *velocity model*. The reason being, most robot bases provide some form of *odometry information*, a measurement of how much the robot has moved in reality, whose greater precision makes it useful to keep track of the pose.\n",
    "\n",
    "Although technically it is a measurement rather than a control, we \n",
    "treat it as control to simplify the modeling. The odometry commands take the form of:\n",
    "\n",
    "$$\n",
    "    u_t = \\begin{bmatrix}\n",
    "            \\Delta x  \\\\\n",
    "            \\Delta y \\\\\n",
    "            \\Delta \\theta\n",
    "        \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "We'll implement this motion model in both analytical and sample form."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# IMPORTS\n",
    "from utils.pause import pause\n",
    "from utils.Jacobians import J1, J2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.3.1 Analytical form\n",
    "\n",
    "Just as we did in lesson 3.1, the analytic form of the odometry motion model uses the composition of poses to model the robot's movement, providing only a notion of how much the pose has changed, not how did it get there.\n",
    "\n",
    "$$\n",
    "    p_1 \\oplus \\Delta p\n",
    "    = \\begin{bmatrix}\n",
    "        x_1 + \\Delta x \\cos \\theta_1 - \\Delta y \\sin \\theta_1 \\\\ \n",
    "        y_1 + \\Delta x \\sin \\theta_1 - \\Delta y \\cos \\theta_1 \\\\\n",
    "        \\theta_1 + \\Delta \\theta\n",
    "      \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "The crux of this assignment is to keep track of the covariance matrix of the pose $(P)$, using the Jacobians of this movement model."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Assignment**\n",
    "\n",
    "Similarly to the exercise 3.1, we'll move a robot along a 8-by-8 square (in meters), in increments of 2m. In this case you have to complete:\n",
    "\n",
    "- The `step()` method to compute the new expected pose (`self.pose`), the new true pose (ground-truth `self.true_pose`) after adding some noise to the movement command $u$ according to `Q`, and to update the uncertainty about the robot position (covariance matrix `self-P`).\n",
    "- The `draw()` method to plot the uncertainty of the pose (as an ellipse) and the true position (ground-truth).\n",
    "\n",
    "We are going to consider the following motion covariance matrix (it is already coded for you):\n",
    "\n",
    "$$\n",
    "    \\Sigma_{\\Delta_p} = \\begin{bmatrix}\n",
    "        0.04 & 0 & 0 \\\\\n",
    "        0 & 0.04 & 0 \\\\\n",
    "        0 & 0 & 0.01\n",
    "    \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "**Example**\n",
    "\n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/fig3-3-1.png\" width=\"400\" alt=\"\">\n",
    "  <figcaption>Fig. 1: Movement of a robot using odometry commands. <br/> Representing the expected pose (in red), the true pose (as dots) <br/> and the confidence ellipse.</figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Robot():\n",
    "    \"\"\" Simulation of a robot base\n",
    "    \n",
    "        Attrs:\n",
    "            pose: Expected pose of the robot\n",
    "            P: Covariance of the current pose\n",
    "            true_pose: Real pose of the robot(affected by noise)\n",
    "            Q: Covariance of the movement\n",
    "    \"\"\"\n",
    "    def __init__(self, x, P, Q):\n",
    "        self.pose = x\n",
    "        self.P = P\n",
    "        self.true_pose = self.pose\n",
    "        self.Q = Q\n",
    "        \n",
    "    def step(self, u):\n",
    "        # TODO Update expected pose\n",
    "        self.pose = tcomp(None, None)\n",
    "        \n",
    "        # TODO Generate true pose \n",
    "        noisy_u = np.vstack(None)\n",
    "        self.true_pose = tcomp(None, None)\n",
    "        \n",
    "        # TODO Update covariance\n",
    "        JacF_x = J1(None, None)\n",
    "        JacF_u = J2(None, None)\n",
    "\n",
    "        self.P = (\n",
    "            (None @ None @ None) \n",
    "            + (None @ None @ None)\n",
    "        )\n",
    "    \n",
    "    def draw(self, fig, ax):\n",
    "        DrawRobot(fig, ax, self.pose)\n",
    "        el = PlotEllipse(fig, ax, None, None)\n",
    "        ax.plot(None, None, 'o', color=el[0].get_color())\n",
    "  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def demo_odometry_commands_analytical(robot):  \n",
    "    # MATPLOTLIB\n",
    "    fig, ax = plt.subplots()\n",
    "    ax.set_xlim([-3, 11])\n",
    "    ax.set_ylim([-3, 11])\n",
    "    plt.ion()\n",
    "    plt.grid()\n",
    "    plt.tight_layout()\n",
    "    fig.canvas.draw()\n",
    "    \n",
    "    # MOVEMENT PARAMETERS\n",
    "    nSteps = 15\n",
    "    ang = -np.pi/2 # angle to turn in corners\n",
    "    u = np.vstack((2., 0., 0.))\n",
    "    \n",
    "    # MAIN LOOP\n",
    "    for i in range(nSteps):\n",
    "        # change angle on corners\n",
    "        if i % 4 == 3:\n",
    "            u[2, 0] = ang\n",
    "\n",
    "        #Update positions\n",
    "        robot.step(u)\n",
    "\n",
    "        # Restore angle iff changed\n",
    "        if i % 4 == 3:\n",
    "            u[2, 0] = 0\n",
    "\n",
    "        # Draw every loop\n",
    "        robot.draw(fig, ax)\n",
    "        fig.canvas.draw()\n",
    "        plt.pause(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = np.vstack([0., 0., np.pi/2]) # pose inicial\n",
    "\n",
    "# Probabilistic parameters\n",
    "P = np.diag([0., 0., 0.])\n",
    "Q = np.diag([0.04, 0.04, 0.01])\n",
    "\n",
    "robot = Robot(x, P, Q)\n",
    "demo_odometry_commands_analytical(robot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.3.2 Sample form\n",
    "\n",
    "The analytical form used above, although useful for the probabilistic algorithms we will cover in this course, does not work well for sampling algorithms such as particle filters.\n",
    "\n",
    "The reason being, if we generate random samples from the gaussian distributions generated in the previous exercise, we will find some poses that are not feasible to the non-holonomic movement of a robot, i.e. they do not correspond to a velocity command $(v, w)$ with noise.\n",
    "\n",
    "The following *sample form* is a more realistic way to generate samples of the pose. Now we model the movement of the robot as a sequence of actions: \n",
    "\n",
    "1. **Turn** ($\\theta_1$): to face the destination point.\n",
    "2. **Advance** ($d$): to arrive at the destination.\n",
    "3. **Turn** ($\\theta_2$): to get to the desired angle.\n",
    "\n",
    "So this type of order is expressed as:\n",
    "\n",
    "$$\n",
    "    u_t = \\begin{bmatrix}\n",
    "            \\theta_1  \\\\\n",
    "            d \\\\\n",
    "            \\theta_2\n",
    "        \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "It can easily be generated from odometry poses $[\\hat x_t, \\hat y_t,\\hat \\theta_t]'$ and $[\\hat x_{t-1}, \\hat y_{t-1},\\hat \\theta_{t-1}]'$ given the following equations:\n",
    "\n",
    "$$\n",
    "    \\begin{equation}\n",
    "    \\theta_1 =atan2(\\hat y_t -\\hat y_{t-1}, \\hat x_t -\\hat x_{t-1})- \\hat \\theta_{t-1} \\\\\n",
    "    d = \\sqrt{(\\hat y_t -\\hat y_{t-1})^2 + (\\hat x_t -\\hat x_{t-1})^2} \\\\\n",
    "    \\theta_2  = \\hat{\\theta}_t - \\hat{\\theta}_{t-1} - \\theta_1\n",
    "    \\end{equation}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Assignment**\n",
    "\n",
    "1. Implement a function that, given the previously mentioned $[\\hat x_t, \\hat y_t,\\hat \\theta_t]'$ and $[\\hat x_{t-1}, \\hat y_{t-1},\\hat \\theta_{t-1}]'$ generates an order $u_t = [ \\theta_1, d , \\theta_2 ]'$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def generate_move(pose_now, pose_old):\n",
    "    diff = pose_now - pose_old\n",
    "    theta1 = np.arctan2(None) - None\n",
    "    d = np.sqrt(None)\n",
    "    theta2 = None\n",
    "    return np.vstack((theta1, d, theta2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "generate_move(np.vstack([0., 0., 0.]), np.vstack([1., 1., np.pi/2]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "  Expected output for the commented example:\n",
    "\n",
    "  ```\n",
    "  array([[-3.92699082],\n",
    "       [ 1.41421356],\n",
    "       [ 2.35619449]])\n",
    "  ```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2. Using the resulting control action $u_t = [\\hat \\theta_1, \\hat d, \\hat \\theta_2]'$ we can model the noise of the action in the following way:\n",
    "\n",
    "  $$\n",
    "    \\begin{equation}\n",
    "        \\theta_1 = \\hat\\theta_1 + \\text{sample}\\left(\\alpha_0 \\hat\\theta_1^2 + \\alpha_1 \\hat d^2 \\right) \\\\\n",
    "        d = \\hat d + \\text{sample}\\left(\\alpha_2 \\hat d^2 + \\alpha_3 \\left(\\hat\\theta_1^2 + \\hat d^2 \\right) \\right) \\\\\n",
    "        \\theta_2 = \\hat\\theta_2 + \\text{sample}\\left(\\alpha_0 \\hat\\theta_2^2 + \\alpha_1 \\hat d^2\\right)\n",
    "    \\end{equation}\n",
    "  $$\n",
    "\n",
    "  Where $sample(b)$ generates a random value from a distribution $N(0, b)$. The vector $\\alpha = [\\alpha_0, \\dots, \\alpha_3]$ (`a` in the code), models the robot's intrinsic noise.\n",
    "\n",
    "  The pose of the robot at the end of the movement is computed as follows:\n",
    "\n",
    "$$\n",
    "    \\begin{equation}\n",
    "        x_t = x_{t-1} + d \\cos\\left(\\theta_{t-1} + \\theta_1 \\right) \\\\\n",
    "        y_t = y_{t-1} + d \\sin\\left(\\theta_{t-1} + \\theta_1 \\right) \\\\\n",
    "        \\theta_t = \\theta_{t-1} +  \\theta_1 +  \\theta_2\n",
    "    \\end{equation}\n",
    "$$\n",
    "\n",
    "Complete the `step()` and `draw()` methods to:\n",
    "- Update the expected robot pose and generate new samples.\n",
    "- Draw the true pose of the robot (without angle) as a cloud of particles (samples of possible points which the robot can be at). \n",
    "\n",
    "Play with different values of 'a'. To improve this visualization the robot will move in increments of $0.5$ and we are goint to plot the particles each 4 increments.\n",
    "\n",
    "**Example**\n",
    "\n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/fig3-3-2.png\" width=\"400\" alt=\"\">\n",
    "  <figcaption>Fig. 1: Movement of a robot using odometry commands in sampling form. <br/> Representing the expected pose (in red) and the samples (as clouds of dots) </figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class SampledRobot(object):\n",
    "    def __init__(self, mean, a, n_samples):\n",
    "        self.pose = mean\n",
    "        self.a = a\n",
    "        self.samples = np.tile(mean, n_samples)\n",
    "        \n",
    "    def step(self, u):\n",
    "        # TODO Update pose\n",
    "        ang = self.pose[2, 0] + u[0, 0]\n",
    "        self.pose[0, 0] += None\n",
    "        self.pose[1, 0] += None\n",
    "        self.pose[2, 0] = None\n",
    "                \n",
    "        # TODO Generate new samples\n",
    "        sample = lambda b: stats.norm(loc=0, scale=b).rvs(size=self.samples.shape[1])\n",
    "        \n",
    "        u2 = u**2\n",
    "        \n",
    "        noisy_u = u + np.vstack((\n",
    "            sample(None),\n",
    "            sample(None),\n",
    "            sample(None)\n",
    "        ))\n",
    "        \n",
    "        # TODO Update particles (robots) poses\n",
    "        ang = self.samples[2, :] + noisy_u[0, :]\n",
    "        \n",
    "        self.samples[0, :] += None\n",
    "        self.samples[1, :] += None\n",
    "        self.samples[2, :] = None\n",
    "        \n",
    "    def draw(self, fig, ax):\n",
    "        DrawRobot(fig, ax, self.pose)\n",
    "        ax.plot(None[0, :], None[1, :], '.')   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def demo_odometry_commands_sample(robot):\n",
    "    # PARAMETERS\n",
    "    inc = .5\n",
    "    show_each = 4\n",
    "    limit_iterations = 32\n",
    "    \n",
    "    # MATPLOTLIB\n",
    "    fig, ax = plt.subplots()\n",
    "    ax.set_xlim([-3, 11])\n",
    "    ax.set_ylim([-3, 11])\n",
    "    plt.ion()\n",
    "    plt.grid()\n",
    "    plt.tight_layout()\n",
    "    \n",
    "    # MAIN LOOP\n",
    "    robot.draw(fig, ax)\n",
    "    inc_pose = np.vstack((0., inc, 0.))\n",
    "    \n",
    "    for i in range(limit_iterations):\n",
    "        if i == 16:\n",
    "            inc_pose[0, 0] = inc\n",
    "            inc_pose[1, 0] = 0\n",
    "            inc_pose[2, 0] = -np.pi/2\n",
    "            \n",
    "        u = generate_move(robot.pose+inc_pose, robot.pose)\n",
    "        \n",
    "        robot.step(u)\n",
    "        \n",
    "        if i == 16:\n",
    "            inc_pose[2, 0] = 0\n",
    " \n",
    "        if i % show_each == show_each-1:\n",
    "            robot.draw(fig, ax)\n",
    "            fig.canvas.draw()\n",
    "            plt.pause(0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# RUN \n",
    "n_particles = 100\n",
    "a = np.array([.07, .07, .03, .05])\n",
    "x = np.vstack((0., 0., np.pi/2))\n",
    "\n",
    "robot = SampledRobot(x, a, n_particles)\n",
    "demo_odometry_commands_sample(robot)\n",
    "plt.close()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Student discussion\n",
    "In the cell below, discuss what has been done in the notebook, what you have found interesting, or any other relevant thought."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:green\">***Write your answer here***</span>"
   ]
  }
 ],
 "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.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}