2019-11-27 21:15:13 +08:00
{
"cells": [
{
"cell_type": "code",
2020-04-22 22:32:10 +08:00
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": 2,
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",
2020-04-20 23:48:53 +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",
2020-04-20 23:48:53 +08:00
"-\\sin{(-\\psi_t)} & 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
"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",
2020-04-20 23:48:53 +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",
2020-04-20 23:48:53 +08:00
"-\\sin{-\\psi_t}dt & 0 \n",
2019-11-27 21:15:13 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
2020-03-04 20:32:29 +08:00
"This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
2019-11-27 21:15:13 +08:00
"\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",
2020-03-04 20:32:29 +08:00
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x_t-\\bar{x}) + B(u_t-\\bar{u}) )dt $\n",
2019-11-28 00:09:38 +08:00
"\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",
2019-11-29 22:24:08 +08:00
"$ A' = I+dtA $\n",
2019-11-28 00:09:38 +08:00
"\n",
2019-11-29 22:24:08 +08:00
"$ B' = dtB $\n",
2019-11-27 21:15:13 +08:00
"\n",
2019-11-29 22:24:08 +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",
2019-11-29 22:24:08 +08:00
"NB: psi and cte are expressed w.r.t. the track as reference frame.\n",
"In the reference frame of the veicle the equtions would be:\n",
2019-11-27 21:15:13 +08:00
"* 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
]
},
2020-03-04 20:32:29 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"-----------------\n",
"[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
"\n",
"In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
"\n",
"Typically it is possible to assume that the system is operating about some nominal\n",
"state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
"\n",
"Recall that the Taylor Series expansion of f(x) around the\n",
"point $\\bar{x}$ is given by:\n",
"\n",
"$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
"\n",
"For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
"\n",
"The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
"is given by:\n",
"\n",
"$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
"\n",
"Where:\n",
"\n",
"$ A =\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"$ and $ B = \\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
"\\end{bmatrix}\n",
"\\quad $\n",
"\n",
"Then:\n",
"\n",
"$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"-----------------"
]
},
2019-11-28 01:46:53 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Kinematics Model"
]
},
2019-11-27 21:15:13 +08:00
{
"cell_type": "code",
2020-04-22 22:32:10 +08:00
"execution_count": 3,
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",
2020-04-22 22:32:10 +08:00
"execution_count": 4,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
" \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",
2020-04-20 23:48:53 +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",
2020-04-20 23:48:53 +08:00
" B[4,0]=-np.sin(-psi)\n",
2019-11-27 21:15:13 +08:00
" 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",
2020-04-22 22:32:10 +08:00
"execution_count": 5,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"def kinematics_model(x,t,u):\n",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
"\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",
" \"\"\"\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",
2020-04-22 22:32:10 +08:00
"execution_count": 6,
2019-11-27 21:15:13 +08:00
"metadata": {},
2019-11-28 00:09:38 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-04-22 22:32:10 +08:00
"CPU times: user 7.87 ms, sys: 0 ns, total: 7.87 ms\n",
"Wall time: 7.41 ms\n"
2019-11-28 00:09:38 +08:00
]
}
],
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",
2020-04-22 22:32:10 +08:00
"execution_count": 7,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-04-20 23:48:53 +08:00
"image/png": "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
2019-11-27 21:15:13 +08:00
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
2020-04-20 23:48:53 +08:00
"metadata": {},
2019-11-27 21:15:13 +08:00
"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",
2020-04-22 22:32:10 +08:00
"execution_count": 8,
2019-11-27 21:15:13 +08:00
"metadata": {},
2019-11-28 00:09:38 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-04-22 22:32:10 +08:00
"CPU times: user 1.8 ms, sys: 650 µs, total: 2.45 ms\n",
"Wall time: 1.79 ms\n"
2019-11-28 00:09:38 +08:00
]
}
],
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",
2020-04-22 22:32:10 +08:00
"execution_count": 9,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-04-20 23:48:53 +08:00
"image/png": "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
2019-11-27 21:15:13 +08:00
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
2020-04-20 23:48:53 +08:00
"metadata": {},
2019-11-27 21:15:13 +08:00
"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",
2020-04-22 22:32:10 +08:00
"execution_count": 10,
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",
2020-04-21 23:33:00 +08:00
" #psi = -path[2,target_idx] + state[2]\n",
2019-11-27 21:15:13 +08:00
" \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",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
" 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",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
"\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",
2020-04-20 23:48:53 +08:00
" interp_range = np.linspace(0,1,int(section_len/delta))\n",
2019-11-27 21:15:13 +08:00
" \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",
2020-04-22 22:32:10 +08:00
"execution_count": 11,
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",
2020-04-22 22:32:10 +08:00
"execution_count": 12,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-04-20 23:48:53 +08:00
"image/png": "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
2019-11-27 21:15:13 +08:00
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
2020-04-20 23:48:53 +08:00
"metadata": {},
2019-11-27 21:15:13 +08:00
"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",
2020-04-22 22:32:10 +08:00
"execution_count": 13,
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",
2020-04-22 22:32:10 +08:00
"execution_count": 14,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-04-20 23:48:53 +08:00
"image/png": "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
2019-11-27 21:15:13 +08:00
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
2020-04-20 23:48:53 +08:00
"metadata": {},
2019-11-27 21:15:13 +08:00
"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": [
2019-11-29 22:24:08 +08:00
"### MPC Problem formulation\n",
"\n",
"**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n",
"\n",
"The controller generates a control signal over a fixed lenght T (Horizon) at each time step."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
2020-03-04 20:32:29 +08:00
"\n",
2019-11-29 22:24:08 +08:00
""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"#### Linear MPC Formulation\n",
"\n",
"Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n",
"\n",
"$x_{t+1} = Ax_t + Bu_t$\n",
"\n",
"The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n",
"\n",
"$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n",
"\n",
"The objective function used minimize (drive the state to 0) is:\n",
"\n",
2020-04-07 23:45:11 +08:00
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n",
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
2020-04-07 23:51:12 +08:00
" & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
2020-04-07 23:45:11 +08:00
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
2020-03-04 20:32:29 +08:00
"\n",
"Other linear constrains may be applied,for instance on the control variable:\n",
"\n",
2020-04-07 23:51:12 +08:00
"$ U_{MIN} < u_{j|t} < U_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 $\n",
2020-03-04 20:32:29 +08:00
"\n",
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
2019-11-29 22:24:08 +08:00
"\n",
2020-03-04 20:32:29 +08:00
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
2019-11-29 22:24:08 +08:00
"\n",
2020-04-07 23:45:11 +08:00
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
2019-11-29 22:24:08 +08:00
"\n",
"where the error w.r.t desired state is accounted for:\n",
"\n",
2020-04-07 23:45:11 +08:00
"$ \\delta x = x_{j,t,ref} - x_{j,t} $\n",
2019-11-29 22:24:08 +08:00
"\n",
"#### Non-Linear MPC Formulation\n",
"\n",
"In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
"\n",
2020-04-07 23:45:11 +08:00
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\min_{} \\quad & \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
2020-04-07 23:51:12 +08:00
" & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
2020-04-07 23:45:11 +08:00
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
2019-11-29 22:24:08 +08:00
"\n",
"Other nonlinear constrains may be applied:\n",
"\n",
2020-04-07 23:51:12 +08:00
"$ g(x_{j|t},{j|t})<0 \\quad \\textrm{for} \\quad t<j<t+T-1 $"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "code",
2020-04-22 22:32:10 +08:00
"execution_count": 28,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-04-22 22:32:10 +08:00
"CPU times: user 219 ms, sys: 3.99 ms, total: 223 ms\n",
"Wall time: 217 ms\n"
2019-11-27 21:15:13 +08:00
]
}
],
"source": [
2019-11-28 00:09:38 +08:00
"%%time\n",
"\n",
2020-04-20 23:48:53 +08:00
"track = compute_path_from_wp([0,10],[0,0])\n",
2019-11-28 01:46:53 +08:00
"\n",
"MAX_SPEED = 2.5\n",
"MIN_SPEED = 0.5\n",
"MAX_STEER_SPEED = 1.57/2\n",
2019-11-27 21:15:13 +08:00
"\n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
2019-11-28 01:46:53 +08:00
"u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n",
2019-11-27 21:15:13 +08:00
"u_bar[1,:]=0.01\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
2019-11-28 01:46:53 +08:00
"x0[0] = 0\n",
2020-04-20 23:48:53 +08:00
"x0[1] = 1\n",
2019-11-28 01:46:53 +08:00
"x0[2] = np.radians(-0)\n",
2019-11-27 21:15:13 +08:00
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\n",
"\n",
2020-04-22 22:32:10 +08:00
"# Linearized Model Prediction\n",
2019-11-27 21:15:13 +08:00
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"\n",
"for t in range (1,T+1):\n",
2020-04-20 23:48:53 +08:00
" xt=x_bar[:,t].reshape(5,1)\n",
2019-11-27 21:15:13 +08:00
" 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",
" x_bar[:,t]= xt_plus_one\n",
"\n",
"\n",
2019-11-28 01:46:53 +08:00
"#CVXPY Linear MPC problem statement\n",
2019-11-27 21:15:13 +08:00
"cost = 0\n",
"constr = []\n",
"\n",
"for t in range(T):\n",
" \n",
2020-04-22 22:32:10 +08:00
" # Tracking\n",
" if t > 0:\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",
"\n",
" # Tracking last time step\n",
" if t == T:\n",
" idx,_,_ = calc_err(x_bar[:,t],track)\n",
" delta_x = track[:,idx]-x[0:3,t]\n",
" cost+= cp.quad_form(delta_x,100*np.eye(3))\n",
"\n",
2019-11-27 21:15:13 +08:00
" # Actuation rate of change\n",
" if t < (T - 1):\n",
2020-04-22 22:32:10 +08:00
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M))\n",
2019-11-27 21:15:13 +08:00
" \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",
2020-04-22 22:32:10 +08:00
"solution = prob.solve(solver=cp.OSQP, verbose=False)"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Print Results:"
]
},
{
"cell_type": "code",
2020-04-22 22:32:10 +08:00
"execution_count": 29,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
2020-04-20 23:48:53 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
"No handles with labels found to put in legend.\n"
]
},
2019-11-27 21:15:13 +08:00
{
"data": {
2020-04-22 22:32:10 +08:00
"image/png": "iVBORw0KGgoAAAANSUhEUgAABDAAAALICAYAAACJhQBYAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjMsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+AADFEAAAgAElEQVR4nOzde5yOdeL/8ffnmnsODIZxk8aQmBSFMk45lMZov5s2pX46bHRgS2yl7axSiWlKUrZ0oBBJ2WxHVntPByUZMlRsipQYYhzHYTBzfX5/TM3WIqf7nus+vJ6Ph4e5r7nuz/X+fMbm3rfrYKy1VgAAAAAAAGHM8ToAAAAAAADAoVBgAAAAAACAsEeBAQAAAAAAwh4FBgAAAAAACHsUGAAAAAAAIOxRYAAAAAAAgLDn8zoAAABAuBs7dqwWLVqklJQUjRo16pjG+uqrrzRp0qSK14WFhbr55pvVrl27wx5jxYoVuvfeezV48GB16NBhv+9/+umnmjFjhlzXVevWrXXllVdKkjZu3KhnnnlG27dvV7Vq1XTjjTeqdu3akqQpU6aooKBAknTxxRerY8eOFXknT56s0tJSnXjiibrhhhsUFxd31POXpFdeeUVz5szRjh07NHny5GMaCwAQOzgDAwAA4BC6du2qIUOGBGWs0047TSNHjtTIkSN1//33KyEhQa1atdpvv0GDBh3w/a7r6uWXXz7geySpuLhYkydP1tChQ/X4449r69at+vLLLyVJkydP1llnnaXHHntMl1xyiaZOnSpJWrRokVatWqVHH31UI0aM0Ntvv61du3bJdV09/fTTuvnmmzVq1CjVqVNHH3300TGvQWZmpnJyco55HABAbKHAAAAAOITmzZurWrVqv9m2fv16jRgxQnfeeaeGDh2qtWvXHvG4n332mc444wwlJiYe9ntmzZql9u3bq0aNGgf8/k8//aTjjz++4vstW7bU/PnzJUlr1qzRaaedJkk69dRTtXDhwortzZo1U1xcnJKSktSwYUMtXrxYO3bskM/nU1pa2n5jlZSUaOzYsbr77rt1xx13aMGCBYc9h6ZNm6pWrVqHvT8AABIFBgAAwFF5/vnnde211+qRRx5Rnz59NH78+CMeY+7cuerUqdNh779582bl5+fr3HPPPeg+9erVU2FhoTZs2KCysjLl5+erqKhIknTCCScoPz9fkpSfn6/du3eruLhYJ5xwgpYsWaI9e/Zo+/btWrp0qTZt2qTq1aurrKxMK1eulFReuPwy1owZM3Taaafp4Ycf1v33368pU6aopKTkiNcAAIDDxT0wAAAAjlBJSYmWL1+uxx9/vGJbaWmpJGn+/Pl67bXX9ntPamqq7rnnnorXW7Zs0erVq39zKcj48eO1fPlySeVlxe233y5JOvPMM9WrVy9NnDhRf/7zn+U4B/83qGrVqql///564oknZIzRySefrPXr10uS+vTpoxdffFEffvihmjVrptTUVDmOo1atWmnlypW69957VaNGDTVt2lSO48gYo8GDB2vSpEnat2+fWrVqVXHsL774Qp9//rnefvttSdLevXtVVFSkpKQkPfzwwwfMds899yg1NfXQCwwAwAFQYAAAABwh13WVnJyskSNH7ve99u3bq3379occY968eWrXrp18vv9+HOvfv3/F14MGDdpv/JUrV+rJJ5+UJG3fvl0FBQVyHGe/G4C2adNGbdq0kSQFAoGK0iE1NVW33XabpPISZv78+UpOTpYk9erVS7169ZIkPfnkkzr++OMllV/uMWzYMEnSkiVLVFhYKEmy1urWW2+tuLzk1471RqcAABwIl5AAAAAcoapVq6pu3bqaN2+epPL/M//9998f0RhHevmIJD399NMVvzp06KD+/fsf8Okl27ZtkyTt2LFDs2fPVlZWlqTy0sN1XUnSP//5T51zzjmSyguZ4uJiSdIPP/zwmzNDfhlr3759evPNNysuX2nVqpVmzZola60kadWqVUc0FwAAjhRnYAAAABzCE088oWXLlqm4uFgDBgxQ7969ddNNN2ncuHGaMWOGSktL1alTJzVq1OiwxtuwYYOKiorUvHnzoGW8/fbbK87YmDBhgn744QdJ0iWXXFJxlsSyZcs0depUGWPUrFkz9evXT1L55S9Dhw6VVF7O3HjjjRWPSn3rrbe0aNEiua6rc889t+ImoJdccokmTpyo2267TdZa1a1bV3fddddhZZ0yZYo++eQT7d27VwMGDFBWVpZ69+4dtLUAAEQnY3+pzQEAAAAAAMIUl5AAAAAAAICwF5EFhuu6uuOOO5Sbm+t1FAAAAAAAUAki8h4YM2fOVP369bV79+5D7vvLnbKDxe/3Vzz/HMHF2oYW6xs6rG1osb6hE4q1PdATKaJBsD9PSLHxZzsW5ijFxjyZY3RgjtEj2ud5sM8TEXcGxqZNm7Ro0SJ169bN6ygAAAAAAKCSRFyBMXHiRF155ZUyxngdBQAAAAAAVJKIuoTk888/V0pKiho3bqylS5cecJ9AIKBAICBJys3Nld/vD2oGn88X9DFRjrUNLdY3dFjb0GJ9Q4e1BQAAkSSiCozly5dr4cKFKigo0N69e7V7926NGTNGN910U8U+2dnZys7Orngd7OuCov1aIy+xtqHF+oYOaxtarG/ocA8MAAAQSSKqwLjiiit0xRVXSJKWLl2qt99++zflBQAAAAAAiE4Rdw8MAAAAAAAQeyLqDIxfO/XUU3Xqqad6HQMAAAAAAFQCzsAAAAAAAABhjwIDAAAAAACEPQoMAAAAAAAQ9igwAAAAAABA2KPAAAAAAAAAR8SuXS27b1+lHpMCAwAAAAAAHDZbskvu6KGyL46u1ONSYAAAAAAAgMNm350ubdssk31BpR6XAgMAAAAAABwW+1Oh7L/flDkzS6bJKZV6bAoMAAAAAABwWNxXx0vx8TIXX1Xpx6bAAAAAAAAAh2S/WCB9uVDmT5fJpNSq9ONTYAAAAAAAgN9l9+0rP/uiXrpM1vmeZKDAAAAAAAAAv8sG3pQ2rJNz2V9kfPGeZPB5clQAAIAgGTt2rBYtWqSUlBSNGjVKkrRjxw6NHj1aGzduVJ06dXTLLbeoWrVqstZqwoQJKigoUGJiogYOHKjGjRt7PAMAAMKb3bJJ9t3XpNPby5x6hmc5OAMDAABEtK5du2rIkCG/2fbGG2+oRYsWGjNmjFq0aKE33nhDklRQUKD169drzJgxuu666zR+/HgvIgMAEFHs6xOlsjI5vft5moMCAwAARLTmzZurWrVqv9m2YMECnX322ZKks88+WwsWLJAkLVy4UGeddZaMMWratKl27typLVu2VHpmAAAihf12mez8j2T+cJFMnXqeZuESEgAAEHW2bdumWrXK745es2ZNbdu2TZK0efNm+f3+iv1q166tzZs3V+z7i0AgoEAgIEnKzc39zXuCxefzhWTccBILc5RiY57MMTowx+hRWfO0ZWXaPP0FyX+c/H0GyCQmhfyYv4cCAwAARDVjjIwxR/Se7OxsZWdnV7wuKioKdiz5/f6QjBtOYmGOUmzMkzlGB+YYPSprnu6Hs2RXfStz3R3aVLxDKt4R8mNKUlpa2gG3cwkJAACIOikpKRWXhmzZskU1atSQJKWmpv7mA9+mTZuUmprqSUYAAMKZ3Vks+8YU6eQWMm06eR1HEgUGAACIQm3atNFHH30kSfroo4/Utm3biu1z5syRtVbffPONqlatut/lIwAAQLJvvizt2ln+2NQjPJMxVLiEBAAARLQnnnhCy5YtU3FxsQYMGKDevXvrwgsv1OjRo/X+++9XPEZVks444wwtWrRIN910kxISEjRw4ECP0wMAEH7sj6tkP/yXTNc/yqQ38jpOBQoMAAAQ0QYPHnzA7UOHDt1vmzFG/fv3D3UkAAAilrVW7rTnpeRkmZ5XeB3nN7iEBAAAAAAASJLswk+kb5bKXNRHJrm613F+gwIDAAAAAADI7imRnT5BathEpnN3r+PshwIDAAAAAADIzvyHtKVIzuV/kXHivI6zHwoMAAAAAABinN2wTva9GTIduspkNPc6zgFRYAA
2019-11-27 21:15:13 +08:00
"text/plain": [
2020-04-20 23:48:53 +08:00
"<Figure size 1080x720 with 4 Axes>"
2019-11-27 21:15:13 +08:00
]
},
2020-04-20 23:48:53 +08:00
"metadata": {},
2019-11-27 21:15:13 +08:00
"output_type": "display_data"
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
2020-04-20 23:48:53 +08:00
"theta_mpc=np.array(x.value[2, :]).flatten()\n",
"psi_mpc=np.array(x.value[3, :]).flatten()\n",
"cte_mpc=np.array(x.value[4, :]).flatten()\n",
2019-11-27 21:15:13 +08:00
"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",
2020-04-20 23:48:53 +08:00
"plt.figure(figsize=(15,10))\n",
2019-11-27 21:15:13 +08:00
"#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",
2020-04-20 23:48:53 +08:00
"plt.subplot(2, 2, 4)\n",
"plt.plot(cte_mpc)\n",
"plt.ylabel('cte(t)')\n",
"plt.legend(loc='best')\n",
2019-11-27 21:15:13 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
2019-11-29 23:50:12 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"full track demo"
]
},
{
"cell_type": "code",
2020-04-22 22:32:10 +08:00
"execution_count": 23,
2019-11-29 23:50:12 +08:00
"metadata": {},
"outputs": [
2020-04-21 23:33:00 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.local/lib/python3.6/site-packages/ipykernel_launcher.py:18: RuntimeWarning: invalid value encountered in true_divide\n"
]
},
2019-12-02 23:28:07 +08:00
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-04-22 22:32:10 +08:00
"CVXPY Optimization Time: Avrg: 0.1416s Max: 0.2571s Min: 0.1279s\n"
2019-12-02 23:28:07 +08:00
]
2019-11-29 23:50:12 +08:00
}
],
"source": [
"\n",
2020-04-22 22:32:10 +08:00
"track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
" [0,0,2.5,2.5,0,0,5,10])\n",
2019-11-29 23:50:12 +08:00
"\n",
2020-04-22 22:32:10 +08:00
"sim_duration =100 \n",
2019-12-02 23:28:07 +08:00
"opt_time=[]\n",
2019-11-29 23:50:12 +08:00
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
2019-12-02 23:28:07 +08:00
"MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n",
2019-11-29 23:50:12 +08:00
"MAX_STEER_SPEED = 1.57/2\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = -0.5\n",
"x0[2] = np.radians(-0)\n",
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\n",
"\n",
"x_sim[:,0]=x0\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n",
"u_bar[1,:]=0.01\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
2019-12-02 23:28:07 +08:00
" iter_start=time.time()\n",
" \n",
2019-11-29 23:50:12 +08:00
" # Prediction\n",
" x_bar=np.zeros((N,T+1))\n",
" x_bar[:,0]=x_sim[:,sim_time]\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 Linear MPC problem statement\n",
" cost = 0\n",
" constr = []\n",
"\n",
" for t in range(T):\n",
"\n",
" # Tracking\n",
2020-04-22 22:32:10 +08:00
" if t > 0:\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",
" \n",
" # Tracking last time step\n",
" if t == T:\n",
" idx,_,_ = calc_err(x_bar[:,t],track)\n",
" delta_x = track[:,idx]-x[0:3,t]\n",
" cost+= cp.quad_form(delta_x,100*np.eye(3))\n",
2019-11-29 23:50:12 +08:00
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
2019-12-17 18:37:32 +08:00
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M))\n",
2019-12-02 23:28:07 +08:00
" \n",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" # Constrains\n",
2019-11-29 23:50:12 +08:00
" 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",
2019-12-02 23:28:07 +08:00
" constr += [x[:,0] == x_sim[:,sim_time]] # starting condition\n",
2019-11-29 23:50:12 +08:00
" constr += [u[0, :] <= MAX_SPEED]\n",
" constr += [u[0, :] >= MIN_SPEED]\n",
" constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
" \n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
2020-04-20 23:48:53 +08:00
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
2019-11-29 23:50:12 +08:00
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n",
" (np.array(u.value[1, :]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
2019-12-02 23:28:07 +08:00
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
2019-11-29 23:50:12 +08:00
" # move simulation to t+1\n",
" tspan = [0,dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
2020-04-22 22:32:10 +08:00
" args=(u_bar[:,0],))[1]\n",
2019-12-02 23:28:07 +08:00
" \n",
2019-12-17 18:37:32 +08:00
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
2020-04-22 22:32:10 +08:00
" np.min(opt_time))) "
2019-11-29 23:50:12 +08:00
]
},
{
"cell_type": "code",
2020-04-22 22:32:10 +08:00
"execution_count": 24,
2019-11-29 23:50:12 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-04-22 22:32:10 +08:00
"image/png": "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
2019-11-29 23:50:12 +08:00
"text/plain": [
"<Figure size 1080x720 with 3 Axes>"
]
},
2020-04-20 23:48:53 +08:00
"metadata": {},
2019-11-29 23:50:12 +08:00
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
2019-11-29 23:52:25 +08:00
"grid = plt.GridSpec(2, 3)\n",
2019-11-29 23:50:12 +08:00
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:2, 0:2])\n",
2020-04-20 23:48:53 +08:00
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
2019-11-29 23:50:12 +08:00
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(grid[0, 2])\n",
2019-12-02 23:28:07 +08:00
"plt.plot(u_sim[0,:])\n",
2019-11-29 23:50:12 +08:00
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
2019-12-02 23:28:07 +08:00
"plt.ylabel('w(t) [deg/s]')\n",
2019-11-29 23:50:12 +08:00
"\n",
2019-11-29 23:52:25 +08:00
"plt.tight_layout()\n",
2019-11-29 23:50:12 +08:00
"plt.show()"
]
2020-04-20 23:48:53 +08:00
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
2019-11-27 21:15:13 +08:00
}
],
"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",
2020-03-04 20:32:29 +08:00
"version": "3.6.9"
2019-11-27 21:15:13 +08:00
}
},
"nbformat": 4,
2020-03-04 20:32:29 +08:00
"nbformat_minor": 4
2019-11-27 21:15:13 +08:00
}
