sim_traj_planning.ipynb

{
 "cells": [
  {
   "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",
    "\n",
    "import numpy as np\n",
    "from P1_astar import DetOccupancyGrid2D, AStar\n",
    "from P2_rrt import *\n",
    "from P3_traj_planning import compute_smoothed_traj, modify_traj_with_limits, SwitchingController\n",
    "import scipy.interpolate\n",
    "import matplotlib.pyplot as plt\n",
    "from HW1.P1_differential_flatness import *\n",
    "from HW1.P2_pose_stabilization import *\n",
    "from HW1.P3_trajectory_tracking import *\n",
    "from utils import generate_planning_problem\n",
    "\n",
    "plt.rcParams['figure.figsize'] = [14, 14] # Change default figure size"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Generate workspace, start and goal positions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "width = 100\n",
    "height = 100\n",
    "num_obs = 25\n",
    "min_size = 5\n",
    "max_size = 30\n",
    "\n",
    "occupancy, x_init, x_goal = generate_planning_problem(width, height, num_obs, min_size, max_size)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Solve A* planning problem"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "astar = AStar((0, 0), (width, height), x_init, x_goal, occupancy)\n",
    "if not astar.solve():\n",
    "    print \"No path found\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Smooth Trajectory Generation\n",
    "### Trajectory parameters\n",
    "(Try changing these and see what happens)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_des = 0.3  # Nominal velocity\n",
    "alpha = 0.1   # Smoothness parameter\n",
    "dt = 0.05"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Generate smoothed trajectory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_smoothed, t_smoothed = compute_smoothed_traj(astar.path, V_des, alpha, dt)\n",
    "\n",
    "fig = plt.figure()\n",
    "astar.plot_path(fig.number)\n",
    "def plot_traj_smoothed(traj_smoothed):\n",
    "    plt.plot(traj_smoothed[:,0], traj_smoothed[:,1], color=\"red\", linewidth=2, label=\"solution path\", zorder=10)\n",
    "plot_traj_smoothed(traj_smoothed)\n",
    "plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Control-Feasible Trajectory Generation and Tracking\n",
    "### Robot control limits"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_max = 0.5 # max speed\n",
    "om_max = 1 # max rotational speed"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Tracking control gains\n",
    "Tune these as needed to improve tracking performance."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "kpx = 2\n",
    "kpy = 2\n",
    "kdx = 2\n",
    "kdy = 2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Generate control-feasible trajectory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t_new, V_smooth_scaled, om_smooth_scaled, traj_smooth_scaled = modify_traj_with_limits(traj_smoothed, t_smoothed, V_max, om_max, dt)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create trajectory controller and load trajectory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_controller = TrajectoryTracker(kpx=kpx, kpy=kpy, kdx=kdx, kdy=kdy, V_max=V_max, om_max=om_max)\n",
    "traj_controller.load_traj(t_new, traj_smooth_scaled)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Set simulation input noise\n",
    "(Try changing this and see what happens)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "noise_scale = 0.05"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Simulate closed-loop tracking of smoothed trajectory, compare to open-loop"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tf_actual = t_new[-1]\n",
    "times_cl = np.arange(0, tf_actual, dt)\n",
    "s_0 = State(x=x_init[0], y=x_init[1], V=V_max, th=traj_smooth_scaled[0,3])\n",
    "s_f = State(x=x_goal[0], y=x_goal[1], V=V_max, th=traj_smooth_scaled[-1,3])\n",
    "\n",
    "actions_ol = np.stack([V_smooth_scaled, om_smooth_scaled], axis=-1)\n",
    "states_ol, ctrl_ol = simulate_car_dyn(s_0.x, s_0.y, s_0.th, times_cl, actions=actions_ol, noise_scale=noise_scale)\n",
    "states_cl, ctrl_cl = simulate_car_dyn(s_0.x, s_0.y, s_0.th, times_cl, controller=traj_controller, noise_scale=noise_scale)\n",
    "\n",
    "fig = plt.figure()\n",
    "astar.plot_path(fig.number)\n",
    "plot_traj_smoothed(traj_smoothed)\n",
    "def plot_traj_ol(states_ol):\n",
    "    plt.plot(states_ol[:,0],states_ol[:,1], color=\"orange\", linewidth=1, label=\"open-loop path\", zorder=10)\n",
    "def plot_traj_cl(states_cl):\n",
    "    plt.plot(states_cl[:,0], states_cl[:,1], color=\"purple\", linewidth=1, label=\"TrajController closed-loop path\", zorder=10)\n",
    "plot_traj_ol(states_ol)\n",
    "plot_traj_cl(states_cl)\n",
    "plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=4)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Switching from Trajectory Tracking to Pose Stabilization Control\n",
    "### Zoom in on final pose error"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "l_window = 4.\n",
    "\n",
    "fig = plt.figure(figsize=[7,7])\n",
    "astar.plot_path(fig.number)\n",
    "plot_traj_smoothed(traj_smoothed)\n",
    "plot_traj_cl(states_cl)\n",
    "plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)\n",
    "plt.axis([x_goal[0]-l_window/2, x_goal[0]+l_window/2, x_goal[1]-l_window/2, x_goal[1]+l_window/2])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Pose stabilization control gains\n",
    "Tune these as needed to improve final pose stabilization."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "k1 = 1.\n",
    "k2 = 1.\n",
    "k3 = 1."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create pose controller and load goal pose\n",
    "Note we use the last value of the smoothed trajectory as the goal heading $\\theta$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pose_controller = PoseController(k1, k2, k3, V_max, om_max)\n",
    "pose_controller.load_goal(x_goal[0], x_goal[1], traj_smooth_scaled[-1,2])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Time before trajectory-tracking completion to switch to pose stabilization\n",
    "Try changing this!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t_before_switch = 5.0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create switching controller and compare performance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "switching_controller = SwitchingController(traj_controller, pose_controller, t_before_switch)\n",
    "\n",
    "t_extend = 60.0 # Extra time to simulate after the end of the nominal trajectory\n",
    "times_cl_extended = np.arange(0, tf_actual+t_extend, dt)\n",
    "states_cl_sw, ctrl_cl_sw = simulate_car_dyn(s_0.x, s_0.y, s_0.th, times_cl_extended, controller=switching_controller, noise_scale=noise_scale)\n",
    "\n",
    "fig = plt.figure()\n",
    "astar.plot_path(fig.number)\n",
    "plot_traj_smoothed(traj_smoothed)\n",
    "plot_traj_cl(states_cl)\n",
    "def plot_traj_cl_sw(states_cl_sw):\n",
    "    plt.plot(states_cl_sw[:,0], states_cl_sw[:,1], color=\"black\", linewidth=1, label=\"SwitchingController closed-loop path\", zorder=10)\n",
    "plot_traj_cl_sw(states_cl_sw)\n",
    "plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Zoom in on final pose"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "l_window = 4.\n",
    "\n",
    "fig = plt.figure(figsize=[7,7])\n",
    "astar.plot_path(fig.number)\n",
    "plot_traj_smoothed(traj_smoothed)\n",
    "plot_traj_ol(states_ol)\n",
    "plot_traj_cl(states_cl)\n",
    "plot_traj_cl_sw(states_cl_sw)\n",
    "plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)\n",
    "plt.axis([x_goal[0]-l_window/2, x_goal[0]+l_window/2, x_goal[1]-l_window/2, x_goal[1]+l_window/2])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Plot final sequence of states\n",
    "To see just how well we're able to arrive at the target point (and to assist in choosing values for the pose stabilization controller gains $k_1,k_2,k_3$), we plot the error in $x$ and $y$ for both the tracking controller and the switching controller at the end of the trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = len(times_cl) - int(t_before_switch/dt)\n",
    "fig = plt.figure(figsize=[10,10])\n",
    "plt.subplot(2,1,1)\n",
    "plt.plot([times_cl_extended[T], times_cl_extended[-1]], [0,0], linestyle='--', linewidth=1)\n",
    "plt.plot(times_cl[T:], states_cl[T:,0] - x_goal[0], label='TrajController')\n",
    "plt.plot(times_cl_extended[T:], states_cl_sw[T:,0] - x_goal[0], label='SwitchingController')\n",
    "plt.legend()\n",
    "plt.ylabel(\"x error (m)\")\n",
    "plt.subplot(3,1,2)\n",
    "plt.plot([times_cl_extended[T], times_cl_extended[-1]], [0,0], linestyle='--', linewidth=1)\n",
    "plt.plot(times_cl[T:], states_cl[T:,1] - x_goal[1], label='TrajController')\n",
    "plt.plot(times_cl_extended[T:], states_cl_sw[T:,1] - x_goal[1], label='SwitchingController')\n",
    "plt.legend()\n",
    "plt.ylabel(\"y error (m)\")\n",
    "plt.subplot(2,1,2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}