{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Odometry-based Localization\n",
    "\n",
    "### for the differential-drive robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Localization is based on the kinematics model of the robot and the readings of its wheel encoders. The basic steps for implementing it are:\n",
    "\n",
    "* We start from a known pose: for example, (𝑥=0,𝑦=0, 𝜑=0).\n",
    "* By counting encoder pulses, we calculate the angular speeds of each wheel 𝜔_𝑟, 𝜔_𝑙. \n",
    "* The linear and angular speeds of the robot (𝑢, 𝜔) are calculated.\n",
    "* The displacement of the robot (∆𝑥,∆𝑦, ∆𝜑) is obtained.\n",
    "* Finally, the values of position 𝑥, 𝑦 and orientation 𝜑 are updated at every cycle by integrating the results.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Assuming the encoder readings are stored in the lists `encoderValues` and `oldEncoderValues`, the following function can be used to calculate the speed of each wheel. The variable `delta_t` contains the time interval (in seconds) between function calls, and `pulses_per_turn` indicates how many pulses each encoder generates per full turn of each wheel."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_wheels_speed(encoderValues, oldEncoderValues, pulses_per_turn, delta_t):\n",
    "    \"\"\"Computes speed of the wheels based on encoder readings\n",
    "    \"\"\"\n",
    "    # Calculate the change in angular position of the wheels:\n",
    "    ang_diff_l = 2*np.pi*(encoderValues[0] - oldEncoderValues[0])/pulses_per_turn\n",
    "    ang_diff_r = 2*np.pi*(encoderValues[1] - oldEncoderValues[1])/pulses_per_turn\n",
    "\n",
    "    # Calculate the angular speeds:\n",
    "    wl = ang_diff_l/delta_t\n",
    "    wr = ang_diff_r/delta_t\n",
    "\n",
    "    return wl, wr\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test the function with different encoder values and delta_t."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Left wheel speed  = 5.235987755982988 rad/s.\n",
      "Right wheel speed = 13.089969389957469 rad/s.\n"
     ]
    }
   ],
   "source": [
    "pulses_per_turn = 72\n",
    "delta_t = 0.1  # time step in seconds\n",
    "encoderValues = [1506, 1515]  # Accumulated number of pulses for the left [0] and right [1] encoders.\n",
    "oldEncoderValues = [1500, 1500]     # Accumulated pulses for the left and right encoders in the previous step\n",
    "\n",
    "wl, wr = get_wheels_speed(encoderValues, oldEncoderValues, pulses_per_turn, delta_t)\n",
    "\n",
    "print(f'Left wheel speed  = {wl} rad/s.')\n",
    "print(f'Right wheel speed = {wr} rad/s.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The function below calculates the linear and angular speeds of the robot based on the speeds of its wheels and its physical parameters: `R` is the radius of the wheels and `D` is the distance between the left and right wheels."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_robot_speeds(wl, wr, R, D):\n",
    "    \"\"\"Computes robot linear and angular speeds\"\"\"\n",
    "    u = R/2.0 * (wr + wl)\n",
    "    w = R/D * (wr - wl)\n",
    "    \n",
    "    return u, w\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Robot linear speed  = 0.9162978572970228 m/s\n",
      "Robot angular speed = 1.9634954084936203 rad/s\n"
     ]
    }
   ],
   "source": [
    "# Physical parameters of the robot for the kinematics model\n",
    "R = 0.10    # radius of the wheels of the e-puck robot: 20.5mm \n",
    "D = 0.40    # distance between the wheels of the e-puck robot: 52mm\n",
    "\n",
    "u, w = get_robot_speeds(wl, wr, R, D)\n",
    "\n",
    "print(f\"Robot linear speed  = {u} m/s\")\n",
    "print(f\"Robot angular speed = {w} rad/s\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, the new robot pose can be calculated based on the kinematics model, robot speed and previous pose."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_robot_pose(u, w, x_old, y_old, phi_old, delta_t):\n",
    "    \"\"\"Updates robot pose based on heading and linear and angular speeds\"\"\"\n",
    "    delta_phi = w * delta_t\n",
    "    phi = phi_old + delta_phi\n",
    "    \n",
    "    if phi >= np.pi:\n",
    "        phi = phi - 2*np.pi\n",
    "    elif phi < -np.pi:\n",
    "        phi = phi + 2*np.pi\n",
    "\n",
    "    delta_x = u * np.cos(phi) * delta_t\n",
    "    delta_y = u * np.sin(phi) * delta_t\n",
    "    x = x_old + delta_x\n",
    "    y = y_old + delta_y\n",
    "    \n",
    "    return x, y, phi\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test the function with different speeds and poses."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The new robot pose is: 2.000 m, 3.980 m, -89.141 deg.\n"
     ]
    }
   ],
   "source": [
    "x_old, y_old, phi_old = 2.0, 4.0, -np.pi/2  # Robot pose in the previous step\n",
    "u = 0.2         # m/s\n",
    "w = 0.15        # rad/s\n",
    "delta_t = 0.1   # s\n",
    "\n",
    "x, y, phi = get_robot_pose(u, w, x_old, y_old, phi_old, delta_t)\n",
    "\n",
    "print(f\"The new robot pose is: {x:.3f} m, {y:.3f} m, {phi*180/np.pi:.3f} deg.\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Conclusion\n",
    "\n",
    "After completing this notebook, you should understand how to build functions to implement odometry-based localization for a differential-drive robot."
   ]
  }
 ],
 "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.10.5"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}