{
 "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",
    "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",
    "\n",
    "\"\"\"\n",
    "MODIFIED TO INCLUDE FRAME TRANSFORMATION\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",
    "        # watch out to duplicate points!\n",
    "        final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
    "        final_yp=np.append(final_yp,fy(interp_range)[1:])\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 normalize_angle(angle):\n",
    "    \"\"\"\n",
    "    Normalize an angle to [-pi, pi]\n",
    "    \"\"\"\n",
    "    while angle > np.pi:\n",
    "        angle -= 2.0 * np.pi\n",
    "\n",
    "    while angle < -np.pi:\n",
    "        angle += 2.0 * np.pi\n",
    "\n",
    "    return angle\n",
    "\n",
    "def get_ref_trajectory(state, path, target_v):\n",
    "    \"\"\"\n",
    "    modified reference in robot frame\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",
    "    dx=path[0,ind] - state[0]\n",
    "    dy=path[1,ind] - state[1]\n",
    "    \n",
    "    xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X\n",
    "    xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y\n",
    "    xref[2, 0] = target_v                                        #V\n",
    "    xref[3, 0] = normalize_angle(path[2,ind]- state[3])          #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",
    "            dx=path[0,ind + dind] - state[0]\n",
    "            dy=path[1,ind + dind] - state[1]\n",
    "            \n",
    "            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
    "            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
    "            xref[2, i] = target_v #sp[ind + dind]\n",
    "            xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])\n",
    "            dref[0, i] = 0.0\n",
    "        else:\n",
    "            dx=path[0,ncourse - 1] - state[0]\n",
    "            dy=path[1,ncourse - 1] - state[1]\n",
    "            \n",
    "            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
    "            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
    "            xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
    "            xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])\n",
    "            dref[0, i] = 0.0\n",
    "\n",
    "    return xref, dref"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## SMOOTHEN PATH\n",
    "\n",
    "I use a corner smoothing tecnique to help the line-finding proplem -> this way the line does not change from 0 to pi/2 instantly"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.signal import savgol_filter\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",
    "        # watch out to duplicate points!\n",
    "        final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
    "        final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
    "        \n",
    "    \"\"\"this smoothens up corners\"\"\"\n",
    "    window_size = 7 # Smoothening filter window\n",
    "    final_xp = savgol_filter(final_xp, window_size, 1)\n",
    "    final_yp = savgol_filter(final_yp, window_size, 1)\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"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## V3 Add track constraints\n",
    "inspried from -> https://arxiv.org/pdf/1711.07300.pdf\n",
    "\n",
    "explanation here...\n",
    "\n",
    "benefits:\n",
    "* add a soft form of obstacle aoidance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f10ac8425e0>]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 89644 characters]\n",
      "text/plain": [
       "<Figure size 1080x720 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def generate_track_bounds(track,width=0.5):\n",
    "    \"\"\"\n",
    "    in world frame\n",
    "    \"\"\"\n",
    "    bounds_low=np.zeros((2, track.shape[1]))\n",
    "    bounds_upp=np.zeros((2, track.shape[1]))\n",
    "    \n",
    "    for idx in range(track.shape[1]):\n",
    "        x = track[0,idx]\n",
    "        y = track [1,idx]\n",
    "        th = track [2,idx]\n",
    "        \n",
    "        \"\"\"\n",
    "        trasform the points\n",
    "        \"\"\"\n",
    "        bounds_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n",
    "        bounds_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n",
    "        \n",
    "        bounds_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n",
    "        bounds_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n",
    "        \n",
    "    return bounds_low, bounds_upp\n",
    "\n",
    "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",
    "lower, upper = generate_track_bounds(track)\n",
    "\n",
    "plt.figure(figsize=(15,10))\n",
    "\n",
    "plt.plot(track[0,:],track[1,:],\"b-\")\n",
    "plt.plot(lower[0,:],lower[1,:],\"g-\")\n",
    "plt.plot(upper[0,:],upper[1,:],\"r-\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "the points can be used to generate the **halfplane constrains** for each reference point.\n",
    "the issues (outliers points) should be gone after we are in vehicle frame...\n",
    "\n",
    "the halfplane constrains are defined given the line equation:\n",
    "\n",
    "**lower halfplane**\n",
    "$$ a1x_1 + b1x_2 = c1 \\rightarrow a1x_1 + b1x_2 \\leq c1$$\n",
    "\n",
    "**upper halfplane**\n",
    "$$ a2x_1 - b2x_2 = c2 \\rightarrow a2x_1 + b2x_2 \\leq c2$$\n",
    "\n",
    "we want to combine this in matrix form:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "x_1 \\\\\n",
    "x_2 \n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "a_1 & a_2\\\\\n",
    "b_1 & b_2\n",
    "\\end{bmatrix}\n",
    "\\leq\n",
    "\\begin{bmatrix}\n",
    "c_1 \\\\\n",
    "c_2 \n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "becouse our track points have known heading the coefficients can be computed from:\n",
    "\n",
    "$$ y - y' = \\frac{sin(\\theta)}{cos(\\theta)}(x - x') $$\n",
    "\n",
    "we have:\n",
    "\n",
    "$$\n",
    "-tan(\\theta)x + y =  - tan(\\theta)x' + y'\n",
    "$$\n",
    "where:\n",
    "* $ a =  -tan(\\theta) $\n",
    "* $ b = 1 $\n",
    "* $ c = - tan(\\theta)x' + y' $"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-2.0, 2.0)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 11740 characters]\n",
      "text/plain": [
       "<Figure size 360x360 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "def get_coeff(x,y,theta):\n",
    "    m = np.sin(theta)/np.cos(theta)\n",
    "    return(-m,1,y-m*x)\n",
    "\n",
    "#test -> assume point 10,1,pi/6\n",
    "#coeff = get_coeff(1,-1, np.pi/2)\n",
    "coeff = get_coeff(1,-1, np.pi/4)\n",
    "y = []\n",
    "pts = np.linspace(0,20,100)\n",
    "\n",
    "for x in pts:\n",
    "    y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
    "    \n",
    "plt.figure(figsize=(5,5))\n",
    "plt.plot(pts,y,\"b-\")\n",
    "\n",
    "plt.xlim((-2, 2))\n",
    "plt.ylim((-2, 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## WARN TANGENT BREAKS AROUND PI/2?\n",
    "force the equation to x = val"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-2.0, 2.0)"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 8076 characters]\n",
      "text/plain": [
       "<Figure size 360x360 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# def get_coeff(x,y,theta):\n",
    "    \n",
    "#     if (theta - np.pi/2) < 0.01:\n",
    "#         #print (\"WARN -> theta is 90, tan is: \" + str(theta))\n",
    "#         # eq is x = val\n",
    "#         m = 0\n",
    "#         return (1,1e-6,x)\n",
    "#     else:\n",
    "#         m = np.sin(theta)/np.cos(theta)\n",
    "#         return(-m,1,y-m*x)\n",
    "    \n",
    "#test -> assume point 10,1,pi/6\n",
    "coeff = get_coeff(1,-1, np.pi/2)\n",
    "y = []\n",
    "pts = np.linspace(0,20,100)\n",
    "\n",
    "for x in pts:\n",
    "    y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
    "    \n",
    "plt.figure(figsize=(5,5))\n",
    "\n",
    "plt.plot(pts,y,\"b-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.xlim((-2, 2))\n",
    "plt.ylim((-2, 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "becouse the controller uses vhicle reference frame this rquire adapting -> the semiplane constraints must be gathetered from **x_ref points**\n",
    "\n",
    "*low and up are w.r.t vehicle y axis*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_track_constrains(x_ref, width=0.5):\n",
    "    \"\"\"\n",
    "    x_ref has hape (4,T) -> [x,y,v,theta]_ref \n",
    "    \"\"\"\n",
    "    \n",
    "    #1-> get the upper and lower points\n",
    "    pts_low=np.zeros((3, x_ref.shape[1]))\n",
    "    pts_upp=np.zeros((3, x_ref.shape[1]))\n",
    "    \n",
    "    for idx in range(x_ref.shape[1]):\n",
    "        x = x_ref [0, idx]\n",
    "        y = x_ref [1, idx]\n",
    "        th = x_ref [3, idx]\n",
    "        \n",
    "        \"\"\"\n",
    "        trasform the points\n",
    "        \"\"\"\n",
    "        pts_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n",
    "        pts_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n",
    "        pts_upp[2, idx] = th #heading\n",
    "        \n",
    "        pts_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n",
    "        pts_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n",
    "        pts_low[2, idx] = th #heading\n",
    "    \n",
    "    #get coefficients ->(a,b,c)\n",
    "    coeff_low=np.zeros((3, x_ref.shape[1]))\n",
    "    coeff_upp=np.zeros((3, x_ref.shape[1]))\n",
    "    \n",
    "    for idx in range(pts_upp.shape[1]):\n",
    "        f = get_coeff(pts_low[0,idx],pts_low[1,idx],pts_low[2,idx])\n",
    "        coeff_low[0,idx]=f[0]\n",
    "        coeff_low[1,idx]=f[1]\n",
    "        coeff_low[2,idx]=f[2]\n",
    "        \n",
    "        f = get_coeff(pts_upp[0,idx],pts_upp[1,idx],pts_upp[2,idx])\n",
    "        coeff_upp[0,idx]=f[0]\n",
    "        coeff_upp[1,idx]=f[1]\n",
    "        coeff_upp[2,idx]=f[2]\n",
    "    \n",
    "    return coeff_low, coeff_upp\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## MPC INTEGRATION\n",
    "\n",
    "compare the results with and without"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "simpe u-turn test\n",
    "\n",
    "## 1-> NO BOUNDS"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.1719s Max: 0.2054s Min: 0.1581s\n"
     ]
    }
   ],
   "source": [
    "track = compute_path_from_wp([0,3,3,0],\n",
    "                             [0,0,1,1],0.05)\n",
    "\n",
    "track_lower, track_upper = generate_track_bounds(track,0.12)\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.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 = 0.5 #m/s\n",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0               #x\n",
    "x0[1] = -0.05           #y\n",
    "x0[2] = 0.0             #v\n",
    "x0[3] = np.radians(-0)  #yaw\n",
    "x_sim[:,0] = x0         #simulation_starting conditions\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 w.r.t. robot are always null except vel \n",
    "    x_bar = np.zeros((N,T+1))\n",
    "    x_bar[2,0] = x_sim[2,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,20]) #state error cost\n",
    "    Qf = np.diag([30,30,30,30]) #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",
    "    #dont use x0 in this case\n",
    "    x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,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": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 71132 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(track_lower[0,:],track_lower[1,:],\"g-\")\n",
    "plt.plot(track_upper[0,:],track_upper[1,:],\"r-\")\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()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2-> WITH BOUNDS\n",
    "if there is 90 deg turn the optimization fails!\n",
    "if speed is too high it also fails ..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.1803s Max: 0.2286s Min: 0.1630s\n"
     ]
    }
   ],
   "source": [
    "WIDTH=0.18\n",
    "REF_VEL = 0.4 #m/s\n",
    "\n",
    "computed_coeff = []\n",
    "\n",
    "track = compute_path_from_wp([0,3,3,0],\n",
    "                             [0,0,1,1],0.05)\n",
    "\n",
    "track_lower, track_upper = generate_track_bounds(track,WIDTH)\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",
    "\n",
    "# Starting Condition\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0               #x\n",
    "x0[1] = WIDTH/2           #y\n",
    "x0[2] = 0.0             #v\n",
    "x0[3] = np.radians(-0)  #yaw\n",
    "x_sim[:,0] = x0         #simulation_starting conditions\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 w.r.t. robot are always null except vel \n",
    "    x_bar = np.zeros((N,T+1))\n",
    "    x_bar[2,0] = x_sim[2,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,20]) #state error cost\n",
    "    Qf = np.diag([30,30,30,30]) #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",
    "    #dont use x0 in this case\n",
    "    x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,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",
    "    #Track constrains\n",
    "    low,upp = get_track_constrains(x_ref,WIDTH)\n",
    "    computed_coeff.append((low,upp))\n",
    "    for ii in range(low.shape[1]):\n",
    "        constr += [low[0,ii]*x[0,ii] + x[1,ii] >= low[2,ii]]\n",
    "        #constr += [upp[0,ii]*x[0,ii] + x[1,ii] <= upp[2,ii]]\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": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 67544 characters]\n",
      "text/plain": [
       "<Figure size 1080x720 with 5 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib inline\n",
    "#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(track_lower[0,:],track_lower[1,:],\"g.\")\n",
    "plt.plot(track_upper[0,:],track_upper[1,:],\"r.\")\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",
    "\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()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## VISUALIZE THE COMPUTED HALF-PLANES"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 92032 characters]\n",
      "text/plain": [
       "<Figure size 1080x1080 with 9 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "\n",
    "def plot_low_upp(low, up):\n",
    "    \"\"\"\n",
    "    low and upp arre arrays of shape (3,N)\n",
    "    each column represent the couefficient at each\n",
    "    step of the optimization\n",
    "    \"\"\"\n",
    "    \n",
    "    #for low,up in zip(low, up):\n",
    "    for idx in range(low.shape[1]):\n",
    "        \n",
    "        ll = low[:,idx]\n",
    "        uu = up[:,idx]\n",
    "        \n",
    "        x = np.linspace(-2,2,100)\n",
    "\n",
    "        #low\n",
    "        y = []\n",
    "        for xx in x:\n",
    "            y.append((-ll[0]*xx+ll[2])/ll[1])\n",
    "\n",
    "        plt.plot(x,y,\"b-\")\n",
    "\n",
    "        #high\n",
    "        y = []\n",
    "        for xx in x:\n",
    "            y.append((-uu[0]*xx+uu[2])/uu[1])\n",
    "\n",
    "        plt.plot(x,y,\"r-\")\n",
    "\n",
    "        plt.xlim((-2, 2))\n",
    "        plt.ylim((-2, 2))\n",
    "\n",
    "def plot_lines():\n",
    "    \"\"\"\n",
    "    sample randomly from computed coeff\n",
    "    and plot grid\n",
    "    \"\"\"\n",
    "    \n",
    "    plt.figure(figsize=(15,15))\n",
    "    indices = np.random.choice(len(computed_coeff), 9)\n",
    "\n",
    "    for i in range(len(indices)):\n",
    "        plt.subplot(3,3,i+1)\n",
    "        plt.title(\"t = \"+str(indices[i]))\n",
    "        plot_low_upp(computed_coeff[indices[i]][0]\n",
    "                     ,computed_coeff[indices[i]][1])\n",
    "    \n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "\n",
    "plot_lines()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}
