{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 138,
   "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",
    "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",
    "\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",
    "\n",
    "Discretize with forward Euler Integration for time step dt:\n",
    "\n",
    "${x_{t+1}} = x_{t} + f(x,u)*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",
    "\n",
    "----------------------\n",
    "\n",
    "The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n",
    "\n",
    "$ f(x,u) \\approx f(\\bar{x},\\bar{u}) +  \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
    "\n",
    "This can be rewritten usibg the State Space model form Ax+Bu :\n",
    "\n",
    "$ f(\\bar{x},\\bar{u}) +  A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
    "\n",
    "Where:\n",
    "\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} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "=\n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "0 & 0 & -v\\sin{\\theta} \\\\\n",
    "0 & 0 & v\\cos{\\theta} \\\\\n",
    "0 & 0 & 0\\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "$\n",
    "\n",
    "and\n",
    "\n",
    "$\n",
    "B = \n",
    "\\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} & 0 \\\\\n",
    "\\sin{\\theta} & 0 \\\\\n",
    "0 & 1\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "$\n",
    "\n",
    "are the *Jacobians*.\n",
    "\n",
    "\n",
    "\n",
    "So the discretized model is given by:\n",
    "\n",
    "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) +  A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(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 LTI-equivalent kinematics model is:\n",
    "\n",
    "$ x_{t+1} = A'x_t + B' u_t + C' $\n",
    "\n",
    "with:\n",
    "\n",
    "$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n",
    "\n",
    "$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
    "\n",
    "$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $\n",
    "\n",
    "**Errors** are:\n",
    "* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n",
    "* $cte$ crosstrack error = lateral distance of the robot from the track w.r.t robot frame, cte is a function of the track and x\n",
    "\n",
    "described by:"
   ]
  },
  {
   "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": 139,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Control problem statement.\n",
    "\n",
    "N = 3 #number of state variables\n",
    "M = 2 #number of control variables\n",
    "T = 20 #Prediction Horizon\n",
    "dt = 0.25 #discretization step"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 140,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_linear_model(x_bar,u_bar):\n",
    "    \"\"\"\n",
    "    \"\"\"\n",
    "    \n",
    "    x = x_bar[0]\n",
    "    y = x_bar[1]\n",
    "    theta = x_bar[2]\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_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_lin=dt*B\n",
    "    \n",
    "    f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).reshape(N,1)\n",
    "    C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
    "    \n",
    "    return A_lin,B_lin,C_lin"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Motion Prediction: using scipy intergration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 141,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define process model\n",
    "# This uses the continuous model \n",
    "def kinematics_model(x,t,u):\n",
    "    \"\"\"\n",
    "    \"\"\"\n",
    "\n",
    "    dxdt = u[0]*np.cos(x[2])\n",
    "    dydt = u[0]*np.sin(x[2])\n",
    "    dthetadt = u[1]\n",
    "\n",
    "    dqdt = [dxdt,\n",
    "            dydt,\n",
    "            dthetadt]\n",
    "\n",
    "    return dqdt\n",
    "\n",
    "def predict(x0,u):\n",
    "    \"\"\"\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,\n",
    "                         x0,\n",
    "                         tspan,\n",
    "                         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": 142,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 6.71 ms, sys: 0 ns, total: 6.71 ms\n",
      "Wall time: 5.35 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",
    "\n",
    "x_bar=predict(x0,u_bar)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check the model prediction"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 143,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 2 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",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Motion Prediction: using the state space model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 144,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 2.28 ms, sys: 0 ns, total: 2.28 ms\n",
      "Wall time: 1.55 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",
    "\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(N,1)\n",
    "    ut=u_bar[:,t-1].reshape(M,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": 145,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 2 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)')\n",
    "#plt.xlabel('time')\n",
    "\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The results are the same as expected, so the linearized model is equivalent 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": "markdown",
   "metadata": {},
   "source": [
    "## PRELIMINARIES"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 146,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\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(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
    "\n",
    "        interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\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",
    "    return np.vstack((final_xp,final_yp))\n",
    "\n",
    "def get_nn_idx(state,path):\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 = [path[0,nn_idx+1] - path[0,nn_idx],\n",
    "             path[1,nn_idx+1] - path[1,nn_idx]]   \n",
    "        v /= np.linalg.norm(v)\n",
    "\n",
    "        d = [path[0,nn_idx] - state[0],\n",
    "             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",
    "    return target_idx\n",
    "\n",
    "def road_curve(state,track):\n",
    "    \n",
    "    #given vehicle pos find lookahead waypoints\n",
    "    nn_idx=get_nn_idx(state,track)-1\n",
    "    LOOKAHED=6\n",
    "    lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
    "\n",
    "    #trasform lookahead waypoints to vehicle ref frame\n",
    "    dx = lk_wp[0,:] - state[0]\n",
    "    dy = lk_wp[1,:] - state[1]\n",
    "\n",
    "    wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
    "                                   dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
    "\n",
    "    #fit poly\n",
    "    return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n",
    "\n",
    "def f(x,coeff):\n",
    "    return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n",
    "\n",
    "# def f(x,coeff):\n",
    "#     return  round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)\n",
    "\n",
    "def df(x,coeff):\n",
    "    return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
    "# def df(x,coeff):\n",
    "#     return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)"
   ]
  },
  {
   "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": 147,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0. 0. 0.]\n"
     ]
    }
   ],
   "source": [
    "track = compute_path_from_wp([0,5,10],[0,0,2])\n",
    "\n",
    "# Constrains\n",
    "MAX_SPEED = 2.5\n",
    "MIN_SPEED = 0.5\n",
    "MAX_STEER_SPEED = 1.57/2\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = 1\n",
    "x0[2] = np.radians(-10)\n",
    "\n",
    "#X0 is needed ONLY to initialize road curve\n",
    "K=road_curve(x0,track)\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.0\n",
    "\n",
    "# Linearized Model for Prediction##############\n",
    "x_bar=np.zeros((N,T+1))\n",
    "\n",
    "#starting state is 0,0,0,psi,cte\n",
    "x_bar[0,0]=0\n",
    "x_bar[1,0]=0\n",
    "x_bar[2,0]=0\n",
    "\n",
    "\n",
    "print(x_bar[:,0])\n",
    "\n",
    "for t in range (1,T+1):\n",
    "    xt=x_bar[:,t-1].reshape(N,1)\n",
    "    ut=u_bar[:,t-1].reshape(M,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",
    "    x_bar[:,t]= xt_plus_one\n",
    "#################################################\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 148,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-----------------------------------------------------------------\n",
      "           OSQP v0.6.0  -  Operator Splitting QP Solver\n",
      "              (c) Bartolomeo Stellato,  Goran Banjac\n",
      "        University of Oxford  -  Stanford University 2019\n",
      "-----------------------------------------------------------------\n",
      "problem:  variables n = 241, constraints m = 281\n",
      "          nnz(P) + nnz(A) = 715\n",
      "settings: linear system solver = qdldl,\n",
      "          eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
      "          eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,\n",
      "          rho = 1.00e-01 (adaptive),\n",
      "          sigma = 1.00e-06, alpha = 1.60, max_iter = 10000\n",
      "          check_termination: on (interval 25),\n",
      "          scaling: on, scaled_termination: off\n",
      "          warm start: on, polish: on, time_limit: off\n",
      "\n",
      "iter   objective    pri res    dua res    rho        time\n",
      "   1   0.0000e+00   1.02e+00   1.02e+02   1.00e-01   2.28e-04s\n",
      " 125   1.6631e+03   2.07e-05   3.42e-04   7.12e+01   9.98e-04s\n",
      "\n",
      "status:               solved\n",
      "solution polish:      unsuccessful\n",
      "number of iterations: 125\n",
      "optimal objective:    1663.1222\n",
      "run time:             1.17e-03s\n",
      "optimal rho estimate: 7.91e+01\n",
      "\n",
      "CPU times: user 196 ms, sys: 90 µs, total: 196 ms\n",
      "Wall time: 194 ms\n"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "\n",
    "x = cp.Variable((N, T+1))\n",
    "u = cp.Variable((M, T))\n",
    "\n",
    "#CVXPY Linear MPC problem statement\n",
    "cost = 0\n",
    "constr = []\n",
    "\n",
    "for t in range(T):\n",
    "    \n",
    "    # Cost function\n",
    "    #cost += 10*cp.sum_squares( x[3, t]) # psi\n",
    "    #cost += 500*cp.sum_squares( x[4, t]) # cte\n",
    "    \n",
    "    cost += 10*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
    "    cost += 500*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
    "    # Actuation effort\n",
    "    cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
    "    \n",
    "    # Actuation rate of change\n",
    "    if t < (T - 1):\n",
    "        cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n",
    "    \n",
    "    # KINEMATICS 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_bar[:,0]] #<--watch out the start condition\n",
    "constr += [u[0, :] <= MAX_SPEED]\n",
    "constr += [u[0, :] >= MIN_SPEED]\n",
    "constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
    "\n",
    "prob = cp.Problem(cp.Minimize(cost), constr)\n",
    "solution = prob.solve(solver=cp.OSQP, verbose=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 149,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 2 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",
    "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",
    "\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, 3)\n",
    "# plt.plot(psi_mpc)\n",
    "# plt.ylabel('psi(t)')\n",
    "\n",
    "# plt.subplot(2, 2, 4)\n",
    "# plt.plot(cte_mpc)\n",
    "# plt.ylabel('cte(t)')\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "full track demo"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 160,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/marcello/.local/lib/python3.6/site-packages/ipykernel_launcher.py:29: RuntimeWarning: invalid value encountered in true_divide\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.1705s Max: 0.2163s Min: 0.1577s\n"
     ]
    }
   ],
   "source": [
    "track = compute_path_from_wp([0,3,4,6,10,13],\n",
    "                             [0,0,2,4,3,3],0.25)\n",
    "\n",
    "# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
    "#                              [0,0,2.5,2.5,0,0,5,10],0.5)\n",
    "\n",
    "sim_duration = 50 #time steps\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\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = -0.25\n",
    "x0[2] = np.radians(-0)\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.00\n",
    "\n",
    "for sim_time in range(sim_duration-1):\n",
    "    \n",
    "    iter_start=time.time()\n",
    "    \n",
    "    K=road_curve(x_sim[:,sim_time],track)\n",
    "    \n",
    "    # dynamics starting state w.r.t vehicle frame\n",
    "    x_bar=np.zeros((N,T+1))\n",
    "    \n",
    "    #prediction for linearization of costrains\n",
    "    for t in range (1,T+1):\n",
    "        xt=x_bar[:,t-1].reshape(N,1)\n",
    "        ut=u_bar[:,t-1].reshape(M,1)\n",
    "        A,B,C=get_linear_model(xt,ut)\n",
    "        xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
    "        x_bar[:,t]= xt_plus_one\n",
    "    \n",
    "    #CVXPY Linear MPC problem statement\n",
    "    cost = 0\n",
    "    constr = []\n",
    "    x = cp.Variable((N, T+1))\n",
    "    u = cp.Variable((M, T))\n",
    "    \n",
    "    for t in range(T):\n",
    "\n",
    "        #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
    "        cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n",
    "        cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
    "\n",
    "        # Actuation rate of change\n",
    "        if t < (T - 1):\n",
    "            cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n",
    "        \n",
    "        # Actuation effort\n",
    "        cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
    "        \n",
    "        # Kinrmatics Constrains (Linearized model)\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_bar[:,0]] #<--watch out the start 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((np.array(u.value[0, :]).flatten(),\n",
    "                    (np.array(u.value[1, :]).flatten())))\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(kinematics_model,\n",
    "                                 x_sim[:,sim_time],\n",
    "                                 tspan,\n",
    "                                 args=(u_bar[:,0],))[1]\n",
    "    \n",
    "print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
    "                                                                                np.max(opt_time),\n",
    "                                                                                np.min(opt_time)))   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 161,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}