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

1219 lines
186 KiB
Plaintext
Raw Normal View History

WIP: racecar mpc
2020-07-01 00:21:27 +08:00
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"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",
"* $v$ velocity of the robot\n",
"* $\\theta$ heading of the robot\n",
"\n",
"The inputs of the model are:\n",
"\n",
added input delay in sim
2020-08-06 17:21:47 +08:00
"* $a$ acceleration of the robot\n",
new cost function
2020-10-23 18:23:12 +08:00
"* $\\delta$ steering of the robot\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"These are the differential equations f(x,u) of the model:\n",
"\n",
"$\\dot{x} = f(x,u)$\n",
"\n",
"* $\\dot{x} = v\\cos{\\theta}$ \n",
"* $\\dot{y} = v\\sin{\\theta}$\n",
"* $\\dot{v} = a$\n",
"* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n",
"\n",
"Discretize with forward Euler Integration for time step dt:\n",
"\n",
"${x_{t+1}} = x_{t} + f(x,u)dt$\n",
"\n",
"* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}dt$\n",
"* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}dt$\n",
"* ${v_{t+1}} = v_{t} + a_tdt$\n",
"* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n",
"\n",
"----------------------\n",
"\n",
"The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n",
"\n",
"$ f(x,u) \\approx f(\\bar{x},\\bar{u}) + \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"This can be rewritten usibg the State Space model form Ax+Bu :\n",
"\n",
"$ f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"Where:\n",
"\n",
"$\n",
"A =\n",
"\\quad\n",
"\\begin{bmatrix}\n",
updated racecar files
2020-07-01 21:33:16 +08:00
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
updated racecar files
2020-07-01 21:33:16 +08:00
"\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"$\n",
"\n",
"and\n",
"\n",
"$\n",
"B = \n",
"\\quad\n",
"\\begin{bmatrix}\n",
updated racecar files
2020-07-01 21:33:16 +08:00
"\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"= \n",
updated racecar files
2020-07-01 21:33:16 +08:00
"\\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]\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"$\n",
"\n",
"are the *Jacobians*.\n",
"\n",
"\n",
"\n",
"So the discretized model is given by:\n",
"\n",
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(u_t-\\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 LTI-equivalent kinematics model is:\n",
"\n",
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
"\n",
"with:\n",
"\n",
"$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n",
"\n",
"$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
"\n",
"$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $"
]
},
{
"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",
"-----------------"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Kinematics Model"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
"\"\"\"\n",
"Control problem statement.\n",
"\"\"\"\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
new cost function
2020-10-23 18:23:12 +08:00
"DT = 0.25 #discretization step"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
" \"\"\"\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \"\"\"\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \n",
" L=0.3 #vehicle wheelbase\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
" \n",
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
" A[1,2]=np.sin(theta)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" A[1,3]=v*np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n",
new cost function
2020-10-23 18:23:12 +08:00
" A_lin=np.eye(N)+DT*A\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
" B[3,1]=v/(L*np.cos(delta)**2)\n",
new cost function
2020-10-23 18:23:12 +08:00
" B_lin=DT*B\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
new cost function
2020-10-23 18:23:12 +08:00
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using scipy intergration"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"# This uses the continuous model \n",
"def kinematics_model(x,t,u):\n",
" \"\"\"\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" Returns the set of ODE of the vehicle model.\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \"\"\"\n",
" \n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" L=0.3 #vehicle wheelbase\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" dxdt = x[2]*np.cos(x[3])\n",
" dydt = x[2]*np.sin(x[3])\n",
" dvdt = u[0]\n",
" dthetadt = x[2]*np.tan(u[1])/L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" x_ = np.zeros((N,T+1))\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" x_[:,0] = x0\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n",
new cost function
2020-10-23 18:23:12 +08:00
" tspan = [0,DT]\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"\n",
" x0 = x_next[1]\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" x_[:,t]=x_next[1]\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" return x_"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Validate the model, here the status w.r.t a straight line with constant heading 0"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"CPU times: user 2.77 ms, sys: 767 µs, total: 3.54 ms\n",
"Wall time: 2.99 ms\n"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 0.2 #m/ss\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = 0\n",
"x0[3] = np.radians(0)\n",
"\n",
"x_bar=predict(x0,u_bar)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Check the model prediction"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
new cost function
2020-10-23 18:23:12 +08:00
"image/png": "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
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"text/plain": [
"<Figure size 432x288 with 2 Axes>"
]
},
"metadata": {},
"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",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using the state space model"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"CPU times: user 2.99 ms, sys: 0 ns, total: 2.99 ms\n",
"Wall time: 1.91 ms\n"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 0.2 #m/s\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = 0\n",
"x0[3] = np.radians(0)\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(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,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",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
new cost function
2020-10-23 18:23:12 +08:00
"image/png": "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
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"text/plain": [
"<Figure size 432x288 with 2 Axes>"
]
},
"metadata": {},
"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)')\n",
"#plt.xlabel('time')\n",
"\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The results are the same as expected, so the linearized model is equivalent as expected."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## PRELIMINARIES"
]
},
{
"cell_type": "code",
fixed paper link
2020-10-23 23:31:57 +08:00
"execution_count": 9,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
"outputs": [],
"source": [
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" 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",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" \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))\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"\n",
"def get_nn_idx(state,path):\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" dist = np.hypot(dx,dy)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" 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\n",
"\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"def get_ref_trajectory(state, path, target_v):\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" \"\"\"\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
" ncourse = path.shape[1]\n",
"\n",
new cost function
2020-10-23 18:23:12 +08:00
" ind = get_nn_idx(state, path)\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"\n",
" xref[0, 0] = path[0,ind] #X\n",
" xref[1, 0] = path[1,ind] #Y\n",
new cost function
2020-10-23 18:23:12 +08:00
" xref[2, 0] = target_v #sp[ind] #V\n",
" xref[3, 0] = path[2,ind] #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" \n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" dl = 0.05 # Waypoints spacing [m]\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" travel = 0.0\n",
"\n",
" for i in range(T + 1):\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" travel += abs(target_v) * DT #current V or target V?\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" dind = int(round(travel / dl))\n",
"\n",
" if (ind + dind) < ncourse:\n",
" xref[0, i] = path[0,ind + dind]\n",
" xref[1, i] = path[1,ind + dind]\n",
" xref[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = path[2,ind + dind]\n",
" dref[0, i] = 0.0\n",
" else:\n",
" xref[0, i] = path[0,ncourse - 1]\n",
" xref[1, i] = path[1,ncourse - 1]\n",
fixed paper link
2020-10-23 23:31:57 +08:00
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" xref[3, i] = path[2,ncourse - 1]\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 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": [
added trajectory generation method
2020-10-23 17:43:10 +08:00
"<!-- ![mpc](img/mpc_block_diagram.png) -->\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"<!-- ![mpc](img/mpc_t.png) -->"
WIP: racecar mpc
2020-07-01 00:21:27 +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",
"$\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",
" & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"Other linear constrains may be applied,for instance on the control variable:\n",
"\n",
"$ U_{MIN} < u_{j|t} < U_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 $\n",
"\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",
"\n",
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
"\n",
"$\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",
"\n",
"where the error w.r.t desired state is accounted for:\n",
"\n",
"$ \\delta x = x_{j,t,ref} - x_{j,t} $"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Problem Formulation: Study case\n",
"\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"In this case, the objective function to minimize is given by:\n",
"\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} (x_{j} - x_{j,ref})^TQ(x_{j} - x_{j,ref}) + \\sum^{t+T-1}_{j=t+1} u^T_{j}Ru_{j} + (u_{j} - u_{j-1})^T(u_{j} - u_{j-1}) \\\\\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" & x_{j+1} = Ax_{j}+Bu_{j} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
" & u_{MIN} < u_{j} < u_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
" & \\dot{u}_{MIN} < \\frac{(u_{j} - u_{j-1})}{ts} < \\dot{u}_{MAX} \\quad \\textrm{for} \\quad t+1<j<t+T-1 \\\\\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"Where R,P,Q are the cost matrices used to tune the response.\n"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"execution_count": 21,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"-----------------------------------------------------------------\n",
" OSQP v0.6.0 - Operator Splitting QP Solver\n",
" (c) Bartolomeo Stellato, Goran Banjac\n",
" University of Oxford - Stanford University 2019\n",
"-----------------------------------------------------------------\n",
new cost function
2020-10-23 18:23:12 +08:00
"problem: variables n = 322, constraints m = 404\n",
" nnz(P) + nnz(A) = 1051\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"settings: linear system solver = qdldl,\n",
new cost function
2020-10-23 18:23:12 +08:00
" eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,\n",
" rho = 1.00e-01 (adaptive),\n",
" sigma = 1.00e-06, alpha = 1.60, max_iter = 10000\n",
" check_termination: on (interval 25),\n",
" scaling: on, scaled_termination: off\n",
" warm start: on, polish: on, time_limit: off\n",
"\n",
"iter objective pri res dua res rho time\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" 1 0.0000e+00 5.08e+00 5.08e+02 1.00e-01 3.57e-04s\n",
" 175 1.7545e+02 5.84e-05 5.89e-05 6.89e+00 1.81e-03s\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"number of iterations: 175\n",
"optimal objective: 175.4495\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"run time: 2.16e-03s\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"optimal rho estimate: 6.44e+00\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"CPU times: user 117 ms, sys: 2 µs, total: 117 ms\n",
"Wall time: 114 ms\n"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"MAX_SPEED = 1.5 #m/s\n",
"MAX_STEER = np.radians(30) #rad\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"MAX_ACC = 1.0\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"REF_VEL=1.0\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"track = compute_path_from_wp([0,3,6],\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
" [0,0,0],0.05)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
new cost function
2020-10-23 18:23:12 +08:00
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.1 #delta\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"# dynamics starting state w.r.t world frame\n",
new cost function
2020-10-23 18:23:12 +08:00
"x_bar = np.zeros((N,T+1))\n",
"x_bar[:,0] = x0\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"#prediction for linearization of costrains\n",
"for t in range (1,T+1):\n",
new cost function
2020-10-23 18:23:12 +08:00
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A, B, C = get_linear_model(xt,ut)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
new cost function
2020-10-23 18:23:12 +08:00
" x_bar[:,t] = xt_plus_one\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"#CVXPY Linear MPC problem statement\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"x = cp.Variable((N, T+1))\n",
"u = cp.Variable((M, T))\n",
"cost = 0\n",
"constr = []\n",
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"# Cost Matrices\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"Q = np.diag([10,10,10,10]) #state error cost\n",
new cost function
2020-10-23 18:23:12 +08:00
"Qf = np.diag([10,10,10,10]) #state final error cost\n",
"R = np.diag([10,10]) #input cost\n",
"R_ = np.diag([10,10]) #input rate of change cost\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"#Get Reference_traj\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"x_ref, d_ref = get_ref_trajectory(x_bar[:,0], track, REF_VEL)\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"#Prediction Horizon\n",
"for t in range(T):\n",
" \n",
" # Tracking Error\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
" # Actuation effort\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t], R)\n",
new cost function
2020-10-23 18:23:12 +08:00
" \n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
new cost function
2020-10-23 18:23:12 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
"\n",
"# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"constr += [x[2,:] <= MAX_SPEED] #max speed\n",
"constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
"constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
"constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
"# for t in range(T):\n",
"# if t < (T - 1):\n",
"# constr += [cp.abs(u[0,t] - u[0,t-1])/DT <= MAX_ACC] #max acc\n",
"# constr += [cp.abs(u[1,t] - u[1,t-1])/DT <= MAX_STEER] #max steer\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
"solution = prob.solve(solver=cp.OSQP, verbose=True)"
]
},
{
"cell_type": "code",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"execution_count": 22,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
"outputs": [
{
"data": {
fixed paper link
2020-10-23 23:31:57 +08:00
"image/png": "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
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"text/plain": [
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"<Figure size 432x288 with 4 Axes>"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"v_mpc=np.array(x.value[2, :]).flatten()\n",
"theta_mpc=np.array(x.value[3, :]).flatten()\n",
"a_mpc=np.array(u.value[0, :]).flatten()\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"delta_mpc=np.array(u.value[1, :]).flatten()\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"#simulate robot state trajectory for optimized U\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"x_traj=predict(x0, np.vstack((a_mpc,delta_mpc)))\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"#plt.figure(figsize=(15,10))\n",
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.plot(track[0,:],track[1,:],\"b\")\n",
"plt.plot(x_ref[0,:],x_ref[1,:],\"g+\")\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"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",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 3)\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"plt.plot(a_mpc)\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.ylabel('a_in(t)')\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"#plt.xlabel('time')\n",
"\n",
"\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 2)\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"plt.plot(theta_mpc)\n",
"plt.ylabel('theta(t)')\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 4)\n",
"plt.plot(delta_mpc)\n",
"plt.ylabel('d_in(t)')\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"full track demo"
]
},
{
"cell_type": "code",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"execution_count": 31,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"outputs": [
fixed paper link
2020-10-23 23:31:57 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.conda/envs/jupyter/lib/python3.8/site-packages/cvxpy/problems/problem.py:1054: UserWarning: Solution may be inaccurate. Try another solver, adjusting the solver settings, or solve with verbose=True for more information.\n",
" warnings.warn(\n"
]
},
master of plots
2020-07-01 17:45:08 +08:00
{
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"name": "stdout",
"output_type": "stream",
"text": [
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"CVXPY Optimization Time: Avrg: 0.1672s Max: 0.2519s Min: 0.1402s\n"
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
]
}
],
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"source": [
plotting sim racecar
2020-07-01 22:59:13 +08:00
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"# 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],0.5)\n",
"\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"sim_duration = 200 #time steps\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"MAX_SPEED = 1.5 #m/s\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"MAX_ACC = 1.0 #m/ss\n",
"MAX_DACC = 1.0 #m/sss\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"MAX_STEER = np.radians(30) #rad\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"MAX_DSTEER = np.radians(30) #rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"u_bar[0,:] = MAX_ACC/2 #a\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"u_bar[1,:] = 0.0 #delta\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start=time.time()\n",
" \n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
" A,B,C=get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t]= xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state error cost\n",
" Qf = np.diag([10,10,10,10]) #state final error cost\n",
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" # Tracking Error\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"\n",
" # Actuation effort\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t], R)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t]) <= MAX_DACC * DT] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t]) <= MAX_DSTEER * DT] #max steer rate of change\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" # Kinrmatics Constrains (Linearized model)\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" # move simulation to t+1\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" tspan = [0,DT]\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
]
},
{
"cell_type": "code",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"execution_count": 33,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"outputs": [
{
"data": {
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"image/png": "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
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"source": [
"#plot trajectory\n",
master of plots
2020-07-01 17:45:08 +08:00
"grid = plt.GridSpec(4, 5)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
master of plots
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[0:4, 0:4])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
master of plots
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[0, 4])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"plt.plot(u_sim[0,:])\n",
master of plots
2020-07-01 17:45:08 +08:00
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"plt.ylabel('v(t) [m/s]')\n",
"\n",
master of plots
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[2, 4])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"plt.plot(np.degrees(u_sim[1,:]))\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"plt.ylabel('delta(t) [rad]')\n",
master of plots
2020-07-01 17:45:08 +08:00
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"plt.ylabel('theta(t) [rad]')\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## OBSTACLE AVOIDANCE\n",
"see dccp paper for reference"
]
},
{
master of plots
2020-07-01 17:45:08 +08:00
"cell_type": "markdown",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
"source": [
"import dccp\n",
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
" [0,0,2,4,3,3],0.25)\n",
"\n",
"obstacles=np.array([[4,6],[2,4]])\n",
"obstacle_radius=0.5\n",
"\n",
"def to_vehic_frame(pt,pos_x,pos_y,theta):\n",
" dx = pt[0] - pos_x\n",
" dy = pt[1] - pos_y\n",
"\n",
" return [dx * np.cos(-theta) - dy * np.sin(-theta),\n",
" dy * np.cos(-theta) + dx * np.sin(-theta)]\n",
" \n",
"# 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],0.5)\n",
"\n",
"sim_duration = 80 #time steps\n",
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
"MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n",
"MAX_STEER_SPEED = 1.57\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = -0.25\n",
"x0[2] = np.radians(-0)\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.00\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start=time.time()\n",
" \n",
" #compute coefficients\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
" \n",
" #compute opstacles in ref frame\n",
" o_=[]\n",
" for j in range(2):\n",
" o_.append(to_vehic_frame(obstacles[:,j],x_sim[0,sim_time],x_sim[1,sim_time],x_sim[2,sim_time]) )\n",
" \n",
" # dynamics starting state w.r.t vehicle frame\n",
" x_bar=np.zeros((N,T+1))\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
" A,B,C=get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t]= xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" cost = 0\n",
" constr = []\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n",
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n",
" \n",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" # Kinrmatics Constrains (Linearized model)\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",
" # Obstacle Avoidance Contrains\n",
" for j in range(2):\n",
" constr += [ cp.norm(x[0:2,t]-o_[j],2) >= obstacle_radius ]\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n",
" 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",
" solution = prob.solve(method=\"dccp\", verbose=False)\n",
" \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",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" # 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",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
]
},
{
master of plots
2020-07-01 17:45:08 +08:00
"cell_type": "markdown",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(2, 3)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"ax=plt.subplot(grid[0:2, 0:2])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"#obstacles\n",
"circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), obstacle_radius, color='r')\n",
"circle2=plt.Circle((obstacles[0,1], obstacles[1,1]), obstacle_radius, color='r')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"ax.add_artist(circle1)\n",
"ax.add_artist(circle2)\n",
"\n",
"plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('w(t) [deg/s]')\n",
"\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"display_name": "Python 3",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"language": "python",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"name": "python3"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"version": "3.8.5"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
}
},
"nbformat": 4,
"nbformat_minor": 4
}