2020-04-22 22:32:10 +08:00
{
"cells": [
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 73,
2020-04-22 22:32:10 +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",
"* $cte$ crosstrack error = lateral distance of the robot from the track w.r.t robot frame, cte is a function of the track and x\n",
"\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",
"* $\\dot{cte} = v\\sin{\\psi}$\n",
"\n",
"The **Continuous** State Space Model is\n",
"\n",
"$ {\\dot{x}} = Ax + Bu $\n",
"\n",
"with:\n",
"\n",
"$ A =\n",
"\\quad\n",
"\\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",
"0 & 0 & 0 & vcos(\\psi) & 0 \n",
"\\end{bmatrix}\n",
"\\quad $\n",
"\n",
"\n",
"$ B = \\quad\n",
"\\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",
"\\quad $\n",
"\n",
"discretize with forward Euler Integration for time step 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",
"* ${\\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",
"The **Discrete** State Space Model is then:\n",
"\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",
"0 & 0 & 0 & vcos{(\\psi)}dt & 1 \n",
"\\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 **non-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",
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x_t-\\bar{x}) + B(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 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",
"\n",
"$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\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",
2020-05-02 00:05:24 +08:00
"execution_count": 74,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Control problem statement.\n",
"\n",
2020-05-02 00:05:24 +08:00
"N = 3 #number of state variables\n",
2020-04-22 22:32:10 +08:00
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"dt = 0.25 #discretization step"
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 75,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" theta = x_bar[2]\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",
" 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_lin=dt*B\n",
" \n",
2020-05-02 00:05:24 +08:00
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).reshape(N,1)\n",
2020-04-22 22:32:10 +08:00
" 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-05-02 00:05:24 +08:00
"execution_count": 76,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"def kinematics_model(x,t,u):\n",
" \"\"\"\n",
" \"\"\"\n",
"\n",
" dxdt = u[0]*np.cos(x[2])\n",
" dydt = u[0]*np.sin(x[2])\n",
" dthetadt = u[1]\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
2020-05-02 00:05:24 +08:00
" dthetadt]\n",
2020-04-22 22:32:10 +08:00
"\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-05-02 00:05:24 +08:00
"execution_count": 77,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-05-02 00:05:24 +08:00
"CPU times: user 5.97 ms, sys: 113 µs, total: 6.08 ms\n",
"Wall time: 4.96 ms\n"
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
"%%time\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",
"\n",
"x_bar=predict(x0,u_bar)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Check the model prediction"
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 78,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-05-02 00:05:24 +08:00
"image/png": "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
2020-04-22 22:32:10 +08:00
"text/plain": [
2020-05-02 00:05:24 +08:00
"<Figure size 432x288 with 2 Axes>"
2020-04-22 22:32:10 +08:00
]
},
"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": [
"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-05-02 00:05:24 +08:00
"execution_count": 79,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-05-02 00:05:24 +08:00
"CPU times: user 2.2 ms, sys: 132 µs, total: 2.33 ms\n",
"Wall time: 1.53 ms\n"
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
"%%time\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",
"\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"\n",
"for t in range (1,T+1):\n",
2020-05-02 00:05:24 +08:00
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
2020-04-22 22:32:10 +08:00
" \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-05-02 00:05:24 +08:00
"execution_count": 80,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-05-02 00:05:24 +08:00
"image/png": "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
2020-04-22 22:32:10 +08:00
"text/plain": [
2020-05-02 00:05:24 +08:00
"<Figure size 432x288 with 2 Axes>"
2020-04-22 22:32:10 +08:00
]
},
"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(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"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"------------------\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",
"-----------------"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## PRELIMINARIES"
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 81,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\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,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",
"\n",
" return np.vstack((final_xp,final_yp))\n",
"\n",
"def get_nn_idx(state,path):\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",
" return target_idx\n",
"\n",
"def road_curve(state,track):\n",
" \n",
" #given vehicle pos find lookahead waypoints\n",
" nn_idx=get_nn_idx(state,track)\n",
" LOOKAHED=6\n",
" lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
"\n",
" #trasform lookahead waypoints to vehicle ref frame\n",
" dx = lk_wp[0,:] - state[0]\n",
" dy = lk_wp[1,:] - state[1]\n",
"\n",
" wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
"\n",
" #fit poly\n",
" return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n",
"\n",
"def f(x,coeff):\n",
" return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n",
"\n",
"def df(x,coeff):\n",
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"test it"
]
},
{
"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": [
"\n",
"\n",
""
]
},
{
"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} $\n",
"\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",
"$\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",
" & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"Other nonlinear constrains may be applied:\n",
"\n",
"$ g(x_{j|t},{j|t})<0 \\quad \\textrm{for} \\quad t<j<t+T-1 $"
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 82,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-05-02 00:05:24 +08:00
"[0. 0. 0.]\n"
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
"track = compute_path_from_wp([0,5,10],[0,0,2])\n",
"\n",
"# Constrains\n",
"MAX_SPEED = 2.5\n",
"MIN_SPEED = 0.5\n",
"MAX_STEER_SPEED = 1.57/2\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(-10)\n",
"\n",
"#X0 is needed ONLY to initialize road curve\n",
"K=road_curve(x0,track)\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.0\n",
"\n",
"# Linearized Model for Prediction##############\n",
"x_bar=np.zeros((N,T+1))\n",
"\n",
"#starting state is 0,0,0,psi,cte\n",
"x_bar[0,0]=0\n",
"x_bar[1,0]=0\n",
"x_bar[2,0]=0\n",
2020-05-02 00:05:24 +08:00
"\n",
2020-04-22 22:32:10 +08:00
"\n",
"print(x_bar[:,0])\n",
"\n",
"for t in range (1,T+1):\n",
2020-05-02 00:05:24 +08:00
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
2020-04-22 22:32:10 +08:00
" \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",
" x_bar[:,t]= xt_plus_one\n",
"#################################################\n"
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 83,
2020-04-22 22:32:10 +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",
2020-05-02 00:05:24 +08:00
"problem: variables n = 241, constraints m = 281\n",
" nnz(P) + nnz(A) = 715\n",
2020-04-22 22:32:10 +08:00
"settings: linear system solver = qdldl,\n",
" eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
" 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",
2020-05-02 00:05:24 +08:00
" 1 0.0000e+00 1.02e+00 1.02e+02 1.00e-01 2.26e-04s\n",
" 125 1.6631e+03 2.07e-05 3.42e-04 7.12e+01 1.00e-03s\n",
2020-04-22 22:32:10 +08:00
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
"number of iterations: 125\n",
2020-05-01 23:40:00 +08:00
"optimal objective: 1663.1222\n",
2020-05-02 00:05:24 +08:00
"run time: 1.17e-03s\n",
2020-05-01 23:40:00 +08:00
"optimal rho estimate: 7.91e+01\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-02 00:05:24 +08:00
"CPU times: user 234 ms, sys: 8.58 ms, total: 242 ms\n",
"Wall time: 229 ms\n"
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
"x = cp.Variable((N, T+1))\n",
"u = cp.Variable((M, T))\n",
"\n",
"#CVXPY Linear MPC problem statement\n",
"cost = 0\n",
"constr = []\n",
"\n",
"for t in range(T):\n",
" \n",
" # Cost function\n",
2020-05-01 23:40:00 +08:00
" #cost += 10*cp.sum_squares( x[3, t]) # psi\n",
" #cost += 500*cp.sum_squares( x[4, t]) # cte\n",
" \n",
" cost += 10*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 500*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
2020-04-22 22:32:10 +08:00
" # 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], 100*np.eye(M))\n",
" \n",
" # KINEMATICS 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] == 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",
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
"solution = prob.solve(solver=cp.OSQP, verbose=True)"
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 84,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-05-02 00:05:24 +08:00
"image/png": "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
2020-04-22 22:32:10 +08:00
"text/plain": [
2020-05-02 00:05:24 +08:00
"<Figure size 432x288 with 2 Axes>"
2020-04-22 22:32:10 +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",
"theta_mpc=np.array(x.value[2, :]).flatten()\n",
"v_mpc=np.array(u.value[0, :]).flatten()\n",
"w_mpc=np.array(u.value[1, :]).flatten()\n",
"\n",
"#simulate robot state trajectory for optimized U\n",
"x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n",
"\n",
"#plt.figure(figsize=(15,10))\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",
2020-05-02 00:05:24 +08:00
"# plt.subplot(2, 2, 3)\n",
"# plt.plot(psi_mpc)\n",
"# plt.ylabel('psi(t)')\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-02 00:05:24 +08:00
"# plt.subplot(2, 2, 4)\n",
"# plt.plot(cte_mpc)\n",
"# plt.ylabel('cte(t)')\n",
2020-04-22 22:32:10 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"full track demo"
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 99,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
2020-05-01 23:40:00 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.local/lib/python3.6/site-packages/ipykernel_launcher.py:29: RuntimeWarning: invalid value encountered in true_divide\n"
]
},
2020-04-22 22:32:10 +08:00
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-05-02 00:05:24 +08:00
"CVXPY Optimization Time: Avrg: 0.1807s Max: 0.2490s Min: 0.1668s\n"
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
2020-05-01 23:40:00 +08:00
"track = compute_path_from_wp([0,3,4,6,10],\n",
2020-05-02 00:05:24 +08:00
" [0,0,2,4,3])\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-02 00:05:24 +08:00
"sim_duration = 60 #time steps\n",
2020-04-22 22:32:10 +08:00
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
2020-05-02 00:05:24 +08:00
"MAX_SPEED = 1.5\n",
"MIN_SPEED = 0.5\n",
"MAX_STEER_SPEED = 1.57\n",
2020-04-22 22:32:10 +08:00
"\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",
" K=road_curve(x_sim[:,sim_time],track)\n",
" \n",
" # dynamics starting state\n",
" x_bar=np.zeros((N,T+1))\n",
" \n",
" #x_bar[:,0]=x_sim[:,sim_time]\n",
" for t in range (1,T+1):\n",
2020-05-02 00:05:24 +08:00
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
2020-04-22 22:32:10 +08:00
"\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",
" 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",
" for t in range(T):\n",
"\n",
" # Tracking\n",
2020-05-01 23:40:00 +08:00
" #cost += 10*cp.sum_squares( x[3, t]) # psi\n",
" #cost += 50*cp.sum_squares( x[4, t]) # cte\n",
" cost += 10*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
2020-05-02 00:05:24 +08:00
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
2020-04-22 22:32:10 +08:00
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
2020-05-02 00:05:24 +08:00
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 1*np.eye(M))\n",
2020-04-22 22:32:10 +08:00
" \n",
" # Actuation effort\n",
2020-05-02 00:05:24 +08:00
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
2020-04-22 22:32:10 +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] == 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(solver=cp.OSQP, 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))) "
]
},
{
"cell_type": "code",
2020-05-02 00:05:24 +08:00
"execution_count": 100,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-05-02 00:05:24 +08:00
"image/png": "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
2020-04-22 22:32:10 +08:00
"text/plain": [
"<Figure size 1080x720 with 3 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(2, 3)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"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",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\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",
"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.6.9"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
