{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "7308eded-83b7-413e-ac51-fbf87e96b396",
   "metadata": {},
   "outputs": [],
   "source": [
    "# https://python-control.readthedocs.io/en/latest/optimal.html"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "c44a6607-4dc9-4f71-a4ee-4ff0ea751d15",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import control as ct\n",
    "import control.optimal as opt\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "e3c9bd6f-51ae-4c29-80ef-b6ae4a452655",
   "metadata": {},
   "outputs": [],
   "source": [
    "def vehicle_update(t, x, u, params):\n",
    "    # Get the parameters for the model\n",
    "    l = params.get('wheelbase', 3.)         # vehicle wheelbase\n",
    "    phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)\n",
    "\n",
    "    # Saturate the steering input\n",
    "    phi = np.clip(u[1], -phimax, phimax)\n",
    "\n",
    "    # Return the derivative of the state\n",
    "    return np.array([\n",
    "        np.cos(x[2]) * u[0],            # xdot = cos(theta) v\n",
    "        np.sin(x[2]) * u[0],            # ydot = sin(theta) v\n",
    "        (u[0] / l) * np.tan(phi)        # thdot = v/l tan(phi)\n",
    "    ])\n",
    "\n",
    "def vehicle_output(t, x, u, params):\n",
    "    return x                            # return x, y, theta (full state)\n",
    "\n",
    "# Define the vehicle steering dynamics as an input/output system\n",
    "vehicle = ct.NonlinearIOSystem(\n",
    "    vehicle_update, vehicle_output, states=3, name='vehicle',\n",
    "    inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "48617bfa-1d36-4e7d-abc6-8830c1432b07",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We consider an optimal control problem that consists of “changing lanes” by moving from the \n",
    "# point x = 0 m, y = -2 m, \\theta = 0 to the point x = 100 m, y = 2 m, \\theta = 0) over a \n",
    "# period of 10 seconds and with a with a starting and ending velocity of 10 m/s:\n",
    "\n",
    "x0 = [0., -2., 0.]; u0 = [10., 0.]\n",
    "xf = [100., 2., 0.]; uf = [10., 0.]\n",
    "Tf = 10"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "d579d0be-0e94-4263-8db0-d2293657f72d",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = np.diag([0.1, 10, .1])    # keep lateral error low\n",
    "R = np.eye(2) * 0.1\n",
    "cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "1bed10b4-199c-4a06-932a-a7fd2dc5ce0d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We also constraint the maximum turning rate to 0.1 radians (about 6 degees) and \n",
    "# constrain the velocity to be in the range of 9 m/s to 11 m/s:\n",
    "constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "09bee894-2ecf-4b09-9d56-0da56077c4b3",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Summary statistics:\n",
      "* Cost function calls: 41\n",
      "* Constraint calls: 84\n",
      "* System simulations: 123\n"
     ]
    }
   ],
   "source": [
    "horizon = np.linspace(0, Tf, 20, endpoint=True)\n",
    "bend_left = [10, 0.01]        # slight left veer\n",
    "\n",
    "result = opt.solve_ocp(\n",
    "    vehicle, horizon, x0, cost, constraints, initial_guess=bend_left,\n",
    "    options={'eps': 0.01})    # set step size for gradient calculation\n",
    "\n",
    "# Extract the results\n",
    "u = result.inputs\n",
    "t, y = ct.input_output_response(vehicle, horizon, u, x0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "660960a9-1d71-43ca-ba54-38afd075308d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 3 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Plot the results\n",
    "plt.subplot(3, 1, 1)\n",
    "plt.plot(y[0], y[1])\n",
    "plt.plot(x0[0], x0[1], 'ro', xf[0], xf[1], 'ro')\n",
    "plt.xlabel(\"x [m]\")\n",
    "plt.ylabel(\"y [m]\")\n",
    "\n",
    "plt.subplot(3, 1, 2)\n",
    "plt.plot(t, u[0])\n",
    "plt.axis([0, 10, 8.5, 11.5])\n",
    "plt.plot([0, 10], [9, 9], 'k--', [0, 10], [11, 11], 'k--')\n",
    "plt.xlabel(\"t [sec]\")\n",
    "plt.ylabel(\"u1 [m/s]\")\n",
    "\n",
    "plt.subplot(3, 1, 3)\n",
    "plt.plot(t, u[1])\n",
    "plt.axis([0, 10, -0.15, 0.15])\n",
    "plt.plot([0, 10], [-0.1, -0.1], 'k--', [0, 10], [0.1, 0.1], 'k--')\n",
    "plt.xlabel(\"t [sec]\")\n",
    "plt.ylabel(\"u2 [rad/s]\")\n",
    "\n",
    "plt.suptitle(\"Lane change manuever\")\n",
    "plt.tight_layout()\n",
    "plt.savefig('example.jpg')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a7c5a4fe-0ed7-4df0-a9ed-6667d8fa01f4",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}