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. \n",
    "在这篇笔记中，我试图通过曲线拟合来重现轨迹的参数化，就像在Udacity MPC课程中所做的那样。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T04:21:58.444911Z",
     "start_time": "2024-10-23T04:21:57.902473Z"
    }
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.interpolate import interp1d\n",
    "\n",
    "# 函数 compute_path_from_wp 用于生成平滑的轨迹。\n",
    "# 输入：start_xp 和 start_yp 分别是路径点的 x 和 y 坐标，step 是插值的步长。\n",
    "# 作用：通过使用线性插值的方法，在每两个路径点之间生成一些中间点，使路径更加平滑。\n",
    "# 细节：\n",
    "# 计算每段路径的长度。\n",
    "# 使用 interp1d 对每两个路径点进行线性插值，从而在每段路径之间生成更多的中间点。\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",
    "# 函数 get_nn_idx 用于找到车辆当前状态下，最接近的路径点索引。\n",
    "# \n",
    "# 输入：state 是车辆的当前位置和航向角信息，path 是轨迹点集合。\n",
    "# 作用：找到车辆当前位置与轨迹的最近点，并根据路径点的方向矢量来决定目标点的位置。\n",
    "# 细节：\n",
    "# 计算车辆与路径点之间的欧氏距离，通过 np.argmin(dist) 找到最近的路径点索引。\n",
    "# 然后根据路径点方向矢量，判断是否需要向前一个点继续行驶，最终返回目标路径点索引 target_idx。\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": 6,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-23T04:23:27.055937Z",
     "start_time": "2024-10-23T04:23:27.047349Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_8828/1217459931.py:29: RankWarning: Polyfit may be poorly conditioned\n",
      "  coeff = np.polyfit(\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",
    "# 转换前视路径点到车辆参考框架\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",
    "# 拟合多项式\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\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:48:10.578934Z",
     "start_time": "2024-10-22T09:48:10.573997Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "array([ 0.10275887,  0.03660033, -0.21750601,  0.03551043, -0.53861442,\n       -0.58083993])"
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "coeff"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "outputs": [
    {
     "data": {
      "text/plain": "array([-0.39433757,  0.08678766,  0.56791288,  1.04903811,  1.04903811,\n        1.67104657])"
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "wp_vehicle_frame[0, :]"
   ],
   "metadata": {
    "collapsed": false,
    "ExecuteTime": {
     "end_time": "2024-10-23T04:23:59.035926Z",
     "start_time": "2024-10-23T04:23:59.011734Z"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "outputs": [
    {
     "data": {
      "text/plain": "array([-0.34967937, -0.62745715, -0.90523492, -1.1830127 , -1.1830127 ,\n       -0.7723291 ])"
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "wp_vehicle_frame[1, :]"
   ],
   "metadata": {
    "collapsed": false,
    "ExecuteTime": {
     "end_time": "2024-10-23T04:24:00.859595Z",
     "start_time": "2024-10-23T04:24:00.856168Z"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:48:14.652868Z",
     "start_time": "2024-10-22T09:48:14.124049Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 640x480 with 2 Axes>",
      "image/png": "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
     },
     "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",
    "\n",
    "<!-- ![mpc](img/fitted_poly.png) -->\n",
    "\n",
    "Recall A fitted cubic poly has the form:\n",
    "回顾一下，拟合的三次多项式形式为：\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",
    "$\n",
    "f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
    "$\n",
    "\n",
    "Then we can formulate\n",
    "然后我们可以进行如下公式化\n",
    "\n",
    "* **crosstrack error** cte: desired y-position - y-position of vehicle -> this is the value of the fitted polynomial\n",
    "* **横向误差** cte：期望的y位置 - 车辆的y位置 -> 这是拟合多项式的值\n",
    "\n",
    "* **heading error** epsi:  desired heading - heading of vehicle -> is the inclination of  tangent to the  fitted polynomial\n",
    "* **航向误差** epsi：期望航向 - 车辆航向 -> 是拟合多项式的切线的倾斜\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",
    "\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...\n",
    "我使用了一个凸优化的 MPC，因此不允许非线性项（在 Udacity 的课程中，他们使用的是通用的非线性求解器） -> 因此这个解决方案对于我的情况并不太适用"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Extras"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:59:44.820293Z",
     "start_time": "2024-10-22T09:59:44.813407Z"
    }
   },
   "outputs": [],
   "source": [
    "# 五次样条曲线\n",
    "# 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"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "def spline_planning(xs, xf, ys, yf, dys=0.0, dyf=0.0, ddys=0.0, ddyf=0.0):\n",
    "    \"\"\"\n",
    "    计算五次多项式的系数，满足边界条件。\n",
    "\n",
    "    参数：\n",
    "    - xs: 初始位置 x\n",
    "    - xf: 最终位置 x\n",
    "    - ys: 初始位置 y\n",
    "    - yf: 最终位置 y\n",
    "    - dys: 初始速度 (默认值为 0)\n",
    "    - dyf: 最终速度 (默认值为 0)\n",
    "    - ddys: 初始加速度 (默认值为 0)\n",
    "    - ddyf: 最终加速度 (默认值为 0)\n",
    "\n",
    "    返回：\n",
    "    - a: 五次多项式的系数\n",
    "    \"\"\"\n",
    "    # 定义边界条件矩阵 B\n",
    "    bc = np.array([ys, dys, ddys, yf, dyf, ddyf])\n",
    "\n",
    "    # 定义系数矩阵 A\n",
    "    C = np.array(\n",
    "        [\n",
    "            [1, xs, xs**2, xs**3, xs**4, xs**5],  # f(xs)=ys\n",
    "            [0, 1, 2 * xs, 3 * xs**2, 4 * xs**3, 5 * xs**4],  # df(xs)=dys\n",
    "            [0, 0, 2, 6 * xs, 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, 3 * xf**2, 4 * xf**3, 5 * xf**4],  # df(xf)=dyf\n",
    "            [0, 0, 2, 6 * xf, 12 * xf**2, 20 * xf**3],  # ddf(xf)=ddyf\n",
    "        ]\n",
    "    )\n",
    "\n",
    "    # 计算多项式系数\n",
    "    a = np.linalg.solve(C, bc)  # 使用线性方程组求解 A * a = B\n",
    "    return a"
   ],
   "metadata": {
    "collapsed": false,
    "ExecuteTime": {
     "end_time": "2024-10-22T10:01:56.052349Z",
     "start_time": "2024-10-22T10:01:56.047486Z"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 1000x600 with 1 Axes>",
      "image/png": "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
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# 初始和最终位置及其条件\n",
    "xs = 0  # 初始位置\n",
    "xf = 10  # 最终位置\n",
    "ys = 0  # 初始位置 y\n",
    "yf = 5  # 最终位置 y\n",
    "dys = 1.0  # 初始速度\n",
    "dyf = 0.5  # 最终速度\n",
    "\n",
    "# 计算多项式系数\n",
    "coefficients = spline_planning(xs, xf, ys, yf, dys, dyf)\n",
    "\n",
    "# 使用计算出的系数生成样条曲线\n",
    "x_vals = np.linspace(xs, xf, 100)\n",
    "y_vals = [\n",
    "    coefficients[0] * x**5 + coefficients[1] * x**4 + coefficients[2] * x**3 + coefficients[3] * x**2 + coefficients[4] * x + coefficients[5]\n",
    "    for x in x_vals\n",
    "]\n",
    "\n",
    "# 绘制样条曲线\n",
    "plt.figure(figsize=(10, 6))\n",
    "plt.plot(x_vals, y_vals, label='Splined Path')\n",
    "plt.xlabel('X position')\n",
    "plt.ylabel('Y position')\n",
    "plt.title('5th Order Spline Path')\n",
    "plt.legend()\n",
    "plt.grid()\n",
    "plt.show()"
   ],
   "metadata": {
    "collapsed": false,
    "ExecuteTime": {
     "end_time": "2024-10-22T10:02:42.157868Z",
     "start_time": "2024-10-22T10:02:42.097737Z"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [],
   "metadata": {
    "collapsed": false
   }
  }
 ],
 "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
}