328 lines
32 KiB
Plaintext
328 lines
32 KiB
Plaintext
{
|
|
"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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\n",
|
|
"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",
|
|
"<!--  -->\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
|
|
}
|