{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Mobile Robot Control with PID\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We are going to implement a PID controller to control the orientation of the robot. The goal is to move the robot to a desired position."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To control the robot movement, we can loop through 2 steps:\n",
    "\n",
    "1. Update the robot pose (𝑥, 𝑦, 𝜑);\n",
    "2. Generate the desired linear and angular speeds 𝑢_𝑑, 𝜔_𝑑.\n",
    "\n",
    "The first step can be implemented with Odometry-based localization (see `odometry-based_localization.ipynb`). The second step will be implemented with a simple PID controller with one input (orientation error) and one output (desired angular speed). By defining the desired linear speed (for example, as a constant value), the desired wheel speeds can be calculate (see the first function of the Notebook `robot_behaviors.ipynb`).\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, the orientation error needs to be calculated. For convenience, the function below also calculates the distance error from the desired position `(xd, yd)` to the actual robot position `(x, y)` obtained via odometry.\n",
    "\n",
    "Note that the desired orientation is the orientation of the straight line that connects the current and desired positions of the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_pose_error(xd, yd, x, y, phi):\n",
    "    \"\"\" Returns the position and orientation errors. \n",
    "        Orientation error is bounded between -pi and +pi radians.\n",
    "    \"\"\"\n",
    "    # Position error:\n",
    "    x_err = xd - x\n",
    "    y_err = yd - y\n",
    "    dist_err = np.sqrt(x_err**2 + y_err**2)\n",
    "\n",
    "    # Orientation error\n",
    "    phi_d = np.arctan2(y_err,x_err)\n",
    "    phi_err = phi_d - phi\n",
    "\n",
    "    # Limits the error to (-pi, pi):\n",
    "    phi_err_correct = np.arctan2(np.sin(phi_err),np.cos(phi_err))\n",
    "\n",
    "    return dist_err, phi_err_correct"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test the function for different values of current pose and desired position. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Distance error    = 1.4142135623730951 m.\n",
      "Orientation error = 1.5707963267948966 rad.\n"
     ]
    }
   ],
   "source": [
    "# Actual robot pose:\n",
    "x, y, phi = 0, 0, np.pi/4\n",
    "\n",
    "# Desired robot position:\n",
    "xd, yd = -1, 1\n",
    "\n",
    "position_err, orientation_err = get_pose_error(xd, yd, x, y, phi)\n",
    "\n",
    "print(f'Distance error    = {position_err} m.')\n",
    "print(f'Orientation error = {orientation_err} rad.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A PID controller calculates an output that is propotional to the error (P), to its integral (I), and to its derivative (D). A simple PID controller can be implemented as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pid_controller(e, e_prev, e_acc, delta_t, kp=1.0, kd=0, ki=0):\n",
    "    \"\"\" PID algortithm: must be executed every delta_t seconds\n",
    "    The error e must be calculated as: e = desired_value - actual_value\n",
    "    e_prev contains the error calculated in the previous step.\n",
    "    e_acc contains the integration (accumulation) term.\n",
    "    \"\"\"\n",
    "    P = kp*e                      # Proportional term; kp is the proportional gain\n",
    "    I = e_acc + ki*e*delta_t    # Intergral term; ki is the integral gain\n",
    "    D = kd*(e - e_prev)/delta_t   # Derivative term; kd is the derivative gain\n",
    "\n",
    "    output = P + I + D              # controller output\n",
    "\n",
    "    # store values for the next iteration\n",
    "    e_prev = e     # error value in the previous interation (to calculate the derivative term)\n",
    "    e_acc = I      # accumulated error value (to calculate the integral term)\n",
    "\n",
    "    return output, e_prev, e_acc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Desired angular speed w_d = 0.9440485924037328 rad/s.\n",
      "Previous error = 1.5707963267948966 rad.\n",
      "Accumulated error = 0.0015707963267948967.\n"
     ]
    }
   ],
   "source": [
    "# The values below are initialized to test the function. \n",
    "# When implementing this, you must update e_prev and e_acc properly at every step.\n",
    "e = orientation_err\n",
    "e_prev = orientation_err*0.9\n",
    "e_acc = 0\n",
    "\n",
    "delta_t = 0.01\n",
    "\n",
    "# Controller gains:\n",
    "kp = 0.5\n",
    "kd = 0.01\n",
    "ki = 0.1\n",
    "\n",
    "# Obtain the desired angular speed:\n",
    "w_d, e_prev, e_acc = pid_controller(e, e_prev, e_acc, delta_t, kp, kd, ki)\n",
    "\n",
    "print(f'Desired angular speed w_d = {w_d} rad/s.')\n",
    "print(f'Previous error = {e_prev} rad.')\n",
    "print(f'Accumulated error = {e_acc}.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Conclusion\n",
    "\n",
    "After completing this notebook, you should understand how to apply a PID controller to implement a go-to-goal behavior for a mobile 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
}