mpc_python_learn/notebooks/3.0-MPC-v3-track-constrains.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:24:14.711190Z",
     "start_time": "2024-10-23T09:24:13.777868Z"
    }
   },
   "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:24:14.748515Z",
     "start_time": "2024-10-23T09:24:14.746753Z"
    }
   },
   "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\n",
    "我使用了一种转角平滑技术来帮助解决找线问题 -> 通过这种方式，线不会从 0 瞬间变为 𝜋/2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:25:04.148826Z",
     "start_time": "2024-10-23T09:25:04.116482Z"
    }
   },
   "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:25:27.470304Z",
     "start_time": "2024-10-23T09:25:27.366214Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "[<matplotlib.lines.Line2D at 0x11d851f40>]"
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "text/plain": "<Figure size 1500x1000 with 1 Axes>",
      "image/png": "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
     },
     "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:25:41.213106Z",
     "start_time": "2024-10-23T09:25:41.152165Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "(-2.0, 2.0)"
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "text/plain": "<Figure size 500x500 with 1 Axes>",
      "image/png": "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
     },
     "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:25:48.168113Z",
     "start_time": "2024-10-23T09:25:48.117783Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "(-2.0, 2.0)"
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "text/plain": "<Figure size 500x500 with 1 Axes>",
      "image/png": "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
     },
     "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:25:50.882261Z",
     "start_time": "2024-10-23T09:25:50.874762Z"
    }
   },
   "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:25:57.020073Z",
     "start_time": "2024-10-23T09:25:53.799988Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:27: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[0, 2] = np.cos(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:28: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[0, 3] = -v * np.sin(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:29: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[1, 2] = np.sin(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:30: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[1, 3] = v * np.cos(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:31: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[3, 2] = v * np.tan(delta) / L\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:36: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  B[3, 1] = v / (L * np.cos(delta) ** 2)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.0656s Max: 0.0909s Min: 0.0533s\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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:25:59.769712Z",
     "start_time": "2024-10-23T09:25:59.600540Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 1500x1000 with 5 Axes>",
      "image/png": "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
     },
     "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:26:36.831723Z",
     "start_time": "2024-10-23T09:26:14.421549Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:27: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[0, 2] = np.cos(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:28: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[0, 3] = -v * np.sin(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:29: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[1, 2] = np.sin(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:30: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[1, 3] = v * np.cos(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:31: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[3, 2] = v * np.tan(delta) / L\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_24294/4187762036.py:36: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
      "/Users/qp/Applications/miniforge3/envs/mpc_python12/lib/python3.12/site-packages/cvxpy/reductions/solvers/solving_chain.py:356: FutureWarning: \n",
      "    You specified your problem should be solved by ECOS. Starting in\n",
      "    CXVPY 1.6.0, ECOS will no longer be installed by default with CVXPY.\n",
      "    Please either add ECOS as an explicit install dependency to your project\n",
      "    or switch to our new default solver, Clarabel, by either not specifying a\n",
      "    solver argument or specifying ``solver=cp.CLARABEL``. To suppress this\n",
      "    warning while continuing to use ECOS, you can filter this warning using\n",
      "    Python's ``warnings`` module until you are using 1.6.0.\n",
      "    \n",
      "  warnings.warn(ECOS_DEP_DEPRECATION_MSG, FutureWarning)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVXPY Optimization Time: Avrg: 0.1127s Max: 0.1371s Min: 0.0927s\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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:26:42.544764Z",
     "start_time": "2024-10-23T09:26:42.368460Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 1500x1000 with 5 Axes>",
      "image/png": "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
     },
     "metadata": {},
     "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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:26:48.629815Z",
     "start_time": "2024-10-23T09:26:47.949306Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 1500x1500 with 9 Axes>",
      "image/png": "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
     },
     "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",
    "实现中似乎存在一些缺陷\n",
    "the plane to the LEFT of the vehicle causes issues in the optimization...\n",
    "车辆左侧的平面会导致优化问题...\n",
    "\n",
    "*maybe the \"hanging\" points?*\n",
    "*也许是“悬挂”点？*\n",
    "\n",
    "different approach -> compute those points from the path directly ?  Then from contour get semiplanes consrtains?\n",
    "不同的方法 -> 直接从路径计算这些点？然后从轮廓中获取半平面约束？"
   ]
  },
  {
   "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
}