PD-Controller.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x1a86e03d670>]"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 576x576 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# -----------\n",
    "# User Instructions\n",
    "#\n",
    "# Implement a PD controller by running 100 iterations\n",
    "# of robot motion. The steering angle should be set\n",
    "# by the parameter tau_p and tau_d so that:\n",
    "#\n",
    "# steering = -tau_p * CTE - tau_d * diff_CTE\n",
    "# where differential crosstrack error (diff_CTE)\n",
    "# is given by CTE(t) - CTE(t-1)\n",
    "#\n",
    "#\n",
    "# Only modify code at the bottom! Look for the TODO\n",
    "# ------------\n",
    " \n",
    "import random\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# ------------------------------------------------\n",
    "# \n",
    "# this is the Robot class\n",
    "#\n",
    "\n",
    "class Robot(object):\n",
    "    def __init__(self, length=20.0):\n",
    "        \"\"\"\n",
    "        Creates robot and initializes location/orientation to 0, 0, 0.\n",
    "        \"\"\"\n",
    "        self.x = 0.0\n",
    "        self.y = 0.0\n",
    "        self.orientation = 0.0\n",
    "        self.length = length\n",
    "        self.steering_noise = 0.0\n",
    "        self.distance_noise = 0.0\n",
    "        self.steering_drift = 0.0\n",
    "\n",
    "    def set(self, x, y, orientation):\n",
    "        \"\"\"\n",
    "        Sets a robot coordinate.\n",
    "        \"\"\"\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.orientation = orientation % (2.0 * np.pi)\n",
    "\n",
    "    def set_noise(self, steering_noise, distance_noise):\n",
    "        \"\"\"\n",
    "        Sets the noise parameters.\n",
    "        \"\"\"\n",
    "        # makes it possible to change the noise parameters\n",
    "        # this is often useful in particle filters\n",
    "        self.steering_noise = steering_noise\n",
    "        self.distance_noise = distance_noise\n",
    "\n",
    "    def set_steering_drift(self, drift):\n",
    "        \"\"\"\n",
    "        Sets the systematical steering drift parameter\n",
    "        \"\"\"\n",
    "        self.steering_drift = drift\n",
    "\n",
    "    def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):\n",
    "        \"\"\"\n",
    "        steering = front wheel steering angle, limited by max_steering_angle\n",
    "        distance = total distance driven, most be non-negative\n",
    "        \"\"\"\n",
    "        if steering > max_steering_angle:\n",
    "            steering = max_steering_angle\n",
    "        if steering < -max_steering_angle:\n",
    "            steering = -max_steering_angle\n",
    "        if distance < 0.0:\n",
    "            distance = 0.0\n",
    "\n",
    "        # apply noise\n",
    "        steering2 = random.gauss(steering, self.steering_noise)\n",
    "        distance2 = random.gauss(distance, self.distance_noise)\n",
    "\n",
    "        # apply steering drift\n",
    "        steering2 += self.steering_drift\n",
    "\n",
    "        # Execute motion\n",
    "        turn = np.tan(steering2) * distance2 / self.length\n",
    "\n",
    "        if abs(turn) < tolerance:\n",
    "            # approximate by straight line motion\n",
    "            self.x += distance2 * np.cos(self.orientation)\n",
    "            self.y += distance2 * np.sin(self.orientation)\n",
    "            self.orientation = (self.orientation + turn) % (2.0 * np.pi)\n",
    "        else:\n",
    "            # approximate bicycle model for motion\n",
    "            radius = distance2 / turn\n",
    "            cx = self.x - (np.sin(self.orientation) * radius)\n",
    "            cy = self.y + (np.cos(self.orientation) * radius)\n",
    "            self.orientation = (self.orientation + turn) % (2.0 * np.pi)\n",
    "            self.x = cx + (np.sin(self.orientation) * radius)\n",
    "            self.y = cy - (np.cos(self.orientation) * radius)\n",
    "\n",
    "    def __repr__(self):\n",
    "        return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)\n",
    "\n",
    "############## ADD / MODIFY CODE BELOW ####################\n",
    "# ------------------------------------------------------------------------\n",
    "#\n",
    "# run - does a single control run\n",
    "\n",
    "# previous P controller\n",
    "def run_p(robot, tau, n=100, speed=1.0):\n",
    "    x_trajectory = []\n",
    "    y_trajectory = []\n",
    "    for i in range(n):\n",
    "        cte = robot.y\n",
    "        steer = -tau * cte\n",
    "        robot.move(steer, speed)\n",
    "        x_trajectory.append(robot.x)\n",
    "        y_trajectory.append(robot.y)\n",
    "    return x_trajectory, y_trajectory\n",
    "    \n",
    "robot = Robot()\n",
    "robot.set(0, 1, 0)\n",
    "\n",
    "def run(robot, tau_p, tau_d, n=100, speed=1.0):\n",
    "    x_trajectory = []\n",
    "    y_trajectory = []\n",
    "    # TODO: your code here\n",
    "    prev_CTE = robot.y\n",
    "    for i in range(n):\n",
    "        CTE = robot.y\n",
    "        diff_CTE = CTE - prev_CTE\n",
    "        steering = -tau_p * CTE - tau_d * diff_CTE\n",
    "        prev_CTE = robot.y\n",
    "        robot.move(steering,speed)\n",
    "\n",
    "        x_trajectory.append(robot.x)\n",
    "        y_trajectory.append(robot.y)\n",
    "\n",
    "    return x_trajectory, y_trajectory\n",
    "    \n",
    "x_trajectory, y_trajectory = run(robot, 0.2, 3.0)\n",
    "n = len(x_trajectory)\n",
    "\n",
    "fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))\n",
    "ax1.plot(x_trajectory, y_trajectory, 'g', label='PD controller')\n",
    "ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.9.12 ('base')",
   "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.9.12"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "e572caa66b9d49951af5480c4e65bd874b4d163b9e077ed4369f2912d83d117b"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}