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