680 lines
41 KiB
Plaintext
680 lines
41 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# STATE SPACE MODEL MATRICES\n",
|
|
"\n",
|
|
"### Diff drive"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & v \\cos{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & v \\cos{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[0, 0, -v*sin(theta), 0, 0],\n",
|
|
"[0, 0, v*cos(theta), 0, 0],\n",
|
|
"[0, 0, 0, 0, 0],\n",
|
|
"[0, 0, 0, 0, 0],\n",
|
|
"[0, 0, 0, v*cos(psi), 0]])"
|
|
]
|
|
},
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"import sympy as sp\n",
|
|
"\n",
|
|
"x,y,theta,psi,cte,v,w = sp.symbols(\"x y theta psi cte v w\")\n",
|
|
"\n",
|
|
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
|
|
" [ sp.sin(theta)*v],\n",
|
|
" [w],\n",
|
|
" [-w],\n",
|
|
" [ v*sp.sin(psi)]])\n",
|
|
"\n",
|
|
"state = sp.Matrix([x,y,theta,psi,cte])\n",
|
|
"\n",
|
|
"#A\n",
|
|
"gs.jacobian(state)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & 0\\\\0 & 1\\\\0 & -1\\\\\\sin{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[cos(theta), 0],\n",
|
|
"[sin(theta), 0],\n",
|
|
"[ 0, 1],\n",
|
|
"[ 0, -1],\n",
|
|
"[ sin(psi), 0]])"
|
|
]
|
|
},
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"state = sp.Matrix([v,w])\n",
|
|
"\n",
|
|
"#B\n",
|
|
"gs.jacobian(state)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}1 & 0 & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[1, 0, -dt*v*sin(theta)],\n",
|
|
"[0, 1, dt*v*cos(theta)],\n",
|
|
"[0, 0, 1]])"
|
|
]
|
|
},
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"import sympy as sp\n",
|
|
"\n",
|
|
"x,y,theta,psi,cte,v,w ,dt= sp.symbols(\"x y theta psi cte v w dt\")\n",
|
|
"\n",
|
|
"gs = sp.Matrix([[x + sp.cos(theta)*v*dt],\n",
|
|
" [y+ sp.sin(theta)*v*dt],\n",
|
|
" [theta + w*dt]])\n",
|
|
"\n",
|
|
"state = sp.Matrix([x,y,theta])\n",
|
|
"\n",
|
|
"#A\n",
|
|
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}dt \\cos{\\left(\\theta \\right)} & 0\\\\dt \\sin{\\left(\\theta \\right)} & 0\\\\0 & dt\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[dt*cos(theta), 0],\n",
|
|
"[dt*sin(theta), 0],\n",
|
|
"[ 0, dt]])"
|
|
]
|
|
},
|
|
"execution_count": 4,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"state = sp.Matrix([v,w])\n",
|
|
"\n",
|
|
"#B\n",
|
|
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"### Ackermann"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 5,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"x,y,theta,v,delta,L,a = sp.symbols(\"x y theta v delta L a\")\n",
|
|
"\n",
|
|
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
|
|
" [ sp.sin(theta)*v],\n",
|
|
" [a],\n",
|
|
" [ v*sp.tan(delta)/L]])\n",
|
|
"\n",
|
|
"X = sp.Matrix([x,y,v,theta])\n",
|
|
"\n",
|
|
"#A\n",
|
|
"A=gs.jacobian(X)\n",
|
|
"\n",
|
|
"U = sp.Matrix([a,delta])\n",
|
|
"\n",
|
|
"#B\n",
|
|
"B=gs.jacobian(U)#.subs({x:0,y:0,theta:0})B="
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 6,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[0, 0],\n",
|
|
"[0, 0],\n",
|
|
"[1, 0],\n",
|
|
"[0, v*(tan(delta)**2 + 1)/L]])"
|
|
]
|
|
},
|
|
"execution_count": 6,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"B"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}1 & 0 & dt \\cos{\\left(\\theta \\right)} & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt \\sin{\\left(\\theta \\right)} & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & \\frac{dt \\tan{\\left(\\delta \\right)}}{L} & 1\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[1, 0, dt*cos(theta), -dt*v*sin(theta)],\n",
|
|
"[0, 1, dt*sin(theta), dt*v*cos(theta)],\n",
|
|
"[0, 0, 1, 0],\n",
|
|
"[0, 0, dt*tan(delta)/L, 1]])"
|
|
]
|
|
},
|
|
"execution_count": 7,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"#A LIN\n",
|
|
"DT = sp.symbols(\"dt\")\n",
|
|
"sp.eye(4)+A*DT"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 8,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\dt & 0\\\\0 & \\frac{dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[ 0, 0],\n",
|
|
"[ 0, 0],\n",
|
|
"[dt, 0],\n",
|
|
"[ 0, dt*v*(tan(delta)**2 + 1)/L]])"
|
|
]
|
|
},
|
|
"execution_count": 8,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"B*DT"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 9,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}dt \\theta v \\sin{\\left(\\theta \\right)}\\\\- dt \\theta v \\cos{\\left(\\theta \\right)}\\\\0\\\\- \\frac{\\delta dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[ dt*theta*v*sin(theta)],\n",
|
|
"[ -dt*theta*v*cos(theta)],\n",
|
|
"[ 0],\n",
|
|
"[-delta*dt*v*(tan(delta)**2 + 1)/L]])"
|
|
]
|
|
},
|
|
"execution_count": 9,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"DT*(gs - A*X - B*U)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# PATH WAYPOINTS AS PARAMETRIZED CURVE"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 10,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import numpy as np\n",
|
|
"from scipy.interpolate import 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(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\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",
|
|
"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 = [path[0,nn_idx+1] - path[0,nn_idx],\n",
|
|
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
|
|
" v /= np.linalg.norm(v)\n",
|
|
"\n",
|
|
" d = [path[0,nn_idx] - state[0],\n",
|
|
" 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=get_nn_idx(state,track)-1 #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(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
|
|
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
|
|
"\n",
|
|
"#fit poly\n",
|
|
"coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\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 coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0\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",
|
|
"# 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",
|
|
"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": [
|
|
"## With SPLINES"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 17,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"(array([-0.39433757, -0.39433757, -0.39433757, -0.39433757, 0.56791288,\n",
|
|
" 1.04903811, 1.67104657, 1.67104657, 1.67104657, 1.67104657]), array([-0.34967937, 0.15467936, -2.19173016, 1.11089663, -8. ,\n",
|
|
" -0.7723291 , 0. , 0. , 0. , 0. ]), 3)\n",
|
|
"[[ 4.64353595 4.64353595 4.64353595 4.64353595 -23.21767974\n",
|
|
" 65.74806776 65.74806776 65.74806776 65.74806776]\n",
|
|
" [ -6.70236682 -6.70236682 -6.70236682 -6.70236682 6.70236682\n",
|
|
" -26.8094673 95.8780974 95.8780974 95.8780974 ]\n",
|
|
" [ 1.57243489 1.57243489 1.57243489 1.57243489 1.57243489\n",
|
|
" -8.10159833 34.85967446 34.85967446 34.85967446]\n",
|
|
" [ -0.34967937 -0.34967937 -0.34967937 -0.34967937 -0.90523492\n",
|
|
" -1.1830127 -0.7723291 -0.7723291 -0.7723291 ]]\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=get_nn_idx(state,track)-1 #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(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
|
|
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
|
|
"\n",
|
|
"#fit poly\n",
|
|
"import scipy\n",
|
|
"from scipy.interpolate import CubicSpline\n",
|
|
"from scipy.interpolate import PPoly,splrep\n",
|
|
"spl=splrep(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:])\n",
|
|
"print( spl)\n",
|
|
"print(PPoly.from_spline(spl).c)\n",
|
|
"#coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\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",
|
|
"\n"
|
|
]
|
|
},
|
|
{
|
|
"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([[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]]) #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": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# COMPUTE ERRORS"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"* **crosstrack error** cte -> desired y-position - y-position of vehicle: this is the value of the fitted polynomial (road curve)\n",
|
|
" \n",
|
|
"$\n",
|
|
"f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
|
|
"$\n",
|
|
"\n",
|
|
"Then for the origin cte = K_3\n",
|
|
" \n",
|
|
"* **heading error** epsi -> desired heading - heading of vehicle : is the inclination of tangent to the fitted polynomial (road curve)\n",
|
|
"\n",
|
|
"The derivative of the fitted 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 for the origin the equation of the tangent in the origin is $y=k2$ \n",
|
|
"\n",
|
|
"epsi = -atan(K_2)\n",
|
|
"\n",
|
|
"in general:\n",
|
|
"\n",
|
|
"$\n",
|
|
"y_{desired} = f(px) \\\\\n",
|
|
"heading_{desired} = -atan(f`(px))\n",
|
|
"$"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 15,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"-0.5808399313875324\n",
|
|
"28.307545725691345\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"#for 0\n",
|
|
"\n",
|
|
"# cte=coeff[3]\n",
|
|
"# epsi=-np.arctan(coeff[2])\n",
|
|
"cte=f(0,coeff)\n",
|
|
"epsi=-np.arctan(df(0,coeff))\n",
|
|
"print(cte)\n",
|
|
"print(np.degrees(epsi))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# ADD DELAY (for real time implementation)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"It is necessary to take *actuation latency* into account: so instead of using the actual state as estimated, the delay factored in using the kinematic model\n",
|
|
"\n",
|
|
"Starting State is :\n",
|
|
"\n",
|
|
"* $x_{delay} = 0.0 + v * dt$\n",
|
|
"* $y_{delay} = 0.0$\n",
|
|
"* $psi_{delay} = 0.0 + w * dt$\n",
|
|
"* $cte_{delay} = cte + v * sin(epsi) * dt$\n",
|
|
"* $epsi_{delay} = epsi - w * dt$\n",
|
|
"\n",
|
|
"Note that the starting position and heading is always 0; this is becouse the path is parametrized to **vehicle reference frame**"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": []
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "Python 3",
|
|
"language": "python",
|
|
"name": "python3"
|
|
},
|
|
"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.3"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 4
|
|
}
|