mpc_python_learn/notebooks/3.1-better-track.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "thanks to -> https://stackoverflow.com/questions/49558464/shrink-polygon-using-corner-coordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-0.8500000000000001, 6.85, -1.3744678440936948, 11.065450849718747)"
      ]
     },
     "execution_count": 1,
     "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": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "\n",
    "def shrink_polygon(coords, shrink_value_x, shrink_value_y):\n",
    "    \"\"\" \"\"\"\n",
    "\n",
    "    def det(a, b):\n",
    "        return a[0] * b[1] - a[1] * b[0]\n",
    "\n",
    "    def line_intersection(line1, line2):\n",
    "        xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])\n",
    "        ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])  # Typo was here\n",
    "\n",
    "        div = det(xdiff, ydiff)\n",
    "        if div == 0:\n",
    "            raise Exception(\"lines do not intersect\")\n",
    "\n",
    "        d = (det(*line1), det(*line2))\n",
    "        x = det(d, xdiff) / div\n",
    "        y = det(d, ydiff) / div\n",
    "        return x, y\n",
    "\n",
    "    # how much the coordinates are moved as an absolute value\n",
    "    # shrink_value_x = 2\n",
    "    # shrink_value_y = 2\n",
    "\n",
    "    # coords must be clockwise\n",
    "    # coords = [(0, 0), (0, 100), (20, 100), (30, 60), (40, 100), (60, 100), (60, 0), (40, 10), (40, 40), (20, 40), (20, 10)]\n",
    "\n",
    "    lines = [[coords[i - 1], coords[i]] for i in range(len(coords))]\n",
    "\n",
    "    new_lines = []\n",
    "    for i in lines:\n",
    "        dx = i[1][0] - i[0][0]\n",
    "        dy = i[1][1] - i[0][1]\n",
    "\n",
    "        # this is to take into account slopes\n",
    "        factor = 1 / (dx * dx + dy * dy) ** 0.5\n",
    "        new_dx = dy * shrink_value_x * factor\n",
    "        new_dy = dx * shrink_value_y * factor\n",
    "\n",
    "        new_lines.append(\n",
    "            [(i[0][0] + new_dx, i[0][1] - new_dy), (i[1][0] + new_dx, i[1][1] - new_dy)]\n",
    "        )\n",
    "\n",
    "    # find position of intersection of all the lines\n",
    "    new_coords = []\n",
    "    for i in range(len(new_lines)):\n",
    "        new_coords.append((line_intersection(new_lines[i - 1], new_lines[i])))\n",
    "\n",
    "    return new_coords\n",
    "\n",
    "\n",
    "# must be clockwise!\n",
    "coords = [\n",
    "    (0, 0),\n",
    "    (0, 10),\n",
    "    (2, 10),\n",
    "    (3, 8),\n",
    "    (4, 10),\n",
    "    (6, 10),\n",
    "    (6, 0),\n",
    "    (4, 1),\n",
    "    (4, 4),\n",
    "    (2, 4),\n",
    "    (2, 1),\n",
    "]\n",
    "\n",
    "# how much the coordinates are moved as an absolute value\n",
    "SHIFT = 0.5  # [m]\n",
    "\n",
    "up = shrink_polygon(coords, -SHIFT, -SHIFT)\n",
    "lo = shrink_polygon(coords, SHIFT, SHIFT)\n",
    "\n",
    "\"\"\"\n",
    "the last point is out of order for my needs\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "def rotate(l, n):\n",
    "    return l[n:] + l[:n]\n",
    "\n",
    "\n",
    "up = rotate(up, 1)\n",
    "lo = rotate(lo, 1)\n",
    "\n",
    "a = np.array(up)\n",
    "b = np.array(coords)\n",
    "c = np.array(lo)\n",
    "\n",
    "plt.figure(figsize=(15, 10))\n",
    "plt.plot(a[:, 0], a[:, 1], \"b-\")\n",
    "plt.plot(b[:, 0], b[:, 1], \"r-\")\n",
    "plt.plot(c[:, 0], c[:, 1], \"g-\")\n",
    "plt.axis(\"equal\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "now these edges can be used to compute the path as before! Plus outer and inner track"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.interpolate import interp1d\n",
    "from scipy.signal import savgol_filter\n",
    "\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step=0.1, smooth_factor=7):\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 = smooth_factor  # 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": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Text(0.5, 0, 'x')"
      ]
     },
     "execution_count": 3,
     "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": [
    "track = compute_path_from_wp(b[:, 0], b[:, 1])\n",
    "track_lower = compute_path_from_wp(c[:, 0], c[:, 1])\n",
    "track_upper = compute_path_from_wp(a[:, 0], a[:, 1])\n",
    "\n",
    "plt.figure(figsize=(15, 10))\n",
    "\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.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7fc509665310>]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.plot(np.degrees(track[2, :]), \"b-\")\n",
    "plt.plot(np.degrees(track_lower[2, :]), \"g-\")\n",
    "plt.plot(np.degrees(track_upper[2, :]), \"r-\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "probably smarter to to save this in a file and load it in a notebook, so I dont have to carry over the code..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "with open(\"tracks/test.npy\", \"wb\") as f:\n",
    "    np.save(f, track)\n",
    "    np.save(f, track_lower)\n",
    "    np.save(f, track_upper)\n",
    "\n",
    "with open(\"tracks/test.npy\", \"rb\") as f:\n",
    "    a = np.load(f)\n",
    "    b = np.load(f)\n",
    "    c = np.load(f)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# matches perfectly!\n",
    "# plt.plot(np.degrees(a[2,:]),\"b-\")\n",
    "# plt.plot(np.degrees(b[2,:]),\"g-\")\n",
    "# plt.plot(np.degrees(c[2,:]),\"r-\")"
   ]
  },
  {
   "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
}