{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2 MPC\n",
    "This notebook contains the CVXPY implementation of a MPC\n",
    "This is the simplest one"
   ]
  },
  {
   "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} $"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Problem Formulation: Study case\n",
    "\n",
    "In this case, the objective function to minimize is given by:\n",
    "\n",
    "https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf\n",
    "\n",
    "$\n",
    "\\begin{equation}\n",
    "\\begin{aligned}\n",
    "\\min_{} \\quad &  \\sum^{t+T-1}_{j=t} (x_{j} - x_{j,ref})^TQ(x_{j} - x_{j,ref}) + \\sum^{t+T-1}_{j=t+1} u^T_{j}Ru_{j} + (u_{j} - u_{j-1})^TP(u_{j} - u_{j-1}) \\\\\n",
    "\\textrm{s.t.} \\quad &  x(0) = x0\\\\\n",
    "  & x_{j+1} = Ax_{j}+Bu_{j} \\quad  \\textrm{for} \\quad t<j<t+T-1  \\\\\n",
    "  & u_{MIN} < u_{j} < u_{MAX} \\quad  \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
    "  & \\dot{u}_{MIN} < \\frac{(u_{j} - u_{j-1})}{ts} < \\dot{u}_{MAX} \\quad  \\textrm{for} \\quad t+1<j<t+T-1 \\\\\n",
    "\\end{aligned}\n",
    "\\end{equation}\n",
    "$\n",
    "\n",
    "\n",
    "Where R,P,Q are the cost matrices used to tune the response.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## PRELIMINARIES\n",
    "\n",
    "* linearized system dynamics\n",
    "* function to represent the track\n",
    "* function to represent the **reference trajectory** to track"
   ]
  },
  {
   "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",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "Control problem statement.\n",
    "\"\"\"\n",
    "\n",
    "N = 4 #number of state variables\n",
    "M = 2 #number of control variables\n",
    "T = 20 #Prediction Horizon\n",
    "DT = 0.2 #discretization step\n",
    "\n",
    "def get_linear_model(x_bar,u_bar):\n",
    "    \"\"\"\n",
    "    Computes the LTI approximated state space model x' = Ax + Bu + C\n",
    "    \"\"\"\n",
    "    \n",
    "    L=0.3 #vehicle wheelbase\n",
    "    \n",
    "    x = x_bar[0]\n",
    "    y = x_bar[1]\n",
    "    v = x_bar[2]\n",
    "    theta = x_bar[3]\n",
    "    \n",
    "    a = u_bar[0]\n",
    "    delta = u_bar[1]\n",
    "    \n",
    "    A = np.zeros((N,N))\n",
    "    A[0,2]=np.cos(theta)\n",
    "    A[0,3]=-v*np.sin(theta)\n",
    "    A[1,2]=np.sin(theta)\n",
    "    A[1,3]=v*np.cos(theta)\n",
    "    A[3,2]=v*np.tan(delta)/L\n",
    "    A_lin=np.eye(N)+DT*A\n",
    "    \n",
    "    B = np.zeros((N,M))\n",
    "    B[2,0]=1\n",
    "    B[3,1]=v/(L*np.cos(delta)**2)\n",
    "    B_lin=DT*B\n",
    "    \n",
    "    f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).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 np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n",
    "\n",
    "\"\"\"\n",
    "the ODE is used to update the simulation given the mpc results\n",
    "I use this insted of using the LTI twice\n",
    "\"\"\"\n",
    "def kinematics_model(x,t,u):\n",
    "    \"\"\"\n",
    "    Returns the set of ODE of the vehicle model.\n",
    "    \"\"\"\n",
    "    \n",
    "    L=0.3 #vehicle wheelbase\n",
    "    dxdt = x[2]*np.cos(x[3])\n",
    "    dydt = x[2]*np.sin(x[3])\n",
    "    dvdt = u[0]\n",
    "    dthetadt = x[2]*np.tan(u[1])/L\n",
    "\n",
    "    dqdt = [dxdt,\n",
    "            dydt,\n",
    "            dvdt,\n",
    "            dthetadt]\n",
    "\n",
    "    return dqdt\n",
    "\n",
    "def predict(x0,u):\n",
    "    \"\"\"\n",
    "    \"\"\"\n",
    "    \n",
    "    x_ = np.zeros((N,T+1))\n",
    "    \n",
    "    x_[:,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_[:,t]=x_next[1]\n",
    "        \n",
    "    return x_\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
    "    \"\"\"\n",
    "    Computes a reference path given a set of waypoints\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(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",
    "    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))\n",
    "\n",
    "\n",
    "def get_nn_idx(state,path):\n",
    "    \"\"\"\n",
    "    Computes the index of the waypoint closest to vehicle\n",
    "    \"\"\"\n",
    "\n",
    "    dx = state[0]-path[0,:]\n",
    "    dy = state[1]-path[1,:]\n",
    "    dist = np.hypot(dx,dy)\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 get_ref_trajectory(state, path, target_v):\n",
    "    \"\"\"\n",
    "    Adapted from pythonrobotics\n",
    "    \"\"\"\n",
    "    xref = np.zeros((N, T + 1))\n",
    "    dref = np.zeros((1, T + 1))\n",
    "    \n",
    "    #sp = np.ones((1,T +1))*target_v #speed profile\n",
    "    \n",
    "    ncourse = path.shape[1]\n",
    "\n",
    "    ind = get_nn_idx(state, path)\n",
    "\n",
    "    xref[0, 0] = path[0,ind] #X\n",
    "    xref[1, 0] = path[1,ind] #Y\n",
    "    xref[2, 0] = target_v    #sp[ind] #V\n",
    "    xref[3, 0] = path[2,ind] #Theta\n",
    "    dref[0, 0] = 0.0         # steer operational point should be 0\n",
    "    \n",
    "    dl = 0.05 # Waypoints spacing [m]\n",
    "    travel = 0.0\n",
    "\n",
    "    for i in range(T + 1):\n",
    "        travel += abs(target_v) * DT #current V or target V?\n",
    "        dind = int(round(travel / dl))\n",
    "\n",
    "        if (ind + dind) < ncourse:\n",
    "            xref[0, i] = path[0,ind + dind]\n",
    "            xref[1, i] = path[1,ind + dind]\n",
    "            xref[2, i] = target_v #sp[ind + dind]\n",
    "            xref[3, i] = path[2,ind + dind]\n",
    "            dref[0, i] = 0.0\n",
    "        else:\n",
    "            xref[0, i] = path[0,ncourse - 1]\n",
    "            xref[1, i] = path[1,ncourse - 1]\n",
    "            xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
    "            xref[3, i] = path[2,ncourse - 1]\n",
    "            dref[0, i] = 0.0\n",
    "\n",
    "    return xref, dref"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## MPC \n",
    "\n",
    "test single iteration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "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 = 326, constraints m = 408\n",
      "          nnz(P) + nnz(A) = 1063\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   4.27e+00   4.67e+02   1.00e-01   4.07e-04s\n",
      " 175   1.6965e+02   2.63e-05   3.49e-05   7.14e+00   1.86e-03s\n",
      "\n",
      "status:               solved\n",
      "solution polish:      unsuccessful\n",
      "number of iterations: 175\n",
      "optimal objective:    169.6454\n",
      "run time:             2.50e-03s\n",
      "optimal rho estimate: 6.34e+00\n",
      "\n",
      "CPU times: user 122 ms, sys: 75 µs, total: 122 ms\n",
      "Wall time: 119 ms\n"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "\n",
    "MAX_SPEED = 1.5 #m/s\n",
    "MAX_STEER = np.radians(30) #rad\n",
    "MAX_ACC = 1.0\n",
    "REF_VEL=1.0\n",
    "\n",
    "track = compute_path_from_wp([0,3,6],\n",
    "                             [0,0,0],0.05)\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0               #x\n",
    "x0[1] = -0.25           #y\n",
    "x0[2] = 0.0             #v\n",
    "x0[3] = np.radians(-0)  #yaw\n",
    "                 \n",
    "#starting guess\n",
    "u_bar = np.zeros((M,T))\n",
    "u_bar[0,:] = MAX_ACC/2 #a\n",
    "u_bar[1,:] = 0.1       #delta\n",
    "\n",
    "# dynamics starting state w.r.t world frame\n",
    "x_bar = np.zeros((N,T+1))\n",
    "x_bar[:,0] = x0\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",
    "x = cp.Variable((N, T+1))\n",
    "u = cp.Variable((M, T))\n",
    "cost = 0\n",
    "constr = []\n",
    "\n",
    "# Cost Matrices\n",
    "Q = np.diag([10,10,10,10]) #state error cost\n",
    "Qf = np.diag([10,10,10,10]) #state  final error cost\n",
    "R = np.diag([10,10])       #input cost\n",
    "R_ = np.diag([10,10])      #input rate of change cost\n",
    "\n",
    "#Get Reference_traj\n",
    "x_ref, d_ref = get_ref_trajectory(x_bar[:,0], track, REF_VEL)\n",
    "\n",
    "#Prediction Horizon\n",
    "for t in range(T):\n",
    "    \n",
    "    # Tracking Error\n",
    "    cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
    "\n",
    "    # Actuation effort\n",
    "    cost += cp.quad_form(u[:,t], R)\n",
    "    \n",
    "    # Actuation rate of change\n",
    "    if t < (T - 1):\n",
    "        cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\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",
    "#Final Point tracking\n",
    "cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
    "\n",
    "# sums problem objectives and concatenates constraints.\n",
    "constr += [x[:,0] == x_bar[:,0]]           #starting condition\n",
    "constr += [x[2,:] <= MAX_SPEED]           #max speed\n",
    "constr += [x[2,:] >= 0.0]                 #min_speed (not really needed)\n",
    "constr += [cp.abs(u[0,:]) <= MAX_ACC]     #max acc\n",
    "constr += [cp.abs(u[1,:]) <= MAX_STEER]   #max steer\n",
    "# for t in range(T):\n",
    "#     if t < (T - 1):\n",
    "#         constr += [cp.abs(u[0,t] - u[0,t-1])/DT <= MAX_ACC]     #max acc\n",
    "#         constr += [cp.abs(u[1,t] - u[1,t-1])/DT <= MAX_STEER]   #max steer\n",
    "\n",
    "prob = cp.Problem(cp.Minimize(cost), constr)\n",
    "solution = prob.solve(solver=cp.OSQP, verbose=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 27148 characters]\n",
      "text/plain": [
       "<Figure size 432x288 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",
    "v_mpc=np.array(x.value[2, :]).flatten()\n",
    "theta_mpc=np.array(x.value[3, :]).flatten()\n",
    "a_mpc=np.array(u.value[0, :]).flatten()\n",
    "delta_mpc=np.array(u.value[1, :]).flatten()\n",
    "\n",
    "#simulate robot state trajectory for optimized U\n",
    "x_traj=predict(x0, np.vstack((a_mpc,delta_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_ref[0,:],x_ref[1,:],\"g+\")\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, 3)\n",
    "plt.plot(a_mpc)\n",
    "plt.ylabel('a_in(t)')\n",
    "#plt.xlabel('time')\n",
    "\n",
    "\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(theta_mpc)\n",
    "plt.ylabel('theta(t)')\n",
    "\n",
    "plt.subplot(2, 2, 4)\n",
    "plt.plot(delta_mpc)\n",
    "plt.ylabel('d_in(t)')\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## full track demo"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/marcello/.conda/envs/jupyter/lib/python3.8/site-packages/cvxpy/problems/problem.py:1054: UserWarning: Solution may be inaccurate. Try another solver, adjusting the solver settings, or solve with verbose=True for more information.\n",
      "  warnings.warn(\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.1677s Max: 0.2731s Min: 0.1438s\n"
     ]
    }
   ],
   "source": [
    "track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
    "                             [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
    "\n",
    "# track = compute_path_from_wp([0,10,10,0],\n",
    "#                              [0,0,1,1],0.05)\n",
    "\n",
    "sim_duration = 200 #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.5 #m/s\n",
    "MAX_ACC = 1.0 #m/ss\n",
    "MAX_D_ACC = 1.0 #m/sss\n",
    "MAX_STEER = np.radians(30) #rad\n",
    "MAX_D_STEER = np.radians(30) #rad/s\n",
    "\n",
    "REF_VEL = 1.0 #m/s\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0               #x\n",
    "x0[1] = -0.25           #y\n",
    "x0[2] = 0.0             #v\n",
    "x0[3] = np.radians(-0)  #yaw\n",
    "                 \n",
    "#starting guess\n",
    "u_bar = np.zeros((M,T))\n",
    "u_bar[0,:] = MAX_ACC/2 #a\n",
    "u_bar[1,:] = 0.0      #delta\n",
    "\n",
    "for sim_time in range(sim_duration-1):\n",
    "    \n",
    "    iter_start = time.time()\n",
    "    \n",
    "    # dynamics starting state\n",
    "    x_bar = np.zeros((N,T+1))\n",
    "    x_bar[:,0] = x_sim[:,sim_time]\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",
    "    x = cp.Variable((N, T+1))\n",
    "    u = cp.Variable((M, T))\n",
    "    cost = 0\n",
    "    constr = []\n",
    "\n",
    "    # Cost Matrices\n",
    "    Q = np.diag([20,20,10,0]) #state error cost\n",
    "    Qf = np.diag([30,30,30,0]) #state final error cost\n",
    "    R = np.diag([10,10])       #input cost\n",
    "    R_ = np.diag([10,10])      #input rate of change cost\n",
    "\n",
    "    #Get Reference_traj\n",
    "    x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
    "    \n",
    "    #Prediction Horizon\n",
    "    for t in range(T):\n",
    "\n",
    "        # Tracking Error\n",
    "        cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
    "\n",
    "        # Actuation effort\n",
    "        cost += cp.quad_form(u[:,t], R)\n",
    "\n",
    "        # Actuation rate of change\n",
    "        if t < (T - 1):\n",
    "            cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
    "            constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC]    #max acc rate of change\n",
    "            constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\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",
    "    #Final Point tracking\n",
    "    cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
    "\n",
    "    # sums problem objectives and concatenates constraints.\n",
    "    constr += [x[:,0] == x_bar[:,0]]           #starting condition\n",
    "    constr += [x[2,:] <= MAX_SPEED]           #max speed\n",
    "    constr += [x[2,:] >= 0.0]                 #min_speed (not really needed)\n",
    "    constr += [cp.abs(u[0,:]) <= MAX_ACC]     #max acc\n",
    "    constr += [cp.abs(u[1,:]) <= MAX_STEER]   #max steer\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": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 102224 characters]\n",
      "text/plain": [
       "<Figure size 1080x720 with 5 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#plot trajectory\n",
    "grid = plt.GridSpec(4, 5)\n",
    "\n",
    "plt.figure(figsize=(15,10))\n",
    "\n",
    "plt.subplot(grid[0:4, 0:4])\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, 4])\n",
    "plt.plot(u_sim[0,:])\n",
    "plt.ylabel('a(t) [m/ss]')\n",
    "\n",
    "plt.subplot(grid[1, 4])\n",
    "plt.plot(x_sim[2,:])\n",
    "plt.ylabel('v(t) [m/s]')\n",
    "\n",
    "plt.subplot(grid[2, 4])\n",
    "plt.plot(np.degrees(u_sim[1,:]))\n",
    "plt.ylabel('delta(t) [rad]')\n",
    "\n",
    "plt.subplot(grid[3, 4])\n",
    "plt.plot(x_sim[3,:])\n",
    "plt.ylabel('theta(t) [rad]')\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python [conda env:.conda-jupyter] *",
   "language": "python",
   "name": "conda-env-.conda-jupyter-py"
  },
  "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
