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": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:28:36.727289Z",
     "start_time": "2024-10-23T09:28:36.365530Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "(np.float64(-0.8500000000000001),\n np.float64(6.85),\n np.float64(-1.3744678440936948),\n np.float64(11.065450849718747))"
     },
     "execution_count": 1,
     "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": [
    "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": 3,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:28:51.991289Z",
     "start_time": "2024-10-23T09:28:51.987250Z"
    }
   },
   "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": 4,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:28:52.737070Z",
     "start_time": "2024-10-23T09:28:52.653026Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "Text(0.5, 0, 'x')"
     },
     "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": [
    "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": 5,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:28:54.589399Z",
     "start_time": "2024-10-23T09:28:54.532313Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "[<matplotlib.lines.Line2D at 0x1369457f0>]"
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "text/plain": "<Figure size 640x480 with 1 Axes>",
      "image/png": "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
     },
     "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": 6,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:29:05.534206Z",
     "start_time": "2024-10-23T09:29:05.527716Z"
    }
   },
   "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": 7,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:29:06.705696Z",
     "start_time": "2024-10-23T09:29:06.699962Z"
    }
   },
   "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": 7,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T09:29:07.582893Z",
     "start_time": "2024-10-23T09:29:07.568518Z"
    }
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "name": "python3",
   "language": "python",
   "display_name": "Python 3 (ipykernel)"
  },
  "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
}