mpc_python_learn/notebooks/old_scipy_implementation/MPC_tracking_cvxpy.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.integrate import odeint\n",
    "from scipy.interpolate import interp1d\n",
    "import cvxpy as cp\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### kinematics model equations\n",
    "\n",
    "The variables of the model are:\n",
    "\n",
    "* $x$ coordinate of the robot\n",
    "* $y$ coordinate of the robot\n",
    "* $\\theta$ heading of the robot\n",
    "* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n",
    "* $cte$ crosstrack error = lateral distance of the robot from the path \n",
    "\n",
    "The inputs of the model are:\n",
    "\n",
    "* $v$ linear velocity of the robot\n",
    "* $w$ angular velocity of the robot\n",
    "\n",
    "These are the differential equations f(x,u) of the model:\n",
    "\n",
    "* $\\dot{x} = v\\cos{\\theta}$ \n",
    "* $\\dot{y} = v\\sin{\\theta}$\n",
    "* $\\dot{\\theta} = w$\n",
    "* $\\dot{\\psi} = -w$\n",
    "* $\\dot{cte} = v\\sin{-\\psi}$\n",
    "\n",
    "The **Continuous** State Space Model is\n",
    "\n",
    "$ {\\dot{x}} = Ax + Bu $\n",
    "\n",
    "with:\n",
    "\n",
    "$ A =\n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} & \\frac{\\partial f(x,u)}{\\partial \\psi} & \\frac{\\partial f(x,u)}{\\partial cte} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "=\n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "0 & 0 & -vsin(\\theta) & 0 & 0 \\\\\n",
    "0 & 0 & vcos(\\theta) & 0 & 0 \\\\\n",
    "0 & 0 & 0 & 0 & 0 \\\\\n",
    "0 & 0 & 0 & 0 & 0 \\\\\n",
    "0 & 0 & 0 & -vcos(-\\psi) & 0 \n",
    "\\end{bmatrix}\n",
    "\\quad $\n",
    "\n",
    "\n",
    "$ B = \\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial w} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "=\n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "\\cos{\\theta_t} & 0 \\\\\n",
    "\\sin{\\theta_t} & 0 \\\\\n",
    "0 & 1 \\\\\n",
    "0 & -1 \\\\\n",
    "-\\sin{(-\\psi_t)} & 0  \n",
    "\\end{bmatrix}\n",
    "\\quad $\n",
    "\n",
    "discretize with forward Euler Integration for time step dt:\n",
    "\n",
    "* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}*dt$\n",
    "* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}*dt$\n",
    "* ${\\theta_{t+1}} = \\theta_{t} + w_t*dt$\n",
    "* ${\\psi_{t+1}} = \\psi_{t} - w_t*dt$\n",
    "* ${cte_{t+1}} = cte_{t} + v_t\\sin{-\\psi}*dt$\n",
    "\n",
    "The **Discrete** State Space Model is then:\n",
    "\n",
    "${x_{t+1}} = Ax_t + Bu_t $\n",
    "\n",
    "with:\n",
    "\n",
    "$\n",
    "A = \\quad\n",
    "\\begin{bmatrix}\n",
    "1 & 0 & -v\\sin{\\theta}dt & 0 & 0 \\\\\n",
    "0 & 1 & v\\cos{\\theta}dt & 0 & 0 \\\\\n",
    "0 & 0 & 1 & 0 & 0 \\\\\n",
    "0 & 0 & 0 & 1 & 0 \\\\\n",
    "0 & 0 & 0 & -vcos{(-\\psi)}dt & 1 \n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "$\n",
    "\n",
    "$\n",
    "B = \\quad\n",
    "\\begin{bmatrix}\n",
    "\\cos{\\theta_t}dt & 0 \\\\\n",
    "\\sin{\\theta_t}dt & 0 \\\\\n",
    "0 & dt \\\\\n",
    "0 & -dt \\\\\n",
    "-\\sin{-\\psi_t}dt & 0  \n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "$\n",
    "\n",
    "This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
    "\n",
    "$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) +  A(x-\\bar{x}) + B(u-\\bar{u})$\n",
    "\n",
    "So:\n",
    "\n",
    "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) +  A(x_t-\\bar{x}) + B(u_t-\\bar{u}) )dt $\n",
    "\n",
    "$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
    "\n",
    "The Discrete linearized kinematics model is\n",
    "\n",
    "$ x_{t+1} = A'x_t + B' u_t + C' $\n",
    "\n",
    "with:\n",
    "\n",
    "$ A' = I+dtA $\n",
    "\n",
    "$ B' = dtB $\n",
    "\n",
    "$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}) $"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "------------------\n",
    "NB: psi and cte are expressed w.r.t. the track as reference frame.\n",
    "In the reference frame of the veicle the equtions would be:\n",
    "* psi_dot = w\n",
    "* cte_dot = sin(psi)\n",
    "-----------------"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "-----------------\n",
    "[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
    "\n",
    "In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
    "\n",
    "Typically it is possible to assume that the system is operating about some nominal\n",
    "state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
    "\n",
    "Recall that the Taylor Series expansion of f(x) around the\n",
    "point $\\bar{x}$ is given by:\n",
    "\n",
    "$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
    "\n",
    "For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
    "\n",
    "The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
    "is given by:\n",
    "\n",
    "$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
    "\n",
    "Where:\n",
    "\n",
    "$ A =\n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "$ and $ B = \\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad $\n",
    "\n",
    "Then:\n",
    "\n",
    "$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
    "\n",
    "-----------------"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Kinematics Model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Control problem statement.\n",
    "\n",
    "N = 5  # number of state variables\n",
    "M = 2  # number of control variables\n",
    "T = 20  # Prediction Horizon\n",
    "dt = 0.25  # discretization step\n",
    "\n",
    "x = cp.Variable((N, T + 1))\n",
    "u = cp.Variable((M, T))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_linear_model(x_bar, u_bar):\n",
    "    \"\"\" \"\"\"\n",
    "\n",
    "    x = x_bar[0]\n",
    "    y = x_bar[1]\n",
    "    theta = x_bar[2]\n",
    "    psi = x_bar[3]\n",
    "    cte = x_bar[4]\n",
    "\n",
    "    v = u_bar[0]\n",
    "    w = u_bar[1]\n",
    "\n",
    "    A = np.zeros((N, N))\n",
    "    A[0, 2] = -v * np.sin(theta)\n",
    "    A[1, 2] = v * np.cos(theta)\n",
    "    A[4, 3] = -v * np.cos(-psi)\n",
    "    A_lin = np.eye(N) + dt * A\n",
    "\n",
    "    B = np.zeros((N, M))\n",
    "    B[0, 0] = np.cos(theta)\n",
    "    B[1, 0] = np.sin(theta)\n",
    "    B[2, 1] = 1\n",
    "    B[3, 1] = -1\n",
    "    B[4, 0] = -np.sin(-psi)\n",
    "    B_lin = dt * B\n",
    "\n",
    "    f_xu = np.array(\n",
    "        [v * np.cos(theta), v * np.sin(theta), w, -w, v * np.sin(-psi)]\n",
    "    ).reshape(N, 1)\n",
    "    C_lin = dt * (\n",
    "        f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
    "    )\n",
    "\n",
    "    return A_lin, B_lin, C_lin"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Motion Prediction: using scipy intergration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define process model\n",
    "def kinematics_model(x, t, u):\n",
    "    \"\"\" \"\"\"\n",
    "\n",
    "    dxdt = u[0] * np.cos(x[2])\n",
    "    dydt = u[0] * np.sin(x[2])\n",
    "    dthetadt = u[1]\n",
    "    dpsidt = -u[1]\n",
    "    dctedt = u[0] * np.sin(-x[3])\n",
    "\n",
    "    dqdt = [dxdt, dydt, dthetadt, dpsidt, dctedt]\n",
    "\n",
    "    return dqdt\n",
    "\n",
    "\n",
    "def predict(x0, u):\n",
    "    \"\"\" \"\"\"\n",
    "\n",
    "    x_bar = np.zeros((N, T + 1))\n",
    "\n",
    "    x_bar[:, 0] = x0\n",
    "\n",
    "    # solve ODE\n",
    "    for t in range(1, T + 1):\n",
    "\n",
    "        tspan = [0, dt]\n",
    "        x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
    "\n",
    "        x0 = x_next[1]\n",
    "        x_bar[:, t] = x_next[1]\n",
    "\n",
    "    return x_bar"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Validate the model, here the status w.r.t a straight line with constant heading 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 4.59 ms, sys: 294 µs, total: 4.89 ms\n",
      "Wall time: 4.68 ms\n"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 1  # m/s\n",
    "u_bar[1, :] = np.radians(-10)  # rad/s\n",
    "\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = 1\n",
    "x0[2] = np.radians(0)\n",
    "x0[3] = 0\n",
    "x0[4] = 1\n",
    "\n",
    "x_bar = predict(x0, u_bar)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check the model prediction"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 432x288 with 4 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "plt.subplot(2, 2, 1)\n",
    "plt.plot(x_bar[0, :], x_bar[1, :])\n",
    "plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(np.degrees(x_bar[2, :]))\n",
    "plt.ylabel(\"theta(t) [deg]\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 3)\n",
    "plt.plot(np.degrees(x_bar[3, :]))\n",
    "plt.ylabel(\"psi(t) [deg]\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 4)\n",
    "plt.plot(x_bar[4, :])\n",
    "plt.ylabel(\"cte(t)\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "the results seems valid:\n",
    "* the cte is correct\n",
    "* theta error is correct"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Motion Prediction: using the state space model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 4.24 ms, sys: 0 ns, total: 4.24 ms\n",
      "Wall time: 4.74 ms\n"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 1  # m/s\n",
    "u_bar[1, :] = np.radians(-10)  # rad/s\n",
    "\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = 1\n",
    "x0[2] = np.radians(0)\n",
    "x0[3] = 0\n",
    "x0[4] = 1\n",
    "\n",
    "x_bar = np.zeros((N, T + 1))\n",
    "x_bar[:, 0] = x0\n",
    "\n",
    "for t in range(1, T + 1):\n",
    "    xt = x_bar[:, t - 1].reshape(5, 1)\n",
    "    ut = u_bar[:, t - 1].reshape(2, 1)\n",
    "\n",
    "    A, B, C = get_linear_model(xt, ut)\n",
    "\n",
    "    xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
    "\n",
    "    x_bar[:, t] = np.squeeze(xt_plus_one)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 432x288 with 4 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "plt.subplot(2, 2, 1)\n",
    "plt.plot(x_bar[0, :], x_bar[1, :])\n",
    "plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(x_bar[2, :])\n",
    "plt.ylabel(\"theta(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 3)\n",
    "plt.plot(x_bar[3, :])\n",
    "plt.ylabel(\"psi(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 4)\n",
    "plt.plot(x_bar[4, :])\n",
    "plt.ylabel(\"cte(t)\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The results are the same as expected"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "------------------\n",
    "\n",
    "the kinematics model predictits psi and cte for constant heading references. So, for non-constant paths appropriate functions have to be developed.\n",
    "\n",
    "-----------------"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_err(state, path):\n",
    "    \"\"\"\n",
    "    Finds psi and cte w.r.t. the closest waypoint.\n",
    "\n",
    "    :param state: array_like, state of the vehicle [x_pos, y_pos, theta]\n",
    "    :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]\n",
    "    :returns: (float,float)\n",
    "    \"\"\"\n",
    "\n",
    "    dx = state[0] - path[0, :]\n",
    "    dy = state[1] - path[1, :]\n",
    "    dist = np.sqrt(dx**2 + dy**2)\n",
    "    nn_idx = np.argmin(dist)\n",
    "\n",
    "    try:\n",
    "        v = [\n",
    "            path[0, nn_idx + 1] - path[0, nn_idx],\n",
    "            path[1, nn_idx + 1] - path[1, nn_idx],\n",
    "        ]\n",
    "        v /= np.linalg.norm(v)\n",
    "\n",
    "        d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
    "\n",
    "        if np.dot(d, v) > 0:\n",
    "            target_idx = nn_idx\n",
    "        else:\n",
    "            target_idx = nn_idx + 1\n",
    "\n",
    "    except IndexError as e:\n",
    "        target_idx = nn_idx\n",
    "\n",
    "    #     front_axle_vect = [np.cos(state[2] - np.pi / 2),\n",
    "    #                        np.sin(state[2] - np.pi / 2)]\n",
    "    path_ref_vect = [\n",
    "        np.cos(path[2, target_idx] + np.pi / 2),\n",
    "        np.sin(path[2, target_idx] + np.pi / 2),\n",
    "    ]\n",
    "\n",
    "    # heading error w.r.t path frame\n",
    "    psi = path[2, target_idx] - state[2]\n",
    "    # psi = -path[2,target_idx] + state[2]\n",
    "\n",
    "    # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor\n",
    "    # cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)\n",
    "    cte = np.dot([dx[target_idx], dy[target_idx]], path_ref_vect)\n",
    "\n",
    "    return target_idx, psi, cte\n",
    "\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
    "    \"\"\"\n",
    "    Interpolation range is computed to assure one point every fixed distance step [m].\n",
    "\n",
    "    :param start_xp: array_like, list of starting x coordinates\n",
    "    :param start_yp: array_like, list of starting y coordinates\n",
    "    :param step: float, interpolation distance [m] between consecutive waypoints\n",
    "    :returns: array_like, of shape (3,N)\n",
    "    \"\"\"\n",
    "\n",
    "    final_xp = []\n",
    "    final_yp = []\n",
    "    delta = step  # [m]\n",
    "\n",
    "    for idx in range(len(start_xp) - 1):\n",
    "        section_len = np.sum(\n",
    "            np.sqrt(\n",
    "                np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
    "                + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
    "            )\n",
    "        )\n",
    "\n",
    "        interp_range = np.linspace(0, 1, int(section_len / delta))\n",
    "\n",
    "        fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
    "        fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
    "\n",
    "        final_xp = np.append(final_xp, fx(interp_range))\n",
    "        final_yp = np.append(final_yp, fy(interp_range))\n",
    "\n",
    "    dx = np.append(0, np.diff(final_xp))\n",
    "    dy = np.append(0, np.diff(final_yp))\n",
    "    theta = np.arctan2(dy, dx)\n",
    "\n",
    "    return np.vstack((final_xp, final_yp, theta))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "test it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "track = compute_path_from_wp([0, 5], [0, 0])\n",
    "\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 1  # m/s\n",
    "u_bar[1, :] = np.radians(-10)  # rad/s\n",
    "\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = 1\n",
    "x0[2] = np.radians(0)\n",
    "_, psi, cte = calc_err(x0, track)\n",
    "x0[3] = psi\n",
    "x0[4] = cte\n",
    "\n",
    "x_bar = np.zeros((N, T + 1))\n",
    "x_bar[:, 0] = x0\n",
    "\n",
    "for t in range(1, T + 1):\n",
    "    xt = x_bar[:, t - 1].reshape(5, 1)\n",
    "    ut = u_bar[:, t - 1].reshape(2, 1)\n",
    "\n",
    "    A, B, C = get_linear_model(xt, ut)\n",
    "\n",
    "    xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
    "\n",
    "    _, psi, cte = calc_err(xt_plus_one, track)\n",
    "    xt_plus_one[3] = psi\n",
    "    xt_plus_one[4] = cte\n",
    "\n",
    "    x_bar[:, t] = xt_plus_one"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 432x288 with 4 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "plt.subplot(2, 2, 1)\n",
    "plt.plot(x_bar[0, :], x_bar[1, :])\n",
    "plt.plot(track[0, :], track[1, :], \"b-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(x_bar[2, :])\n",
    "plt.ylabel(\"theta(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 3)\n",
    "plt.plot(x_bar[3, :])\n",
    "plt.ylabel(\"psi(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 4)\n",
    "plt.plot(x_bar[4, :])\n",
    "plt.ylabel(\"cte(t)\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "track = compute_path_from_wp([0, 2, 4, 5, 10], [0, 0, 3, 1, 1])\n",
    "\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 1  # m/s\n",
    "u_bar[1, :] = np.radians(10)  # rad/s\n",
    "\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 2\n",
    "x0[1] = 2\n",
    "x0[2] = np.radians(0)\n",
    "_, psi, cte = calc_err(x0, track)\n",
    "x0[3] = psi\n",
    "x0[4] = cte\n",
    "\n",
    "x_bar = np.zeros((N, T + 1))\n",
    "x_bar[:, 0] = x0\n",
    "\n",
    "for t in range(1, T + 1):\n",
    "    xt = x_bar[:, t - 1].reshape(5, 1)\n",
    "    ut = u_bar[:, t - 1].reshape(2, 1)\n",
    "\n",
    "    A, B, C = get_linear_model(xt, ut)\n",
    "\n",
    "    xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
    "\n",
    "    _, psi, cte = calc_err(xt_plus_one, track)\n",
    "    xt_plus_one[3] = psi\n",
    "    xt_plus_one[4] = cte\n",
    "\n",
    "    x_bar[:, t] = xt_plus_one"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 432x288 with 4 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "plt.subplot(2, 2, 1)\n",
    "plt.plot(x_bar[0, :], x_bar[1, :])\n",
    "plt.plot(track[0, :], track[1, :], \"b-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(x_bar[2, :])\n",
    "plt.ylabel(\"theta(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 3)\n",
    "plt.plot(x_bar[3, :])\n",
    "plt.ylabel(\"psi(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 4)\n",
    "plt.plot(x_bar[4, :])\n",
    "plt.ylabel(\"cte(t)\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### MPC Problem formulation\n",
    "\n",
    "**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n",
    "\n",
    "The controller generates a control signal over a fixed lenght T (Horizon) at each time step."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![mpc](img/mpc_block_diagram.png)\n",
    "\n",
    "![mpc](img/mpc_t.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Linear MPC Formulation\n",
    "\n",
    "Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n",
    "\n",
    "$x_{t+1} = Ax_t + Bu_t$\n",
    "\n",
    "The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n",
    "\n",
    "$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n",
    "\n",
    "The objective function used minimize (drive the state to 0) is:\n",
    "\n",
    "$\n",
    "\\begin{equation}\n",
    "\\begin{aligned}\n",
    "\\min_{} \\quad &  \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n",
    "\\textrm{s.t.} \\quad &  x(0) = x0\\\\\n",
    "  & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad  \\textrm{for} \\quad t<j<t+T-1    \\\\\n",
    "\\end{aligned}\n",
    "\\end{equation}\n",
    "$\n",
    "\n",
    "Other linear constrains may be applied,for instance on the control variable:\n",
    "\n",
    "$ U_{MIN} < u_{j|t} < U_{MAX}    \\quad  \\textrm{for} \\quad t<j<t+T-1 $\n",
    "\n",
    "The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
    "\n",
    "Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
    "\n",
    "$\n",
    "\\begin{equation}\n",
    "\\begin{aligned}\n",
    "\\min_{} \\quad &  \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
    "\\end{aligned}\n",
    "\\end{equation}\n",
    "$\n",
    "\n",
    "where the error w.r.t desired state is accounted for:\n",
    "\n",
    "$ \\delta x = x_{j,t,ref} - x_{j,t} $\n",
    "\n",
    "#### Non-Linear MPC Formulation\n",
    "\n",
    "In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
    "\n",
    "$\n",
    "\\begin{equation}\n",
    "\\begin{aligned}\n",
    "\\min_{} \\quad &  \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
    "\\textrm{s.t.} \\quad &  x(0) = x0\\\\\n",
    "  & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad  \\textrm{for} \\quad t<j<t+T-1    \\\\\n",
    "\\end{aligned}\n",
    "\\end{equation}\n",
    "$\n",
    "\n",
    "Other nonlinear constrains may be applied:\n",
    "\n",
    "$ g(x_{j|t},{j|t})<0 \\quad  \\textrm{for} \\quad t<j<t+T-1 $"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 109 ms, sys: 81 µs, total: 109 ms\n",
      "Wall time: 108 ms\n"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "\n",
    "track = compute_path_from_wp([0, 10], [0, 0])\n",
    "\n",
    "MAX_SPEED = 2.5\n",
    "MIN_SPEED = 0.5\n",
    "MAX_STEER_SPEED = 1.57 / 2\n",
    "\n",
    "# starting guess\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 0.5 * (MAX_SPEED + MIN_SPEED)\n",
    "u_bar[1, :] = 0.01\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = 1\n",
    "x0[2] = np.radians(-0)\n",
    "_, psi, cte = calc_err(x0, track)\n",
    "x0[3] = psi\n",
    "x0[4] = cte\n",
    "\n",
    "# Linearized Model Prediction\n",
    "x_bar = np.zeros((N, T + 1))\n",
    "x_bar[:, 0] = x0\n",
    "\n",
    "for t in range(1, T + 1):\n",
    "    xt = x_bar[:, t].reshape(5, 1)\n",
    "    ut = u_bar[:, t - 1].reshape(2, 1)\n",
    "\n",
    "    A, B, C = get_linear_model(xt, ut)\n",
    "\n",
    "    xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
    "\n",
    "    _, psi, cte = calc_err(xt_plus_one, track)\n",
    "    xt_plus_one[3] = psi\n",
    "    xt_plus_one[4] = cte\n",
    "    x_bar[:, t] = xt_plus_one\n",
    "\n",
    "\n",
    "# CVXPY Linear MPC problem statement\n",
    "cost = 0\n",
    "constr = []\n",
    "\n",
    "for t in range(T):\n",
    "\n",
    "    # Tracking\n",
    "    if t > 0:\n",
    "        idx, _, _ = calc_err(x_bar[:, t], track)\n",
    "        delta_x = track[:, idx] - x[0:3, t]\n",
    "        cost += cp.quad_form(delta_x, 10 * np.eye(3))\n",
    "\n",
    "    # Tracking last time step\n",
    "    if t == T:\n",
    "        idx, _, _ = calc_err(x_bar[:, t], track)\n",
    "        delta_x = track[:, idx] - x[0:3, t]\n",
    "        cost += cp.quad_form(delta_x, 100 * np.eye(3))\n",
    "\n",
    "    # Actuation rate of change\n",
    "    if t < (T - 1):\n",
    "        cost += cp.quad_form(u[:, t + 1] - u[:, t], 25 * np.eye(M))\n",
    "\n",
    "    # constrains\n",
    "    A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
    "    constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
    "\n",
    "# sums problem objectives and concatenates constraints.\n",
    "constr += [x[:, 0] == x0]\n",
    "constr += [u[0, :] <= MAX_SPEED]\n",
    "constr += [u[0, :] >= MIN_SPEED]\n",
    "constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
    "\n",
    "\n",
    "prob = cp.Problem(cp.Minimize(cost), constr)\n",
    "solution = prob.solve(solver=cp.OSQP, verbose=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Print Results:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "No handles with labels found to put in legend.\n"
     ]
    },
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 1080x720 with 4 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "x_mpc = np.array(x.value[0, :]).flatten()\n",
    "y_mpc = np.array(x.value[1, :]).flatten()\n",
    "theta_mpc = np.array(x.value[2, :]).flatten()\n",
    "psi_mpc = np.array(x.value[3, :]).flatten()\n",
    "cte_mpc = np.array(x.value[4, :]).flatten()\n",
    "v_mpc = np.array(u.value[0, :]).flatten()\n",
    "w_mpc = np.array(u.value[1, :]).flatten()\n",
    "\n",
    "# simulate robot state trajectory for optimized U\n",
    "x_traj = predict(x0, np.vstack((v_mpc, w_mpc)))\n",
    "plt.figure(figsize=(15, 10))\n",
    "# plot trajectory\n",
    "plt.subplot(2, 2, 1)\n",
    "plt.plot(track[0, :], track[1, :], \"b*\")\n",
    "plt.plot(x_traj[0, :], x_traj[1, :])\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "# plot v(t)\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(v_mpc)\n",
    "plt.ylabel(\"v(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "# plot w(t)\n",
    "plt.subplot(2, 2, 3)\n",
    "plt.plot(w_mpc)\n",
    "plt.ylabel(\"w(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "plt.subplot(2, 2, 4)\n",
    "plt.plot(cte_mpc)\n",
    "plt.ylabel(\"cte(t)\")\n",
    "plt.legend(loc=\"best\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "full track demo"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "<ipython-input-9-004252095cfc>:18: RuntimeWarning: invalid value encountered in true_divide\n",
      "  v /= np.linalg.norm(v)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.1228s Max: 0.2149s Min: 0.1088s\n"
     ]
    }
   ],
   "source": [
    "track = compute_path_from_wp(\n",
    "    [0, 5, 7.5, 10, 12, 13, 13, 10], [0, 0, 2.5, 2.5, 0, 0, 5, 10]\n",
    ")\n",
    "\n",
    "sim_duration = 100\n",
    "opt_time = []\n",
    "\n",
    "x_sim = np.zeros((N, sim_duration))\n",
    "u_sim = np.zeros((M, sim_duration - 1))\n",
    "\n",
    "MAX_SPEED = 1.25\n",
    "MIN_SPEED = 0.75\n",
    "MAX_STEER_SPEED = 1.57 / 2\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = -0.5\n",
    "x0[2] = np.radians(-0)\n",
    "_, psi, cte = calc_err(x0, track)\n",
    "x0[3] = psi\n",
    "x0[4] = cte\n",
    "\n",
    "x_sim[:, 0] = x0\n",
    "\n",
    "# starting guess\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 0.5 * (MAX_SPEED + MIN_SPEED)\n",
    "u_bar[1, :] = 0.01\n",
    "\n",
    "for sim_time in range(sim_duration - 1):\n",
    "\n",
    "    iter_start = time.time()\n",
    "\n",
    "    # Prediction\n",
    "    x_bar = np.zeros((N, T + 1))\n",
    "    x_bar[:, 0] = x_sim[:, sim_time]\n",
    "\n",
    "    for t in range(1, T + 1):\n",
    "        xt = x_bar[:, t - 1].reshape(5, 1)\n",
    "        ut = u_bar[:, t - 1].reshape(2, 1)\n",
    "\n",
    "        A, B, C = get_linear_model(xt, ut)\n",
    "\n",
    "        xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
    "\n",
    "        _, psi, cte = calc_err(xt_plus_one, track)\n",
    "        xt_plus_one[3] = psi\n",
    "        xt_plus_one[4] = cte\n",
    "\n",
    "        x_bar[:, t] = xt_plus_one\n",
    "\n",
    "    # CVXPY Linear MPC problem statement\n",
    "    cost = 0\n",
    "    constr = []\n",
    "\n",
    "    for t in range(T):\n",
    "\n",
    "        # Tracking\n",
    "        if t > 0:\n",
    "            idx, _, _ = calc_err(x_bar[:, t], track)\n",
    "            delta_x = track[:, idx] - x[0:3, t]\n",
    "            cost += cp.quad_form(delta_x, 10 * np.eye(3))\n",
    "\n",
    "        # Tracking last time step\n",
    "        if t == T:\n",
    "            idx, _, _ = calc_err(x_bar[:, t], track)\n",
    "            delta_x = track[:, idx] - x[0:3, t]\n",
    "            cost += cp.quad_form(delta_x, 100 * np.eye(3))\n",
    "\n",
    "        # Actuation rate of change\n",
    "        if t < (T - 1):\n",
    "            cost += cp.quad_form(u[:, t + 1] - u[:, t], 25 * np.eye(M))\n",
    "\n",
    "        # Actuation effort\n",
    "        cost += cp.quad_form(u[:, t], 1 * np.eye(M))\n",
    "\n",
    "        # Constrains\n",
    "        A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
    "        constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
    "\n",
    "    # sums problem objectives and concatenates constraints.\n",
    "    constr += [x[:, 0] == x_sim[:, sim_time]]  # starting condition\n",
    "    constr += [u[0, :] <= MAX_SPEED]\n",
    "    constr += [u[0, :] >= MIN_SPEED]\n",
    "    constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
    "\n",
    "    # Solve\n",
    "    prob = cp.Problem(cp.Minimize(cost), constr)\n",
    "    solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
    "\n",
    "    # retrieved optimized U and assign to u_bar to linearize in next step\n",
    "    u_bar = np.vstack(\n",
    "        (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
    "    )\n",
    "\n",
    "    u_sim[:, sim_time] = u_bar[:, 0]\n",
    "\n",
    "    # Measure elpased time to get results from cvxpy\n",
    "    opt_time.append(time.time() - iter_start)\n",
    "\n",
    "    # move simulation to t+1\n",
    "    tspan = [0, dt]\n",
    "    x_sim[:, sim_time + 1] = odeint(\n",
    "        kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
    "    )[1]\n",
    "\n",
    "print(\n",
    "    \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
    "        np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
    "    )\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 1080x720 with 3 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "grid = plt.GridSpec(2, 3)\n",
    "\n",
    "plt.figure(figsize=(15, 10))\n",
    "\n",
    "plt.subplot(grid[0:2, 0:2])\n",
    "plt.plot(track[0, :], track[1, :], \"b+\")\n",
    "plt.plot(x_sim[0, :], x_sim[1, :])\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(grid[0, 2])\n",
    "plt.plot(u_sim[0, :])\n",
    "plt.ylabel(\"v(t) [m/s]\")\n",
    "\n",
    "plt.subplot(grid[1, 2])\n",
    "plt.plot(np.degrees(u_sim[1, :]))\n",
    "plt.ylabel(\"w(t) [deg/s]\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "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.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}