nonlinear_mpc_casadi.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Nonlinear MPC with Kinematic Bicycle Model\n",
    "\n",
    "The bicycle model is the same one used in <a href=https://link-springer-com.stanford.idm.oclc.org/article/10.1007/s13177-020-00226-1#Tab4>this paper (Figure 3)</a>\n",
    "\n",
    "The model has state and input:\n",
    "\\begin{gather*}\n",
    "z = \\begin{bmatrix}x\\\\ y\\\\ v\\\\ \\psi\\end{bmatrix},\\quad\n",
    "u = \\begin{bmatrix}a\\\\ \\delta_f\\end{bmatrix},\\quad\n",
    "\\end{gather*}\n",
    "\n",
    "The 4 states are:\n",
    "* $x$ and $y$, the car's position in the world frame\n",
    "* $v$, the car's velocity\n",
    "* $\\psi$, the angle of the car's centerline in the world frame\n",
    "\n",
    "\n",
    "The two control signals are $a$, the acceleration command, and $\\delta_f$, the front steering angle (in radians).\n",
    "\n",
    "$\\beta$ is the angle between the car's centerline and its velocity vector. It is not really a state, but a function of $\\delta_f$.\n",
    "\n",
    "The nonlinear model is:\n",
    "\\begin{gather*}\n",
    "\\begin{bmatrix}\n",
    "\\dot x\\\\ \\dot y\\\\ \\dot v\\\\ \\dot\\psi\n",
    "%\\\\ \\dot\\beta\n",
    "\\end{bmatrix} =\n",
    "\\begin{bmatrix}\n",
    "v\\cos(\\psi + \\beta)\n",
    "\\\\\n",
    "v\\sin(\\psi + \\beta)\n",
    "\\\\\n",
    "a\n",
    "\\\\\n",
    "\\frac{v}{l_r}\\sin(\\beta)\n",
    "%\\\\\n",
    "%\\frac{v}{l_f + l_r}\\tan(\\delta_f) - \\frac{v}{l_r}\\sin(\\beta)\n",
    "\\end{bmatrix}, \\quad \\beta = \\tan^{-1}\\Big(\\frac{l_r}{l_r + l_f}\\tan(\\delta_f)\\Big)\n",
    "\\end{gather*}\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Casadi setup\n",
    "From http://casadi.sourceforge.net/v3.4.4/users_guide/casadi-users_guide.pdf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# Library dependencies\n",
    "import casadi\n",
    "import numpy             as np\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib        as mpl\n",
    "import bezier\n",
    "\n",
    "# Local dependencies\n",
    "from direct_collocation  import MpcProblem\n",
    "from KinematicBicycleCar import KinematicBicycleCar\n",
    "\n",
    "from roadrunner_2        import Roadrunner, Segment, OutOfRoadPointsException\n",
    "\n",
    "# Generates a totally straight test_road\n",
    "test_road = np.ones((100,2))\n",
    "for i in range(100):\n",
    "    test_road[i,0] = i\n",
    "#from road import iso_double_lane_change as test_road\n",
    "    \n",
    "# Flips the test road, so the car goes from the end to the start.\n",
    "#test_road = np.flip(test_road, 0)\n",
    "\n",
    "test_road_width = 5.0*np.ones(np.size(test_road)//2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Initialize the car and road model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# Initialize the car model with 0.075 second steps and 30 step lookahead\n",
    "step = 0.075\n",
    "N = 30\n",
    "car = KinematicBicycleCar(N=N, step=step)\n",
    "DESIRED_SPEED = 4.0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "n_test_road_pts,_ = np.shape(test_road)\n",
    "\n",
    "roadrunner = Roadrunner(road_center = test_road, # Road centerline points\n",
    "                        road_width  = test_road_width,\n",
    "                        P           = np.size(test_road_width)-1,\n",
    "                       start_pct = 0.0, end_pct = 1.0) # Number of points to try curve-fitting at a time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Boundary functions\n",
    "We represent the road boundaries as a series of polygons. At each step $x_k$, $x_{k+1},...,x_N$, the car's x-y position is bounded by a 4-sided polygon.\n",
    "The polygons for successive states overlap, allowing the car to steer and slow down / speed up within the confines of the road.\n",
    "\n",
    "This image shows 5 successive bounds from $k$ to $k+5$. The $k+5^{th}$ bound is in red.\n",
    "![title](polygon_bounds.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# What we should do now\n",
    "# 1. Use the velocity estimate to \"step through\" the road,\n",
    "#    obtaining a set of road midpoints and constraint boundaries\n",
    "# 2. Add a term to the cost function that attracts the successive states to the road midpoints - done\n",
    "# 3. Use the midpoints to initialize the variable estimate so the solver is faster. - done,\n",
    "#    and we are initializing the new control uk to the previously computed one\n",
    "#    kind of warm-starting ipopt - done"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# stop sign test\n",
    "stop_sign = (30.0,1.0) # x,y\n",
    "stop_s = roadrunner.find_closest(stop_sign)\n",
    "\n",
    "def dist_to_stop_line(xy):\n",
    "    x0, y0 = stop_sign\n",
    "    x,y = xy\n",
    "    dist = np.sqrt((y-y0)**2 + (x-x0)**2)*np.cos(np.arctan2(y0-y, x0-x))\n",
    "    return dist\n",
    "\n",
    "stop_dist = 0.0 # meters, distance to stop sign when we should stop\n",
    "\n",
    "def desired_speed(k:int, xy:np.array):\n",
    "    xy = np.reshape(xy, (2,))\n",
    "    # 1: Find closest point to xy on road centerline\n",
    "    #s = roadrunner.find_closest(xy)\n",
    "    dist = dist_to_stop_line(xy)\n",
    "    # 3: Linear relationship between distance and speed\n",
    "    if dist > 10:\n",
    "        return DESIRED_SPEED\n",
    "    else:\n",
    "        return np.max([0, DESIRED_SPEED*(dist-stop_dist)/10.0])\n",
    "'''\n",
    "def desired_speed(k:int, xy:np.array):\n",
    "    return DESIRED_SPEED\n",
    "'''"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Run the MPC algorithm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Cost weights"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "weights = {\n",
    "       \"accuracy\":0.1, # 1.0\n",
    "           \"jerk\":500.0,\n",
    "\"steering change\":50.0,\n",
    "   \"acceleration\":500.0,\n",
    "}\n",
    "# use acceleration weight for stopping scenario and decrease accuracy by 10x\n",
    "\n",
    "#car.lr += 0.5 # distance from CG to back wheels\n",
    "#car.lf -= 0.5 # distance from CG to front wheels"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "roadrunner.reset()\n",
    "# start a bit ahead of the first point to have space for looking back on the road\n",
    "roadrunner.advance(step*20*DESIRED_SPEED)\n",
    "\n",
    "xy = np.reshape(roadrunner.evaluate(),(2,))\n",
    "ic = [xy[0], xy[1], DESIRED_SPEED, float(roadrunner.get_angle())]\n",
    "car.set_initial(ic, desired_speed, roadrunner)\n",
    "print(\"Starting at initial conditions\", ic)\n",
    "\n",
    "# This is the model:\n",
    "# https://link-springer-com.stanford.idm.oclc.org/article/10.1007/s13177-020-00226-1/figures/3\n",
    "mpcprob = MpcProblem(car, roadrunner,  weights)\n",
    "#mpcprob = MpcProblem(KinematicBicycle(\"base_vehicle_model.json\"),\n",
    "#                                      roadrunner,step, N, desired_speed )\n",
    "print(desired_speed(0, ic[0:2]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "\n",
    "# Controls the NUMBER of iterations to run the MPC algorithm.\n",
    "n_runs = 230\n",
    "\n",
    "x_plan = np.empty((mpcprob.model.n,n_runs+1)) # store the steps that get executed (n_runs)\n",
    "u_plan = np.empty((mpcprob.model.m,n_runs))   # store the control inputs that get executed\n",
    "x_true = np.empty((mpcprob.model.n,n_runs+1)) # store the state as simulated for each control input by an integrator\n",
    "centers = np.zeros((4,n_runs+1)) # store the state as simulated for each control input by an integrator\n",
    "centers[:,0] = [ic[0], ic[1], ic[3], ic[2]] # stupid indexing difference, sorry\n",
    "x_true[:,0] = ic # initial conditions\n",
    "x_plan[:,0] = ic\n",
    "# store the polygon boundary for each step, so we can plot them later\n",
    "polygon_boundaries = np.zeros((n_runs+N,4,2))\n",
    "\n",
    "fig_traj = plt.figure(figsize=(12,4))\n",
    "ax_traj = fig_traj.add_subplot(111)\n",
    "\n",
    "plt.ion()\n",
    "fig_traj.show()\n",
    "\n",
    "states = ax_traj.plot(x_true[0,0], x_true[1,0], color=\"red\")[0]\n",
    "#ax_traj.quiver(x_true[0,0], x_true[1,0], x_true[2,0]*np.cos(x_true[3,0]), x_true[2,0]*np.sin(x_true[3,0]), color=\"blue\")\n",
    "plt.xlim([0,100])\n",
    "\n",
    "fig_traj.canvas.draw()\n",
    "\n",
    "k = 0\n",
    "# test: hack? TODO fix, July 20th, 2021\n",
    "mpcprob.weights[\"acceleration\"] = 100.\n",
    "acc = mpcprob.weights[\"accuracy\"]\n",
    "\n",
    "try:\n",
    "    while k < n_runs:\n",
    "        # test: hack? TODO fix, July 20th, 2021\n",
    "        if k < 5:\n",
    "            mpcprob.weights[\"accuracy\"] = (k)/4.0*acc\n",
    "            mpcprob.weights[\"acceleration\"] -= 20.\n",
    "            \n",
    "        # Solve the MPC problem\n",
    "        xk_opt, uk_opt, sol = mpcprob.run(x_true[:,k])\n",
    "        \n",
    "        # for plotting\n",
    "        centers[:,k+1] = mpcprob.x_center_plot[:,1]\n",
    "        \n",
    "        \n",
    "        # Save the next state x_k+1 and control u_k+1\n",
    "        for i in range(mpcprob.model.n):\n",
    "            x_plan[i,k+1] = xk_opt[i][1]\n",
    "        for i in range(mpcprob.model.m):\n",
    "            u_plan[i,k] = uk_opt[i][1]\n",
    "        x_true[:,k+1] = mpcprob.simulate(x_true[:,k], u_plan[:,k])\n",
    "\n",
    "        # Compute distance traveled (approximately)\n",
    "        step_xy = np.linalg.norm(x_true[:,k+1]-x_true[:,k], 2)\n",
    "\n",
    "        # Save the 4 points that represent the polygon boundary at state k\n",
    "        bounds,p = roadrunner.bound_x(mpcprob.model.step,0, desired_speed)\n",
    "        polygon_boundaries[k,:,:] = p\n",
    "    \n",
    "        # Advance the roadrunner to our new state at k+1\n",
    "        step_xy = roadrunner.advance_xy(x_true[0:2,k+1])\n",
    "\n",
    "        print(\"Advanced by\", step_xy,\"to\", roadrunner.evaluate())\n",
    "\n",
    "\n",
    "        # Plot arrows and x-y states\n",
    "        ax_traj.clear()\n",
    "        ax_traj.scatter(x_true[0,:k], x_true[1,:k], color=\"navy\")\n",
    "        xy = np.reshape(roadrunner.evaluate(),(2,1))\n",
    "        ax_traj.scatter(xy[0], xy[1], color=\"red\")\n",
    "        # Plot the polygon fill\n",
    "        ax_traj.fill(polygon_boundaries[k,:,0], polygon_boundaries[k,:,1], \"lightblue\", alpha=0.5)\n",
    "        # Plot the line around each polygon\n",
    "        ax_traj.plot(np.concatenate([polygon_boundaries[k,:,0],polygon_boundaries[k,0:1,0]]),\n",
    "                np.concatenate([polygon_boundaries[k,:,1],polygon_boundaries[k,0:1,1]]),\n",
    "                color=\"navy\", alpha=0.5)\n",
    "\n",
    "\n",
    "        ax_traj.scatter(xk_opt[0], xk_opt[1], color=\"orange\")\n",
    "       # ax_traj.scatter(stop_sign[0], stop_sign[1], color=\"red\")\n",
    "\n",
    "        roadrunner.plot(ax=ax_traj, n_points=50)\n",
    "        \n",
    "        fig_traj.canvas.draw()\n",
    "        # Done!\n",
    "\n",
    "        if not sol['success']: # it failed\n",
    "            print(\"FAILED: ipopt returned status\", sol['success'], \"\\n\", sol['return_status'])\n",
    "            raise ValueError(\"ipopt failed and I need to make a better exception name\")\n",
    "        \n",
    "        k += 1\n",
    "        \n",
    "        # stop sign test??\n",
    "        \n",
    "        idx = xk_opt[0].size-1\n",
    "        xy_last = np.array([xk_opt[0][idx], xk_opt[1][idx]])\n",
    "        dist = dist_to_stop_line(xy_last)\n",
    "        print(\"\\n\\ndist\", dist)\n",
    "        mpcprob.indices_to_stop = None\n",
    "        while dist <= stop_dist and idx >= 0:\n",
    "            mpcprob.indices_to_stop = idx\n",
    "            \n",
    "            print(\"\\n\\nStopping index\", idx)\n",
    "            idx -= 1\n",
    "            xy_last = np.array([xk_opt[0][idx], xk_opt[1][idx]]) \n",
    "            dist = dist_to_stop_line(xy_last)\n",
    "        \n",
    "        # restart test??\n",
    "        if mpcprob.indices_to_stop == 0: # car is at a stop\n",
    "            # now begin moving forward\n",
    "            mpcprob.indices_to_stop = None\n",
    "\n",
    "            mpcprob.indices_to_start = N-1\n",
    "            \n",
    "        if mpcprob.indices_to_start is not None:\n",
    "            mpcprob.indices_to_start -= 1\n",
    "\n",
    "            if mpcprob.indices_to_start < 0:\n",
    "                mpcprob.indices_to_start = None\n",
    "        \n",
    "\n",
    "except OutOfRoadPointsException as e:\n",
    "    print(e)\n",
    "    x_plan = x_plan[:,:k+1] # Cut the vectors shorter so they don't have unitialized values\n",
    "    u_plan = u_plan[:,:k]  \n",
    "    x_true = x_true[:,k+1]\n",
    "    centers = centers[:,k+1]\n",
    "    # store the polygon boundary for each step, so we can plot them later\n",
    "    polygon_boundaries = polygon_boundaries[:k+N,:,:]\n",
    "finally:\n",
    "    fig_traj.canvas.draw()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# turn interactive off\n",
    "plt.ioff()\n",
    "# FOR PLOTTING: We also want to include the polygons for the lookahead steps k=1,...,N.\n",
    "polygon_boundaries[k-1:k+mpcprob.model.N,:,:] = mpcprob.p_plot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Control signals: acceleration and steering angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "from KinematicBicycleCar import plot_u\n",
    "u_plt, u_ax1, u_ax2 = plot_u(u_plan, mpcprob.u_opt, step, N)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## x-y plot of car's path\n",
    "Arrows show velocity. Orange components are planned but not executed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "from KinematicBicycleCar import plot_x\n",
    "x_plt, x_ax = plot_x(x_plan, mpcprob.x_opt, step, N, figsize=(12,3))\n",
    "    \n",
    "roadrunner.plot(ax=x_ax, n_points=50)\n",
    "plt.ylim(-5,15)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## x vs time and y vs time, separately, with the boundaries"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# Plot the road boundary as polygons\n",
    "def plot_polygons(polygon_boundaries, ax=None):\n",
    "    n_polys,_,_ = np.shape(polygon_boundaries)\n",
    "    \n",
    "    if ax is None:\n",
    "        fig, ax = plt.subplots(1,1,figsize=(12,4))\n",
    "        \n",
    "    for k in range(n_polys):\n",
    "        # Plot the polygon fill\n",
    "        ax.fill(polygon_boundaries[k,:,0], polygon_boundaries[k,:,1], \"lightblue\", alpha=0.1)\n",
    "        # Plot the line around each polygon\n",
    "        ax.plot(np.concatenate([polygon_boundaries[k,:,0],polygon_boundaries[k,0:1,0]]),\n",
    "                np.concatenate([polygon_boundaries[k,:,1],polygon_boundaries[k,0:1,1]]),\n",
    "                color=\"navy\", alpha=0.25)\n",
    "\n",
    "    # Plot the last polygon in RED\n",
    "    ax.plot(np.concatenate([polygon_boundaries[-1,:,0],polygon_boundaries[-1,0:1,0]]),\n",
    "            np.concatenate([polygon_boundaries[-1,:,1],polygon_boundaries[-1,0:1,1]]),\n",
    "            color=\"red\", alpha=0.5)\n",
    "    return ax\n",
    "\n",
    "ax = plot_polygons(polygon_boundaries)\n",
    "ax.scatter(ic[0], ic[1], color=\"green\")\n",
    "plt.title(\"Polygon boundaries along road\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "fig, ax = plt.subplots(1,1)\n",
    "ax.plot(step*np.arange(np.size(x_plan[2])), x_plan[2])\n",
    "plt.show()\n",
    "plt.ylim(9.95, 10.05)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "for x in np.transpose(x_plan):\n",
    "    print(\"[\"+\",\".join([str(xi) for xi in x])+\"],\")\n",
    "for x in np.transpose(mpcprob.x_opt):\n",
    "    print(\"[\"+\",\".join([str(xi) for xi in x])+\"],\")\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "for x in np.transpose(u_plan):\n",
    "    print(\"[\"+\",\".join([str(xi) for xi in x])+\"],\")\n",
    "for x in np.transpose(mpcprob.u_opt):\n",
    "    print(\"[\"+\",\".join([str(xi) for xi in x])+\"],\")\n",
    "    "
   ]
  }
 ],
 "metadata": {
  "@webio": {
   "lastCommId": null,
   "lastKernelId": null
  },
  "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.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}