12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

interpolation distance of track wp

master
mcarfagno 2020-05-04 15:16:29 +01:00
parent f1f888ec7c
commit e15702c294
2 changed files with 66 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
notebook/MPC_cte_cvxpy.ipynb
View File

File diff suppressed because one or more lines are too long

40
notebook/equations.ipynb
View File

@ -9,7 +9,7 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 6,
"metadata": {},
"outputs": [
{
@ -26,7 +26,7 @@
"[0, 0, 0, v*cos(psi), 0]])"
]
},
"execution_count": 1,
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
@ -81,22 +81,22 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0\\end{matrix}\\right]$"
"$\\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",
"[0, 0, -v*sin(theta)],\n",
"[0, 0, v*cos(theta)],\n",
"[0, 0, 0]])"
"[1, 0, -dt*v*sin(theta)],\n",
"[0, 1, dt*v*cos(theta)],\n",
"[0, 0, 1]])"
]
},
"execution_count": 1,
"execution_count": 9,
"metadata": {},
"output_type": "execute_result"
}
@ -104,36 +104,36 @@
"source": [
"import sympy as sp\n",
"\n",
"x,y,theta,psi,cte,v,w = sp.symbols(\"x y theta psi cte v w\")\n",
"x,y,theta,psi,cte,v,w ,dt= sp.symbols(\"x y theta psi cte v w dt\")\n",
"\n",
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
" [ sp.sin(theta)*v],\n",
" [w]])\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)"
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
]
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 10,
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & 0\\\\0 & 1\\end{matrix}\\right]$"
"$\\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",
"[cos(theta), 0],\n",
"[sin(theta), 0],\n",
"[ 0, 1]])"
"[dt*cos(theta), 0],\n",
"[dt*sin(theta), 0],\n",
"[ 0, dt]])"
]
},
"execution_count": 2,
"execution_count": 10,
"metadata": {},
"output_type": "execute_result"
}
@ -142,7 +142,7 @@
"state = sp.Matrix([v,w])\n",
"\n",
"#B\n",
"gs.jacobian(state)"
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
]
},
{