{
 "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": "[encoded data, 40784 characters]"
     },
     "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": "[encoded data, 38984 characters]"
     },
     "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
}
