2019-11-27 21:15:13 +08:00
{
"cells": [
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 1,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from scipy.integrate import odeint\n",
"from scipy.interpolate import interp1d\n",
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### kinematics model equations\n",
"\n",
"The variables of the model are:\n",
"\n",
"* $x$ coordinate of the robot\n",
"* $y$ coordinate of the robot\n",
"* $\\theta$ heading of the robot\n",
"* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n",
2019-11-28 00:09:38 +08:00
"* $cte$ crosstrack error = lateral distance of the robot from the path \n",
2019-11-27 21:15:13 +08:00
"\n",
"The inputs of the model are:\n",
"\n",
"* $v$ linear velocity of the robot\n",
"* $w$ angular velocity of the robot\n",
"\n",
"These are the differential equations f(x,u) of the model:\n",
"\n",
"* $\\dot{x} = v\\cos{\\theta}$ \n",
"* $\\dot{y} = v\\sin{\\theta}$\n",
"* $\\dot{\\theta} = w$\n",
"* $\\dot{\\psi} = -w$\n",
2019-11-28 00:09:38 +08:00
"* $\\dot{cte} = v\\sin{-\\psi}$\n",
2019-11-27 21:15:13 +08:00
"\n",
"The **Continuous** State Space Model is\n",
"\n",
2019-11-28 00:09:38 +08:00
"$ {\\dot{x}} = Ax + Bu $\n",
2019-11-27 21:15:13 +08:00
"\n",
"with:\n",
"\n",
2019-11-28 00:09:38 +08:00
"$ A =\n",
"\\quad\n",
2019-11-27 21:15:13 +08:00
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} & \\frac{\\partial f(x,u)}{\\partial \\psi} & \\frac{\\partial f(x,u)}{\\partial cte} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"0 & 0 & -vsin(\\theta) & 0 & 0 \\\\\n",
"0 & 0 & vcos(\\theta) & 0 & 0 \\\\\n",
"0 & 0 & 0 & 0 & 0 \\\\\n",
"0 & 0 & 0 & 0 & 0 \\\\\n",
2019-11-28 00:09:38 +08:00
"0 & 0 & 0 & vcos(-\\psi) & 0 \n",
2019-11-27 21:15:13 +08:00
"\\end{bmatrix}\n",
2019-11-28 00:09:38 +08:00
"\\quad $\n",
2019-11-27 21:15:13 +08:00
"\n",
2019-11-28 00:09:38 +08:00
"\n",
"$ B = \\quad\n",
2019-11-27 21:15:13 +08:00
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial w} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"\\cos{\\theta_t} & 0 \\\\\n",
"\\sin{\\theta_t} & 0 \\\\\n",
"0 & 1 \\\\\n",
"0 & -1 \\\\\n",
"\\sin{(-\\psi_t)} & 0 \n",
"\\end{bmatrix}\n",
2019-11-28 00:09:38 +08:00
"\\quad $\n",
2019-11-27 21:15:13 +08:00
"\n",
2019-11-28 00:09:38 +08:00
"discretize with forward Euler Integration for time step dt:\n",
2019-11-27 21:15:13 +08:00
"\n",
"* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}*dt$\n",
"* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}*dt$\n",
"* ${\\theta_{t+1}} = \\theta_{t} + w_t*dt$\n",
"* ${\\psi_{t+1}} = \\psi_{t} - w_t*dt$\n",
"* ${cte_{t+1}} = cte_{t} + v_t\\sin{-\\psi}*dt$\n",
"\n",
2019-11-28 00:09:38 +08:00
"The **Discrete** State Space Model is then:\n",
2019-11-27 21:15:13 +08:00
"\n",
"${x_{t+1}} = Ax_t + Bu_t $\n",
"\n",
"with:\n",
"\n",
"$\n",
"A = \\quad\n",
"\\begin{bmatrix}\n",
"1 & 0 & -v\\sin{\\theta}dt & 0 & 0 \\\\\n",
"0 & 1 & v\\cos{\\theta}dt & 0 & 0 \\\\\n",
"0 & 0 & 1 & 0 & 0 \\\\\n",
"0 & 0 & 0 & 1 & 0 \\\\\n",
2019-11-28 00:09:38 +08:00
"0 & 0 & 0 & vcos{(-\\psi)}dt & 1 \n",
2019-11-27 21:15:13 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
"$\n",
"B = \\quad\n",
"\\begin{bmatrix}\n",
"\\cos{\\theta_t}dt & 0 \\\\\n",
"\\sin{\\theta_t}dt & 0 \\\\\n",
"0 & dt \\\\\n",
"0 & -dt \\\\\n",
"\\sin{-\\psi_t}dt & 0 \n",
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
"This State Space Model is not linear (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
"\n",
"$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
"\n",
"So:\n",
"\n",
2019-11-28 00:09:38 +08:00
"$ x{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u}) )dt $\n",
"\n",
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
"\n",
"The Discrete linearized kinematics model is\n",
"\n",
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
"\n",
"with:\n",
"\n",
"A' = I+dtA\n",
"\n",
"B' = dtB\n",
2019-11-27 21:15:13 +08:00
"\n",
2019-11-28 00:09:38 +08:00
"C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u})"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2019-11-28 00:09:38 +08:00
"------------------\n",
"NB: psi and cte are expressed w.r.t. the reference frame.\n",
2019-11-27 21:15:13 +08:00
"in the reference frame of the veicle the equtions would be:\n",
"* psi_dot = w\n",
2019-11-28 00:09:38 +08:00
"* cte_dot = sin(psi)\n",
"-----------------"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 2,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Control problem statement.\n",
"\n",
"N = 5 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"dt = 0.25 #discretization step\n",
"\n",
"x = cp.Variable((N, T+1))\n",
2019-11-28 00:09:38 +08:00
"u = cp.Variable((M, T))"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 3,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
" \n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" theta = x_bar[2]\n",
" psi = x_bar[3]\n",
" cte = x_bar[4]\n",
" \n",
" v = u_bar[0]\n",
" w = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=-v*np.sin(theta)\n",
" A[1,2]=v*np.cos(theta)\n",
2019-11-28 00:09:38 +08:00
" A[4,3]=v*np.cos(-psi)\n",
2019-11-27 21:15:13 +08:00
" A_lin=np.eye(N)+dt*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[0,0]=np.cos(theta)\n",
" B[1,0]=np.sin(theta)\n",
" B[2,1]=1\n",
" B[3,1]=-1\n",
" B[4,0]=np.sin(-psi)\n",
" B_lin=dt*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)\n",
" C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return A_lin,B_lin,C_lin"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using scipy intergration"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 4,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"def kinematics_model(x,t,u):\n",
"\n",
" dxdt = u[0]*np.cos(x[2])\n",
" dydt = u[0]*np.sin(x[2])\n",
" dthetadt = u[1]\n",
" dpsidt = -u[1]\n",
" dctedt = u[0]*np.sin(-x[3])\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dthetadt,\n",
" dpsidt,\n",
" dctedt]\n",
"\n",
" return dqdt\n",
"\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" x0(5)\n",
" u(2,T)\n",
" \n",
" x_bar(5,T+1)\n",
" \"\"\"\n",
" \n",
" x_bar = np.zeros((N,T+1))\n",
" \n",
" x_bar[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n",
" tspan = [0,dt]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_bar[:,t]=x_next[1]\n",
" \n",
" return x_bar"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Validate the model, here the status w.r.t a straight line with constant heading 0"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 5,
2019-11-27 21:15:13 +08:00
"metadata": {},
2019-11-28 00:09:38 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"CPU times: user 4.05 ms, sys: 38 µs, total: 4.09 ms\n",
"Wall time: 3.75 ms\n"
]
}
],
2019-11-27 21:15:13 +08:00
"source": [
2019-11-28 00:09:38 +08:00
"%%time\n",
"\n",
2019-11-27 21:15:13 +08:00
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(0)\n",
"x0[3] = 0\n",
"x0[4] = 1\n",
"\n",
"x_bar=predict(x0,u_bar)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Check the model prediction"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 6,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2,:]))\n",
"plt.ylabel('theta(t) [deg]')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(np.degrees(x_bar[3,:]))\n",
"plt.ylabel('psi(t) [deg]')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"the results seems valid:\n",
"* the cte is correct\n",
"* theta error is correct"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using the state space model"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 7,
2019-11-27 21:15:13 +08:00
"metadata": {},
2019-11-28 00:09:38 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"CPU times: user 2.53 ms, sys: 2.36 ms, total: 4.9 ms\n",
"Wall time: 1.44 ms\n"
]
}
],
2019-11-27 21:15:13 +08:00
"source": [
2019-11-28 00:09:38 +08:00
"%%time\n",
"\n",
2019-11-27 21:15:13 +08:00
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(0)\n",
"x0[3] = 0\n",
"x0[4] = 1\n",
"\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n",
" \n",
" x_bar[:,t]= np.squeeze(xt_plus_one)"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 8,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAagAAAEYCAYAAAAJeGK1AAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADl0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uIDMuMC4zLCBodHRwOi8vbWF0cGxvdGxpYi5vcmcvnQurowAAIABJREFUeJzt3XeUFMX2wPFv9S45iLCCJAEzmMD8zAERfSr61CuKAQwYCGIERCUoCs8AKCYUBBTDNYL+EHN6PiMqovhUjEQxIKJkun5/9KDLssvusrPTE+7nnD0709PTc6kzzd2urqrrvPcYY4wx6SaIOwBjjDGmOJagjDHGpCVLUMYYY9KSJShjjDFpyRKUMcaYtGQJyhhjTFqyBGWMMSYtWYIyxhiTlixBGWOMSUv5cQdQSWx5DJMsLu4A0pCdXyYZSj23sjVBMX/+/L8eFxQU8PPPP8cYTfqxNtlQ0TZp0qRJjNGUnYh0BEYBecB9qjqsyOvVgInAHsAvwCmq+l3itf7AOcBaoLeqPl+Wzyx8fq1j36mItUNkY+1Q1nPLuviMyWAikgfcARwFtAFOFZE2RXY7B1isqtsCI4Dhife2AToDOwEdgTsTxzMmLViCMiaz7Q3MVtVvVHUV8AjQqcg+nYAJicePA4eLiEtsf0RVV6rqt8DsxPHKzc//gZXv/2eT/gHGlCRru/iMyRFNgTmFns8F9ilpH1VdIyJLgAaJ7e8UeW/T4j5ERLoD3RPHoKCgYL3Xl0y6i99ee47qh/2TOmdfTFCr9qb/izJcfn7+Bu2Ti5LRDpagjDGlUtUxwJjEU1/03oKXc6nVYAv+fPJBVnz8LsFZvXFt2qY8znRg96Aidg/KGDMPaF7oebPEtmL3EZF8YDOiwRJleW+ZuCpVqH36BQT9hkPVaoQjriWcdDd+5YpNOZwxgCUoYzLd+8B2ItJKRKoSDXqYUmSfKcBZiccnAa+oqk9s7ywi1USkFbAd8F5FgnFb70BwzUhc+074158jHNwb/9WsihzS5DBLUMZkMFVdA/QEngc+jzbpZyIyRESOS+w2FmggIrOBS4F+ifd+BigwC5gG9FDVtRWNyVWtRnDKOQSXDQXvCW/qT/jY/fjVqyp6aJNjXJaWfPc2D2rjrE02VMI8KJuouyFf1nlQfsVy/GP349+YBo2bE5zdB9dyuxSFGQ87tyJluAdV6rllV1DGmErjqtcgOOMigosHwfJlhDdeQTh5En7N6rhDMxnAEpQxptK5nXcnGHw7bp+D8c8+SnjD5fi538UdlklzlqCMMSnhatYmOPsSgh5XwW+/El5/KeHUx/BrK3zby2QpS1DGmJRybfclGHwHru0++KceIBzeF79wbtxhmTRkCcoYk3KuTl3c+VfizrscFi0gHNKH8KUp+DCMOzSTRixBGWNi4Zwj2PsggkG3Q+vd8I/eR3jL1fifFsYdmkkTlqCMMbFy9eoT9Lwa17U3/PA14eCLCd+YRpZOgTHlYAnKGBM75xzB/u0JBo2GrbfHP3An4ahB+MW/xB2aiZElKGNM2nANtiDoMxh32vnw1SzCQT0J33nVrqZyVEasZi4izYkqgjYiKjc9RlVHxRuVMaYyuCDAHfpP/E7tCO8fhR87Av/h2wSnX4SrWy/u8EwKZcoV1BrgMlVtA+wL9CimaqgxJou4hk0IrrgBd1I3mDmdcGBP/PS34g7LpFBGJChVXaCqHyYeLyVaFLPYwmrGmOzhgjyCI08guHoENGhIePdwwntvxv+5NO7QTApkRBdfYSLSEmgHvFtke4kVP63C5YasTTZkbZK+XNOtCPr9G//c4/j/exT/xacEZ/bA7bpX3KGZSpRRq5mLSG3gdWCoqj65kV1tNfNSWJtsyFYzL7Myr2ZeKR/+/deE94+Eed/jDjgCJ+fgatSs9M8tKzu3Ijm1mrmIVAGeACaVkpyMMVnMtdiGYMCtuKNOxL/1MuGgXvjPZ8QdlqkEGZGgRMQRFV37XFVvjTseY0y8XJUqBP86i6DvMKhSlfDWawgfusdKzGeZTLkHtT9wBjBTRD5ObLtKVafGGJMxJmZumx0JrhmJf2oi/uVn8J99SNCtD27b1nGHZpIgIxKUqv4HuxdgzHpEpD7wKNAS+A4QVV1cZJ+2wF1AXWAt0f3bRxOvjQcOBpYkdu+qqh+TYVy1arjO5+Hb7kM4/jbCf/fHdeiE69QFV6Vq3OGZCsiILj5jTLH6AS+r6nbAy4nnRS0DzlTVnYCOwEgRKTzb9QpVbZv4ybjkVJjbcVeCQbfhDmiPf/4pwusuwX8/O+6wTAVYgjImc3UCJiQeTwCOL7qDqn6pql8lHs8HFgFbpCzCFHPVaxKc2ZOg90BY/ifhDZcTTnkIv2ZN3KGZTZARXXzGZBoRKQDOBP4J7AZsRtSVNgN4Dpigqj9V8GMaqeqCxOOFREuBbSymvYGqwNeFNg8VkWtJXIGp6soS3lviPMN10moe2aFHEu71D5beN4IVzzxC3mcfslnva8hvsU2lf3RatUOMktEOlqCMSTIRGQZ0AaaSGH0KLAXqAK2J7vt8KCKTVLW4brnCx3oJ2LKYlwYUfqKqXkRKnNQoIo2BB4CzVHVdVcD+RImtKjAG6AsMKe79qjomsQ+AL25+S1rO/zm9B0Gb3Vnz4J38cnk33HFdcEcejwvyKu0j07IdYlCGeVClsgRlTPLNBbYt4WrkI+AhEakOnFvagVS1fUmviciPItJYVRckEtCiEvarC/wfMEBV3yl07HVXXytF5H7g8tLiyURu938QbNeG8ME78U9OwM94l6DrxbgtbbW0dGf3oIxJMlUdvS45iUhxVz8A9VR1dAU/agpwVuLxWcDkojuISFXgKWCiqj5e5LXGid+O6P7VpxWMJ225OpsRXNAPd+5lsGAO4XUXE778rJWYT3N2BWVM5fqSaIh3UbOA+hU89jBAReQc4HtAAERkT+ACVT03se0goIGIdE28b91w8kkisgXRFI6PgQsqGE9ac87h9jkYv8POhBNG4x8Zg//obYKuvXEFG719Z2KSUWvxlcNfa4WF990CM6f/XfDMAVWqQbVqULU61KqNq7MZ1K0X/RQ0ir6sWzSCOvVwLjunX1k/+YYqYy0+EVmqqnWKbKsLfKOqmXonPda1+JLBe4//z4v4R8cC4E45J1rXLwnneya1Q2VKxlp82X8FteOu1NiiEcuXL4+eew+rV8HKFdGyKH/+gZ/zLfz+Gyz/M9pl3Xtr1oImLXDNWkCzVritd4CmW1XqDVaTHURkDtFXqYaI/FDk5QbAw6mPyqzjnMMd2AHfejfC8bfhJ46OiiKe1RNXr0Hc4ZmErE9QwQFHUKeggJVl+IvGr1wJvy6Cnxbif1oIC+bg532Pf/d1eO25KHFVqwFbb4/bfmdcm7bQYltcniUss4HTif5CnEq0TNc6HvhRVb+IJSqzHlfQiODS6/CvTsU/OZ5wYC/caefj9j4oa3tPMknWd/FBxS+5vfdR0vrmf/D1F/jZn8Pcb6MXa9SKElW7fXG77ImrWauisaeEdUNsqJK6+Gqq6rIKhpZuMr6Lrzh+4byojMc3X8Du+xGcfmHU/V9Omd4OyWJdfCninIOGjXENG8O+hwLgly7B/+8TmPUx/pP3Yfpb+Lx8aL0bbp+Dce32xVWrHnPkJg4i0hu4R1VXlpScRKQacL6q3pba6ExJ3JZNCfoOw7/wNH7yJMKBnxGcfhFu93/EHVrOsgS1iVydzXB7HQh7HRgNVf3mC/xH7+A/+A9+7K34ajVwu++LO+hI2Ka1dRfkli2B2SIylajA5hf8PVF3e+AQ4ChgYlwBmuK5IA/X8UT8LnsSjhtBeNeNuH0PwXXujqtVO+7wco518SX7g8MQvpqFf/c1/Af/geXLoFlL3CFHR1/0NLmqsm6IDSWziy+x1FFXokS0C1APWAx8QnRfaqKq/lKxiGOTlV18Rfk1a/BTFf9/CnXrEZzZC7fLHqW+L9vaYVMlo4vPElRlBrFiOf691/GvToW530HtOrjDjsUdejSudnFTY1LHTqINWcn3MsuJBLWO/3424bi
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2,:])\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3,:])\n",
"plt.ylabel('psi(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The results are the same as expected"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2019-11-28 00:09:38 +08:00
"------------------\n",
"\n",
"the kinematics model predictits psi and cte for constant heading references. So, for non-constant paths appropriate functions have to be developed.\n",
"\n",
"-----------------"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 9,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"def calc_err(state,path):\n",
" \"\"\"\n",
" Finds psi and cte w.r.t. the closest waypoint.\n",
"\n",
" :param state: array_like, state of the vehicle [x_pos, y_pos, theta]\n",
" :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]\n",
" :returns: (float,float)\n",
" \"\"\"\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",
"# front_axle_vect = [np.cos(state[2] - np.pi / 2),\n",
"# np.sin(state[2] - np.pi / 2)]\n",
" path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),\n",
" np.sin(path[2,target_idx] + np.pi / 2)]\n",
" \n",
" #heading error w.r.t path frame\n",
" psi = path[2,target_idx] - state[2]\n",
" \n",
" # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor\n",
" #cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)\n",
" cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect)\n",
"\n",
" return target_idx,psi,cte\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" '''\n",
" Interpolation range is computed to assure one point every fixed distance step [m].\n",
" \n",
" :param start_xp: array_like, list of starting x coordinates\n",
" :param start_yp: array_like, list of starting y coordinates\n",
" :param step: float, interpolation distance [m] between consecutive waypoints\n",
" :returns: array_like, of shape (3,N)\n",
" '''\n",
"\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,section_len/delta)\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",
" dx = np.append(0, np.diff(final_xp))\n",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"test it"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 10,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"track = compute_path_from_wp([0,5],[0,0])\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(0)\n",
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\n",
"\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" \n",
" _,psi,cte = calc_err(xt_plus_one,track)\n",
" xt_plus_one[3]=psi\n",
" xt_plus_one[4]=cte\n",
" \n",
" x_bar[:,t]= xt_plus_one"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 11,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(track[0,:],track[1,:],\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2,:])\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3,:])\n",
"plt.ylabel('psi(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 12,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"track = compute_path_from_wp([0,2,4,5,10],[0,0,3,1,1])\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(10) #rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 2\n",
"x0[1] = 2\n",
"x0[2] = np.radians(0)\n",
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\n",
"\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" \n",
" _,psi,cte = calc_err(xt_plus_one,track)\n",
" xt_plus_one[3]=psi\n",
" xt_plus_one[4]=cte\n",
" \n",
" x_bar[:,t]= xt_plus_one"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 13,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(track[0,:],track[1,:],\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2,:])\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3,:])\n",
"plt.ylabel('psi(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"MPC Problem formulation\n"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 18,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2019-11-28 00:09:38 +08:00
"CPU times: user 289 ms, sys: 617 µs, total: 290 ms\n",
"Wall time: 287 ms\n"
2019-11-27 21:15:13 +08:00
]
}
],
"source": [
2019-11-28 00:09:38 +08:00
"%%time\n",
"\n",
2019-11-27 21:15:13 +08:00
"MAX_SPEED = 1.2\n",
"MIN_SPEED = 0.8\n",
"MAX_STEER_SPEED = 0.57\n",
"\n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:]=1\n",
"u_bar[1,:]=0.01\n",
"\n",
"track = compute_path_from_wp([0,10],[2,2])\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
2019-11-28 00:09:38 +08:00
"x0[0] = 2\n",
"x0[1] = -0.5\n",
2019-11-27 21:15:13 +08:00
"x0[2] = np.radians(-30)\n",
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\n",
"\n",
"# Prediction\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" \n",
" _,psi,cte = calc_err(xt_plus_one,track)\n",
" xt_plus_one[3]=psi\n",
" xt_plus_one[4]=cte\n",
" \n",
" x_bar[:,t]= xt_plus_one\n",
"\n",
"\n",
"#CVXPY MPC problem statement\n",
"cost = 0\n",
"constr = []\n",
"\n",
"for t in range(T):\n",
" \n",
" # Cost function\n",
" #cost += 10*cp.sum_squares( x[3, t]) # psi\n",
" #cost += 50*cp.sum_squares( x[4, t]) # cte\n",
" \n",
" # Tracking\n",
" idx,_,_ = calc_err(x_bar[:,t],track)\n",
" delta_x =track[:,idx]-x[0:3,t]\n",
" cost+= cp.quad_form(delta_x,10*np.eye(3))\n",
" #cost += cp.quad_form( x[:, t],10*np.eye(N))\n",
" \n",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 1*np.eye(M))\n",
" \n",
" #constrains\n",
" A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n",
" constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()]\n",
" \n",
"# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x0]\n",
"constr += [u[0, :] <= MAX_SPEED]\n",
"constr += [u[0, :] >= MIN_SPEED]\n",
"constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
"\n",
"\n",
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
2019-11-28 00:09:38 +08:00
"solution = prob.solve(solver=cp.ECOS, verbose=False)"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Print Results:"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 19,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
2019-11-28 00:09:38 +08:00
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAagAAAEYCAYAAAAJeGK1AAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADl0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uIDMuMC4zLCBodHRwOi8vbWF0cGxvdGxpYi5vcmcvnQurowAAIABJREFUeJzt3XmcHFW5//FP9cxkQvaEQUISEkgIS0SJoqAsiqLIIgS98hjUH3BdwCWCgldFZZF7veICyhWuEAMCisijIqDiRQUUriwGkFyEyBYCCSHLZDLZM8lMn98fVRM6k9mnu6u65/t+vfrVtZyperqmu58+VafOiUIIiIiIZE0u7QBEREQ6owQlIiKZpAQlIiKZpAQlIiKZpAQlIiKZpAQlIiKZVJt2ACKSPWZ2HfBeYKW7HzjAbb0D+F7Bov2B2e5+Wy/+djTwU2Ay8ffVd939xwOM5xTgYuAA4BB3f2Qg25PSUQ1KRDpzPXBsMTbk7ve6+0x3nwm8E9gE/KFjOTNb3MmffwZ4yt0PAo4CLjOzIQMM6R/A+4H7BrgdKTHVoERkJ+5+n5ntVbjMzKYBVwG7ESeZT7j7P/u46Q8Av3f3Tb0sH4CRZhYBI4AmoDWJ598AA+qBX7v7Rb3ZoLsvTP6+j6FLuakGJSK9NRf4rLsfDHwB+O9+bGM2cHMfyl9JfCpuGfAEcI67583sGGA6cAgwEzjYzN7Wj3gkw1SDEpEemdkI4DDgFwU1j/pk3fuBSzr5s5fd/T0F29gDeB1wV8Gyq4DDk9kJZvZ4Mv0Ld/8G8B7gceJTg9OAP5rZ/cAxyePvSfkRxAnrPjP7EzC+k3i+6u639+V1S7qUoESkN3JAc3IdaQfufitway+2YcSn4rYV/O1ntq80W9zJ9v8VuNTdA/Ccmb1A3MgiAr7p7td0Es+7evOCJPt0ik9EeuTu64AXkhZwmFlkZgf1cTOn0rfTewAvAUcn+9wd2A9YRFwL+2hSs8PMJprZa/q4bck41aBEZCdmdjNxq7kGM1sKXAR8GPihmX0NqAN+Dizo5fb2AvYE/tLHUP4duN7MniCuNX3J3RuBP5jZAcCDySnHDcBHgJW9iOV9wA+IG3v8zsweLzwVKdkRabgNERHJooqsQZlZDfAI8UXY96Ydj4iIFF9FJijgHGAhMKqHcqoeSilFaQdQIvrcSKn06TNTcQnKzCYBJwDfAM7tqfyyZcs6Xd7Q0EBjY2Nxg+uHrMQB2YmlEuKYMGFCmaMpr84+N1n5v7RTPN3LWjz9+cxUXIICvg98ERjZ2UozOxM4E8DdaWho6HQjtbW1Xa4rp6zEAdmJRXGICFRYgjKz9s4rHzWzozor4+5zie94Bwhd/YLIyq+LrMQB2YmlEuKo9hqUSF+Fli1QV0eUqynaNivtPqjDgZOSTiV/DrzTzH6abkgiIhJuvIr8BZ8u6jYrqgbl7ucD5wMkNagvuPtHUg1KREQIa5tg1NiibrPSalAiIpJFa1YTjRlX1E1WVA2qkLv/GfhzymGIiAx6IQRY2wRj3lzU7aoGJSIiA7N5E7RsgSLXoJSgRERkYNY2xc9KUCIikinNcYKKxuxa1M0qQYmIyICENavjCdWgREQkU3SKT0REMmnNathlOFH90KJuVglKREQGJKxtKnrtCZSgRERkoNasVoISEZEMWttU9BZ8oAQlIiIDEPJ5WLtGNSgREcmYDWuhrQ3GqgYlIiJZsia5SXe0alAiIpIlSS8SqkGJiEimhOakFwnVoEREJFOamyCKYNSYom9aCUpERPqveTWMGkNUW/zhBZWgRESk30JzU0lO74ESlIiIDERzU0kaSECFDfluZnsCNwK7AwGY6+5XpBuViMgg1ryaaOp+Jdl0pdWgWoHz3H0G8BbgM2Y2o68bWbEix9FH1/Lkk7WceOKuHHtsAyee2LB9vnC647q+lO3NdhYsoKj7HEh8Rx5Zncekv9s58shaVq6stI+ISPmEbdtgwzoYW5pTfFEIoSQbLgczux240t3/2EWRsGzZsp0Wnn/+KH760+Hss882nnmmbvvyffd9db5wuqf5/q4DOOCAwMKFUVH3Wcz4quWY9Hc7p522kW9+cx0dTZgwASDaaUV16PRz09DQQGNjYwrhdE7xdK8c8YTGFeTP/wTRaXPIHXlMt2X785mp2ARlZnsB9wEHuvvO3yCxHT5oU6fuQUtLtX6nSCnV1wcWLXpl+7wSVPoUT/fKkqCeW0j+W18id85FRAce3G3Z/nxmKuoaVDszGwH8Cvhcx+RkZmcCZwK4Ow0NDdvXPf30Nr785Rpuuy3Hli0R8WWswuNVON/dur6ULdZ2qim+ytlnTU3gxBPzXHFF2w7vJREhbmIOJekoFiowQZlZHXFyusndb+243t3nAnOT2VD4C6KuDurqRtPSMoyamkBbG8RfRoVCF9MDKVus7VRTfJWxz7Y2GDVqM7W16yj8MZr8GhQZ1EJ7N0clGGoDKqyRhJlFwLXAQne/vD/baGzMcdppGznyyMCIEXnq6wNTprRSXx+oqYHx49sYP76Nmhp2WteXssXaTjnjGzqUsu8zjdfZm+2MH9/GBz6QZ/LkNlatqinuG1mkWjSvhto6GD6yJJuvtBrU4cD/A54ws8eTZV9x9zt7u4F589YA0NAwJBPni7N03jorsSgOkQrRHA/1HkWluRxbUQnK3f+X6r0wLSJSUUKSoEqlok7xiYhIhjQ3lWQcqHZKUCIi0mchhPgaVIm6OQIlKBER6Y8tm6Fli07xiYhIxpS4iTlUWCMJEXmVmR0LXAHUAPPc/dIO688AvgO8nCy60t3nlTVIqV7JTbpRCWtQSlAiKUp6RRkDNLv7hj78XQ1wFfBuYCkw38zucPenOhS9xd3nFC1gkUSpb9IFJSiRsjOzA4GzgBOAKcS3TgQzWwz8HrjG3Z/oYTOHAM+5+6Jkmz8HZgEdE5RIaZS4myNQghIpqySRzAB+DnwEWAisB0YCBwBvB24ys6fcfXY3m5oILCmYXwoc2km5fzGztwHPAJ939yWdlOm2D8t2tbW1meqPUPF0r9TxrGvZxJZhI9ht4qSS7UMJSqS8furuv+1k+RrggeTxTTN7bxH29RvgZndvMbOzgBuAd3ZWsLs+LNtlrWcNxdO9UsfT9srLMHpsr/fRn/4r1YpPpIwKk5OZdVbjwcwO6SKJFXoZ2LNgfhKvNoZo39dqd29JZucB3Y+HINIXJe5FApSgRNLU1UCb/9OLv50PTDezvc1sCDAbuKOwgJntUTB7EvHpRJHiaF5d0hZ8UORTfGb2PeAGd3+8x8KSKSEE2hpXEJ75J2FtE6xtgo0b4semDUQf/hTRLsPSDrMqmFmOuGFElPTQX9i/5DSgtadtuHurmc0B7iJuZn6duz9pZpcAj7j7HcDZZnZSsr0m4IzivhIZrEI+D2vXlLQFHxT/GlQNcJeZrQJ+Qjxm09Ii70OKIGzZDM8+RVj4OGHR07DsJRo3b9qxUBTBLsNh2HCizZtACapYWnl18KmOySgPfKM3G0l68b+zw7ILC6bPB87vf5giXdiwNh4srZJqUO5+tpl9HjgO+DDwNTN7GLgRuLUv93lI8YUQ4OknCPf/gfDYg9C6LR7LZeq+RG85ihH7zmDjLiPjX0VjxsIuw4lyOgtcAnsT15r+ArytYHkAVrn75lSiEumt5B6oijrFB+DubcBvgd+a2WuBnwHXA/+dNLG9yN1f7mYTUgJh4QLyfh0sfSGuER15DNHMQ2GfA4iG1AMwrKGBTRlqhVSt3P3FZHJKqoGI9Nea0t+kCyVIUGY2CjiF+B6P1xMPz/5p4CXgPOIbEV9f7P1K58K6ZvI3XgkL/ga7vobojHOI3nzE9qQk5WVmlwPfdvfl3ZQZD3zR3c8tX2QivRfWlv4mXSh+I4lfAu8B7gOuBm4raOaKmZ0LrC3mPqVr4dmnyM/9NmzcQPT+04nedSJR3ZC0wxrsngb+ZmYLiU/xPc2rN+ruCxwF7Af8R1oBivRoTVN8jXrU2JLuptg1qIeAOV39OnT3vJntXuR9Sify8+8nXHs57Poacmd
2019-11-27 21:15:13 +08:00
"text/plain": [
"<Figure size 432x288 with 3 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
"theta_mpc=np.array(x.value[1, :]).flatten()\n",
"psi_mpc=np.array(x.value[1, :]).flatten()\n",
"cte_mpc=np.array(x.value[1, :]).flatten()\n",
"v_mpc=np.array(u.value[0, :]).flatten()\n",
"w_mpc=np.array(u.value[1, :]).flatten()\n",
"\n",
2019-11-28 00:09:38 +08:00
"#simulate robot state trajectory for optimized U\n",
2019-11-27 21:15:13 +08:00
"x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n",
"\n",
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(track[0,:],track[1,:],\"b*\")\n",
"plt.plot(x_traj[0,:],x_traj[1,:])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"#plot v(t)\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(v_mpc)\n",
"plt.ylabel('v(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"#plot w(t)\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(w_mpc)\n",
"plt.ylabel('w(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"#plot theta(t)\n",
"#plt.subplot(2, 2, 4)\n",
"#plt.plot(cte_mpc)\n",
"#plt.ylabel('cte(t)')\n",
"#plt.xlabel('time')\n",
"#plt.legend(loc='best')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"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.5.2"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
