sim_trajectory.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Trajectory Optimization and Tracking Simulation\n",
    "\n",
    "In this notebook, we will run the differential flatness planner and trajectory tracking code from Problem 2."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The autoreload extension will automatically load in new code as you edit files, \n",
    "# so you don't need to restart the kernel every time\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "import numpy as np\n",
    "from matplotlib import pyplot as plt\n",
    "from utils import simulate_car_dyn, maybe_makedirs\n",
    "from P2_differential_flatness import State, compute_traj_coeffs, compute_traj, compute_controls\n",
    "from P2_trajectory_tracking import TrajectoryTracker\n",
    "\n",
    "maybe_makedirs('plots')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Various constants\n",
    "(Try changing these and seeing what happens)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_max = 0.75 # max speed\n",
    "om_max = 1 # max rotational speed\n",
    "tf  = 25 # final time (sec)\n",
    "N = 501 # number of timesteps\n",
    "dt = tf*1./(N-1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Starting and Final States\n",
    "(Try changing these and seeing what happens)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "s_0 = State(x=0, y=0, V=0.5, th=-np.pi/2)\n",
    "s_f = State(x=5, y=5, V=0.5, th=-np.pi/2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Trajectory Optimization\n",
    "First, we will use the implementations from problem 2 to compute a dynamically feasible trajectory from the starting point to the goal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "coeffs = compute_traj_coeffs(s_0, s_f, tf)\n",
    "t, traj = compute_traj(coeffs, tf, N)\n",
    "V, om = compute_controls(traj)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=[10,5])\n",
    "plt.subplot(2,2,1)\n",
    "plt.plot(t, traj[:,:3])\n",
    "plt.grid(True)\n",
    "plt.xlabel('t')\n",
    "plt.legend(labels=[\"x\",\"y\",\"th\"])\n",
    "plt.subplot(2,2,3)\n",
    "plt.plot(t, V)\n",
    "plt.plot(t, om)\n",
    "plt.grid(True)\n",
    "plt.xlabel('t')\n",
    "plt.legend(labels=[\"V\",\"om\"])\n",
    "plt.subplot(1,2,2)\n",
    "plt.plot(traj[:,0], traj[:,1], color='k')\n",
    "plt.xlabel('x')\n",
    "plt.ylabel('y')\n",
    "plt.grid(True)\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simulation\n",
    "Next, we will simulate how well we can actually execute these trajectories in the presense of noise. We will make use of the `simulate_car_dyn` function to do the simulation. Take a look in `utils.py` to see what it's doing under the hood."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "print(simulate_car_dyn.__doc__)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Choose a value of noise to apply at each step"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "noise_scale = np.sqrt(0.2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Open Loop\n",
    "First, we simulate tracking this trajectory without any feedback control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "actions_ol = np.stack([V, om], axis=-1)\n",
    "states_ol, ctrl_ol = simulate_car_dyn(s_0.x, s_0.y, s_0.th, t, actions=actions_ol, noise_scale=noise_scale)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=[10,5])\n",
    "plt.subplot(2,2,1)\n",
    "plt.plot(t, states_ol)\n",
    "plt.plot(t, traj[:,0], linestyle=':', color='C0', alpha=0.7)\n",
    "plt.plot(t, traj[:,1], linestyle=':', color='C1', alpha=0.7)\n",
    "plt.plot(t, traj[:,2], linestyle=':', color='C2', alpha=0.7)\n",
    "plt.grid(True)\n",
    "plt.xlabel('t')\n",
    "plt.legend(labels=[\"x\",\"y\",\"th\",\"nominal\"])\n",
    "plt.subplot(2,2,3)\n",
    "plt.plot(t[:-1], ctrl_ol)\n",
    "plt.grid(True)\n",
    "plt.xlabel('t')\n",
    "plt.legend(labels=[\"V\",\"om\"])\n",
    "plt.subplot(1,2,2)\n",
    "plt.plot(states_ol[:,0], states_ol[:,1])\n",
    "plt.plot(traj[:,0], traj[:,1], color=\"k\", linestyle=\":\", alpha=0.7)\n",
    "plt.legend(labels=[\"Open Loop\", \"Nominal\"])\n",
    "plt.xlabel('x')\n",
    "plt.ylabel('y')\n",
    "plt.grid(True)\n",
    "plt.tight_layout()\n",
    "plt.savefig(\"plots/sim_traj_openloop.pdf\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Closed Loop\n",
    "Next, we will use the tracking controller developed in problem 2 to apply feedback and track the trajectory in the presence of noise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try playing with these gains to see the impact on performance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "kpx = 2\n",
    "kpy = 2\n",
    "kdx = 2\n",
    "kdy = 2\n",
    "controller = TrajectoryTracker(kpx=kpx, kpy=kpy, kdx=kdx, kdy=kdy, V_max=V_max, om_max=om_max)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This controller designed to be similar to the controller module you'll use on the real robots for the final project. You can think of it as a module that will track whatever trajectory is given to it from another module (e.g. a planner module). So the first step is to load the controller with the trajectory we have planned"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "controller.load_traj(t, traj)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we can simulate."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "tf_actual = t[-1]\n",
    "times_cl = np.arange(0, tf_actual, dt)\n",
    "states_cl, ctrl_cl = simulate_car_dyn(s_0.x, s_0.y, s_0.th, times_cl, controller=controller, noise_scale=noise_scale)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=[10,5])\n",
    "plt.subplot(2,2,1)\n",
    "plt.plot(times_cl, states_cl)\n",
    "plt.plot(t, traj[:,0], linestyle=':', color='C0', alpha=0.7)\n",
    "plt.plot(t, traj[:,1], linestyle=':', color='C1', alpha=0.7)\n",
    "plt.plot(t, traj[:,2], linestyle=':', color='C2', alpha=0.7)\n",
    "plt.grid(True)\n",
    "plt.xlabel('t')\n",
    "plt.legend(labels=[\"x\",\"y\",\"th\",\"Nominal\"])\n",
    "plt.subplot(2,2,3)\n",
    "plt.plot(times_cl[:-1], ctrl_cl)\n",
    "plt.plot(t[:-1], ctrl_ol[:,0], color='C0', linestyle=':', alpha=0.7)\n",
    "plt.plot(t[:-1], ctrl_ol[:,1], color='C1', linestyle=':', alpha=0.7)\n",
    "plt.grid(True)\n",
    "plt.xlabel('t')\n",
    "plt.legend(labels=[\"V\",\"om\", \"Nominal\"])\n",
    "plt.subplot(1,2,2)\n",
    "plt.plot(traj[:,0], traj[:,1], color=\"k\", linestyle=\":\", alpha=0.7)\n",
    "plt.plot(states_ol[:,0], states_ol[:,1], alpha=0.7)\n",
    "plt.plot(states_cl[:,0], states_cl[:,1])\n",
    "plt.legend(labels=[\"Nominal\", \"Open Loop\", \"Closed Loop\"])\n",
    "plt.xlabel('x')\n",
    "plt.ylabel('y')\n",
    "plt.grid(True)\n",
    "plt.tight_layout()\n",
    "plt.savefig(\"plots/sim_traj_closedloop.pdf\")\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.9.10 64-bit",
   "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.10"
  },
  "vscode": {
   "interpreter": {
    "hash": "aee8b7b246df8f9039afb4144a1f6fd8d2ca17a180786b69acc140d282b71a49"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}