{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Behaviors for Mobile Robot Control\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Differential-drive robot\n",
    "\n",
    "A differential-drive has 2 driving wheels, one on its left and another on its right side. A third wheel (not motorized) is usually used to balance the structure. The difference in speed between the left and right wheels will change the orientation of the robot. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can create controllers that generate desired values for the speeds of left and right wheels. But, in this case, your controllers will only work for differential-drive robots. A more general approach is to create controllers that generate reference (desired) values of linear and angular speeds, and implement a function to convert those values into desired wheel speeds. An example of such function is shown below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "def wheel_speed_commands(u_d, w_d, D, R):\n",
    "    \"\"\"Converts desired speeds to wheel speed commands for a differential-drive robot.\n",
    "    Inputs:\n",
    "        u_d = desired linear speed for the robot [m/s]\n",
    "        w_d = desired angular speed for the robot [rad/s]\n",
    "        R = radius of the robot wheel [m]\n",
    "        D = distance between the left and right wheels [m]\n",
    "    Returns:\n",
    "        wr_d = desired speed for the right wheel [rad/s]\n",
    "        wl_d = desired speed for the left wheel [rad/s]\n",
    "    \"\"\"\n",
    "    wr_d = float((2*u_d + D*w_d)/(2*R))\n",
    "    wl_d = float((2*u_d - D*w_d)/(2*R))\n",
    "\n",
    "    return wl_d, wr_d"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test the function with different desired values:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Desired speed of the left wheel  = 5.512195121951219 rad/s\n",
      "Desired speed of the right wheel = 4.2439024390243905 rad/s\n"
     ]
    }
   ],
   "source": [
    "# Physical parameters of the robot for the kinematics model\n",
    "R = 0.0205    # radius of the wheels of the e-puck robot: 20.5mm \n",
    "D = 0.0520    # distance between the wheels of the e-puck robot: 52mm\n",
    "\n",
    "# Desired speeds:\n",
    "u_d = 0.1   # [m/s]\n",
    "w_d = -0.5  # [rad/s]\n",
    "\n",
    "wl_d, wr_d = wheel_speed_commands(u_d, w_d, D, R)\n",
    "\n",
    "print(f\"Desired speed of the left wheel  = {wl_d} rad/s\")\n",
    "print(f\"Desired speed of the right wheel = {wr_d} rad/s\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The above function was designed to convert desired linear and angular speeds into wheel speeds for a differential-drive robot. The purpose of such function is to serve as an abstraction layer between the robot moving controllers and the speed controllers of its own wheels. If you use a different structure (with omniwheels, for example), all you need to do is to change the equations to generate the speeds of each motor that corresponds to the desired linear and angular speeds. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Implementation of Robot Behaviors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Wall-following Behavior\n",
    "This function implements a wall-following behavior with constant linear speed.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def follow_wall_to_left(kp, d_l, d_desired):\n",
    "    \"\"\" Follows the wall to the left of the robot.\n",
    "    Input Parameters: \n",
    "        kp = proportional controller gain;\n",
    "        d_desired = desired robot distance to the wall;\n",
    "        d_l = measured distance to the left wall;\n",
    "    Returns:\n",
    "        u_ref = reference linear speed command;\n",
    "        w_ref = reference angluar speed command.\n",
    "    \"\"\"\n",
    "    u_ref = 0.5     # [m/s] some constant linear speed\n",
    "    w_ref = kp*(d_l - d_desired)\n",
    "    \n",
    "    return u_ref, w_ref"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Assume you want to follow the wall at a distance of 20cm, and the robot is actually at a distance of 18cm. When the function is called, it will return the desired speed values that the robot should have to get closer to the wall.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Desired linear speed  = 0.5m/s\n",
      "Desired angular speed = -0.020000000000000018rad/s\n"
     ]
    }
   ],
   "source": [
    "d_desired = 0.20    # [m]\n",
    "d_l = 0.18          # [m] - this value must be calculated based on sensor readings\n",
    "\n",
    "# Proportional controller gain: defines how the reaction of the robot will be:\n",
    "# higher controller gain will result in faster reaction, but it can cause oscillations\n",
    "kp = 1\n",
    "\n",
    "u_ref, w_ref = follow_wall_to_left(kp, d_l, d_desired)\n",
    "\n",
    "print(f\"Desired linear speed  = {u_ref}m/s\")\n",
    "print(f\"Desired angular speed = {w_ref}rad/s\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can calculate the desired speed of each wheel using the previous function:\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Desired speed of the left wheel  = 24.41560975609756 rad/s\n",
      "Desired speed of the right wheel = 24.364878048780486 rad/s\n"
     ]
    }
   ],
   "source": [
    "wl_d, wr_d = wheel_speed_commands(u_ref, w_ref, D, R)\n",
    "\n",
    "print(f\"Desired speed of the left wheel  = {wl_d} rad/s\")\n",
    "print(f\"Desired speed of the right wheel = {wr_d} rad/s\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Wall-following Behavior with Obstacle Detection\n",
    "This function implements a wall-following behavior with linear speed that depends on the distance to obstacle.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def follow_wall_to_left_obst(kp, d_l, d_desired, d):\n",
    "    \"\"\" Follows the wall to the left of the robot.\n",
    "    Input Parameters: \n",
    "        kp = proportional controller gain;\n",
    "        d_desired = desired robot distance to the wall;\n",
    "        d_l = measured distance to the left wall;\n",
    "        d = measured distance to the obstacle (front sensor);\n",
    "    Returns:\n",
    "        u_ref = reference linear speed command;\n",
    "        w_ref = reference angluar speed command.\n",
    "    \"\"\"\n",
    "    # Variables:\n",
    "    d_min = 0.05     # [m] minimum admissible distance to the obstacle; \n",
    "    d_max = 0.50     # [m] maximum measurable distance by the front sensor;\n",
    "    u_max = 0.5      # [m/s] linear speed for d = d_max\n",
    "\n",
    "    u_ref = u_max*d/d_max if d >= d_min else 0\n",
    "    w_ref = kp*(d_l - d_desired)\n",
    "    \n",
    "    return u_ref, w_ref"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Assume you want to follow the wall at a distance of 20cm, and the robot is actually at a distance of 18cm. When the function is called, it will return the desired speed values that the robot should have to get closer to the wall.\n",
    "\n",
    "Test the function with different values of `d`.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Desired linear speed  = 0.5m/s\n",
      "Desired angular speed = -0.020000000000000018rad/s\n",
      "Desired speed of the left wheel  = 24.41560975609756 rad/s\n",
      "Desired speed of the right wheel = 24.364878048780486 rad/s\n"
     ]
    }
   ],
   "source": [
    "d = 0.5\n",
    "d_desired = 0.20    # [m]\n",
    "d_l = 0.18          # [m] - this value must be calculated based on sensor readings\n",
    "\n",
    "# Proportional controller gain: defines how the reaction of the robot will be:\n",
    "# higher controller gain will result in  faster reaction, but it can cause oscillations\n",
    "kp = 1\n",
    "\n",
    "u_ref, w_ref = follow_wall_to_left_obst(kp, d_l, d_desired, d)\n",
    "wl_d, wr_d = wheel_speed_commands(u_ref, w_ref, D, R)\n",
    "\n",
    "print(f\"Desired linear speed  = {u_ref}m/s\")\n",
    "print(f\"Desired angular speed = {w_ref}rad/s\")\n",
    "print(f\"Desired speed of the left wheel  = {wl_d} rad/s\")\n",
    "print(f\"Desired speed of the right wheel = {wr_d} rad/s\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The above implementations of wall-following behavior will likely cause the robot to perform poorly. To make sure that the robot follows the wall properly, it is a good idea to include another term that tries to maintain the robot parallel to it. For that, we assume that the robot has two sensors that measure its distance to the left wall, one in the front and another in the back of the robot. The sensors are placed on a distance _s_ from each other."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def improved_follow_wall_to_left_obst(kp, kp2, d_fl, d_rl, d_desired, d):\n",
    "    \"\"\" Follows the wall to the left of the robot.\n",
    "    Input Parameters: \n",
    "        kp = controller gain for controlling the distance to the wall;\n",
    "        kp2 = controller gain for keeping the robot parallel to the wall;\n",
    "        d_desired = desired robot distance to the wall;\n",
    "        d_fl = distance to the left wall measured by the front sensor;\n",
    "        d_rl = distance to the left wall measured by the rear sensor;\n",
    "        d = measured distance to the obstacle (front sensor);\n",
    "    Returns:\n",
    "        u_ref = reference linear speed command;\n",
    "        w_ref = reference angluar speed command.\n",
    "    \"\"\"\n",
    "    # Variables:\n",
    "    d_min = 0.05     # [m] minimum admissible distance to the obstacle; \n",
    "    d_max = 0.50     # [m] maximum measurable distance by the front sensor;\n",
    "    u_max = 0.5      # [m/s] linear speed for d = d_max\n",
    "\n",
    "    u_ref = u_max * d/d_max if d >= d_min else 0\n",
    "    d_l = (d_fl + d_rl)/2\n",
    "    w_ref = kp*(d_l - d_desired) + kp2*(d_fl - d_rl)\n",
    "    \n",
    "    return u_ref, w_ref"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test it with different values for _d_fl_ and _d_rl_."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Desired linear speed  = 0.5m/s\n",
      "Desired angular speed = -0.030000000000000027rad/s\n",
      "Desired speed of the left wheel  = 24.428292682926827 rad/s\n",
      "Desired speed of the right wheel = 24.35219512195122 rad/s\n"
     ]
    }
   ],
   "source": [
    "d = 0.5\n",
    "d_desired = 0.20    # [m]\n",
    "d_fl = 0.18         # [m]\n",
    "d_rl = 0.20         # [m]\n",
    "\n",
    "\n",
    "# Controller gains: define how the reaction of the robot will be:\n",
    "# higher controller gains will result in faster reaction, but can cause oscillations\n",
    "kp = 1\n",
    "kp2 = 1\n",
    "\n",
    "u_ref, w_ref = improved_follow_wall_to_left_obst(kp, kp2, d_fl, d_rl, d_desired, d)\n",
    "wl_d, wr_d = wheel_speed_commands(u_ref, w_ref, D, R)\n",
    "\n",
    "print(f\"Desired linear speed  = {u_ref}m/s\")\n",
    "print(f\"Desired angular speed = {w_ref}rad/s\")\n",
    "print(f\"Desired speed of the left wheel  = {wl_d} rad/s\")\n",
    "print(f\"Desired speed of the right wheel = {wr_d} rad/s\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Conclusion\n",
    "\n",
    "After completing this notebook, you should understand how to build functions to implement simple behaviors for generic mobile robots. You are also able to convert the desired linear and angular speeds generated by the behaviors into speed commands for the wheels of 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
}