mpc_python_learn/notebooks/3.0-MPC-v3-track-constrains.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": "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",
    "\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(\n",
    "        [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\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 np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
    "\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",
    "\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, dydt, dvdt, dthetadt]\n",
    "\n",
    "    return dqdt\n",
    "\n",
    "\n",
    "def predict(x0, u):\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, x0, tspan, 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",
    "\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(\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, 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 = [\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",
    "    return target_idx\n",
    "\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",
    "\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",
    "\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(\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, 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))"
   ]
  },
  {
   "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 0x7f8432529970>]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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
      "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",
    "\n",
    "track = compute_path_from_wp(\n",
    "    [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
    ")\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": "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
      "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",
    "\n",
    "plt.style.use(\"ggplot\")\n",
    "\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",
    "\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": "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
      "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"
   ]
  },
  {
   "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.1602s Max: 0.1839s Min: 0.1493s\n"
     ]
    }
   ],
   "source": [
    "track = compute_path_from_wp([0, 3, 3, 0], [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 += [\n",
    "                cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
    "            ]  # max acc rate of change\n",
    "            constr += [\n",
    "                cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
    "            ]  # 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(\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": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "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": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.3174s Max: 0.3505s Min: 0.3006s\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], [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 += [\n",
    "                cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
    "            ]  # max acc rate of change\n",
    "            constr += [\n",
    "                cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
    "            ]  # 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 += [\n",
    "            upp[0, ii] * x[0, ii] + x[1, ii] <= upp[2, ii]\n",
    "        ]  # <-- CAUSES ISSUES Y?\n",
    "\n",
    "    # Solve\n",
    "    prob = cp.Problem(cp.Minimize(cost), constr)\n",
    "    solution = prob.solve(solver=cp.ECOS, 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": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "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": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "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",
    "\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",
    "\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], computed_coeff[indices[i]][1])\n",
    "\n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "\n",
    "\n",
    "plot_lines()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### NOTE \n",
    "There seems to be some flaws in the impementation\n",
    "the plane to the LEFT of the vehicle causes issues in the optimization...\n",
    "\n",
    "*maybe the \"hanging\" points?*\n",
    "\n",
    "different approach -> compute those points from the path directly ?  Then from contour get semiplanes consrtains?"
   ]
  },
  {
   "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
}