mpc_python_learn/notebooks/1.1-parametrized-path-curves.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PATH WAYPOINTS AS PARAMETRIZED CURVE"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this notebook I try to reproduce the parmetrization of the track via curve-fitting like its done in Udacity MPC Course. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.interpolate import interp1d\n",
    "\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
    "    final_xp = []\n",
    "    final_yp = []\n",
    "    delta = step  # [m]\n",
    "\n",
    "    for idx in range(len(start_xp) - 1):\n",
    "        section_len = np.sum(\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",
    "        final_xp = np.append(final_xp, fx(interp_range))\n",
    "        final_yp = np.append(final_yp, fy(interp_range))\n",
    "\n",
    "    return np.vstack((final_xp, final_yp))\n",
    "\n",
    "\n",
    "def get_nn_idx(state, path):\n",
    "\n",
    "    dx = state[0] - path[0, :]\n",
    "    dy = state[1] - path[1, :]\n",
    "    dist = np.sqrt(dx**2 + dy**2)\n",
    "    nn_idx = np.argmin(dist)\n",
    "\n",
    "    try:\n",
    "        v = [\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"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/marcello/miniconda3/envs/jupyter/lib/python3.8/site-packages/IPython/core/interactiveshell.py:3331: RankWarning: Polyfit may be poorly conditioned\n",
      "  exec(code_obj, self.user_global_ns, self.user_ns)\n"
     ]
    }
   ],
   "source": [
    "# define track\n",
    "wp = np.array([0, 5, 6, 10, 11, 15, 0, 0, 2, 2, 0, 4]).reshape(2, -1)\n",
    "track = compute_path_from_wp(wp[0, :], wp[1, :], step=0.5)\n",
    "\n",
    "# vehicle state\n",
    "state = [3.5, 0.5, np.radians(30)]\n",
    "\n",
    "# given vehicle pos find lookahead waypoints\n",
    "nn_idx = (\n",
    "    get_nn_idx(state, track) - 1\n",
    ")  # index ox closest wp, take the previous to have a straighter line\n",
    "LOOKAHED = 6\n",
    "lk_wp = track[:, nn_idx : nn_idx + LOOKAHED]\n",
    "\n",
    "# trasform lookahead waypoints to vehicle ref frame\n",
    "dx = lk_wp[0, :] - state[0]\n",
    "dy = lk_wp[1, :] - state[1]\n",
    "\n",
    "wp_vehicle_frame = np.vstack(\n",
    "    (\n",
    "        dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
    "        dy * np.cos(-state[2]) + dx * np.sin(-state[2]),\n",
    "    )\n",
    ")\n",
    "\n",
    "# fit poly\n",
    "coeff = np.polyfit(\n",
    "    wp_vehicle_frame[0, :],\n",
    "    wp_vehicle_frame[1, :],\n",
    "    5,\n",
    "    rcond=None,\n",
    "    full=False,\n",
    "    w=None,\n",
    "    cov=False,\n",
    ")\n",
    "\n",
    "# def f(x,coeff):\n",
    "#    return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
    "def f(x, coeff):\n",
    "    return (\n",
    "        coeff[0] * x**5\n",
    "        + coeff[1] * x**4\n",
    "        + coeff[2] * x**3\n",
    "        + coeff[3] * x**2\n",
    "        + coeff[4] * x**1\n",
    "        + coeff[5] * x**0\n",
    "    )\n",
    "\n",
    "\n",
    "def f(x, coeff):\n",
    "    y = 0\n",
    "    j = len(coeff)\n",
    "    for k in range(j):\n",
    "        y += coeff[k] * x ** (j - k - 1)\n",
    "    return y\n",
    "\n",
    "\n",
    "# def df(x,coeff):\n",
    "#     return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
    "def df(x, coeff):\n",
    "    y = 0\n",
    "    j = len(coeff)\n",
    "    for k in range(j - 1):\n",
    "        y += (j - k - 1) * coeff[k] * x ** (j - k - 2)\n",
    "    return y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.10275887,  0.03660033, -0.21750601,  0.03551043, -0.53861442,\n",
       "       -0.58083993])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "coeff"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "x = np.arange(-1, 2, 0.001)  # interp range of curve\n",
    "\n",
    "# VEHICLE REF FRAME\n",
    "plt.subplot(2, 1, 1)\n",
    "plt.title(\"parametrized curve, vehicle ref frame\")\n",
    "plt.scatter(0, 0)\n",
    "plt.scatter(wp_vehicle_frame[0, :], wp_vehicle_frame[1, :])\n",
    "plt.plot(x, [f(xs, coeff) for xs in x])\n",
    "plt.axis(\"equal\")\n",
    "\n",
    "# MAP REF FRAME\n",
    "plt.subplot(2, 1, 2)\n",
    "plt.title(\"waypoints, map ref frame\")\n",
    "plt.scatter(state[0], state[1])\n",
    "plt.scatter(track[0, :], track[1, :])\n",
    "plt.scatter(track[0, nn_idx : nn_idx + LOOKAHED], track[1, nn_idx : nn_idx + LOOKAHED])\n",
    "plt.axis(\"equal\")\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()\n",
    "# plt.savefig(\"fitted_poly\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Error Formulation\n",
    "\n",
    "So, the track can be represented by fitting a curve trough its waypoints, using the vehicle position as reference!\n",
    "\n",
    "<!-- ![mpc](img/fitted_poly.png) -->\n",
    "\n",
    "Recall A fitted cubic poly has the form:\n",
    "\n",
    "$\n",
    "f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
    "$\n",
    "\n",
    "The derivative of a fitted cubic poly has the form:\n",
    "\n",
    "$\n",
    "f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
    "$\n",
    "\n",
    "Then we can formulate\n",
    "\n",
    "* **crosstrack error** cte: desired y-position - y-position of vehicle -> this is the value of the fitted polynomial\n",
    "\n",
    "* **heading error** epsi:  desired heading - heading of vehicle -> is the inclination of  tangent to the  fitted polynomial\n",
    "\n",
    "Becouse the reference is centered on vehicle the eqation are simplified!\n",
    "Then using the fitted polynomial representation in vehicle frame the errors can be easily computed as:\n",
    "\n",
    "$\n",
    "cte = f(px) \\\\\n",
    "\\psi = -atan(f`(px)) \\\\\n",
    "$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### In Practice:\n",
    "I use a **convex** mpc so non-linearities are not allowed (in Udacity they use a general-purpose nonlinear solver) -> so this solution does not really work well for my case..."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Extras"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n",
    "\n",
    "    bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T\n",
    "\n",
    "    C = np.array(\n",
    "        [\n",
    "            [1, xs, xs**2, xs**3, xs**4, xs**5],  # f(xs)=ys\n",
    "            [0, 1, 2 * xs**1, 3 * xs**2, 4 * xs**3, 5**xs ^ 4],  # df(xs)=dys\n",
    "            [0, 0, 1, 6 * xs**1, 12 * xs**2, 20**xs ^ 3],  # ddf(xs)=ddys\n",
    "            [1, xf, xf**2, xf**3, xf**4, xf**5],  # f(xf)=yf\n",
    "            [0, 1, 2 * xf**1, 3 * xf**2, 4 * xf**3, 5**xf ^ 4],  # df(xf)=dyf\n",
    "            [0, 0, 1, 6 * xf**1, 12 * xf**2, 20**xf ^ 3],\n",
    "        ]\n",
    "    )  # ddf(xf)=ddyf\n",
    "\n",
    "    # To compute the polynomial coefficients we solve:\n",
    "    # Ax = B.\n",
    "    # Matrices A and B must have the same number of rows\n",
    "    a = np.linalg.lstsq(C, bc)[0]\n",
    "    return a"
   ]
  }
 ],
 "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
}