2020-04-22 22:32:10 +08:00
{
"cells": [
{
"cell_type": "code",
2020-06-29 22:31:43 +08:00
"execution_count": 17,
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",
"\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",
"\n",
2020-05-04 19:07:01 +08:00
"Discretize with forward Euler Integration for time step dt:\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 19:07:01 +08:00
"${x_{t+1}} = x_{t} + f(x,u)*dt$\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 19:07:01 +08:00
"* ${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",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 22:16:29 +08:00
"----------------------\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",
2020-05-04 19:07:01 +08:00
"\n",
2020-05-04 20:36:18 +08:00
"$ 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",
2020-05-04 19:07:01 +08:00
"\n",
2020-05-04 22:16:29 +08:00
"This can be rewritten usibg the State Space model form Ax+Bu :\n",
2020-05-04 19:07:01 +08:00
"\n",
2020-05-04 20:36:18 +08:00
"$ 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",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 19:07:01 +08:00
"Where:\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 19:07:01 +08:00
"$\n",
"A =\n",
"\\quad\n",
2020-04-22 22:32:10 +08:00
"\\begin{bmatrix}\n",
2020-05-04 19:07:01 +08:00
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
"\\quad\n",
"\\begin{bmatrix}\n",
2020-05-04 19:07:01 +08:00
"0 & 0 & -v\\sin{\\theta} \\\\\n",
"0 & 0 & v\\cos{\\theta} \\\\\n",
"0 & 0 & 0\\\\\n",
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
2020-05-04 19:07:01 +08:00
"\\quad\n",
"$\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 19:07:01 +08:00
"and\n",
2020-04-22 22:32:10 +08:00
"\n",
"$\n",
2020-05-04 19:07:01 +08:00
"B = \n",
"\\quad\n",
2020-04-22 22:32:10 +08:00
"\\begin{bmatrix}\n",
2020-05-04 19:07:01 +08:00
"\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial w} \\\\\n",
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
2020-05-04 19:07:01 +08:00
"= \n",
"\\quad\n",
2020-04-22 22:32:10 +08:00
"\\begin{bmatrix}\n",
2020-05-04 20:36:18 +08:00
"\\cos{\\theta} & 0 \\\\\n",
"\\sin{\\theta} & 0 \\\\\n",
2020-05-04 19:16:04 +08:00
"0 & 1\n",
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
2020-05-04 20:36:18 +08:00
"are the *Jacobians*.\n",
"\n",
"\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 19:07:01 +08:00
"So the discretized model is given by:\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 20:36:18 +08:00
"$ 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",
2020-04-22 22:32:10 +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",
2020-05-04 19:07:01 +08:00
"The LTI-equivalent kinematics model is:\n",
2020-04-22 22:32:10 +08:00
"\n",
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
"\n",
"with:\n",
"\n",
2020-05-04 20:36:18 +08:00
"$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 20:36:18 +08:00
"$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-04 20:36:18 +08:00
"$ 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}) $\n",
2020-05-04 19:07:01 +08:00
"\n",
"**Errors** are:\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",
"described by:"
2020-04-22 22:32:10 +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",
"-----------------"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Kinematics Model"
]
},
{
"cell_type": "code",
2020-06-29 22:31:43 +08:00
"execution_count": 18,
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-06-29 22:31:43 +08:00
"execution_count": 19,
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-06-29 22:31:43 +08:00
"execution_count": 20,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
2020-05-04 19:07:01 +08:00
"# This uses the continuous model \n",
2020-04-22 22:32:10 +08:00
"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-06-29 22:31:43 +08:00
"execution_count": 21,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-06-29 22:31:43 +08:00
"CPU times: user 8.17 ms, sys: 10 µs, total: 8.18 ms\n",
"Wall time: 7.26 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-06-29 22:31:43 +08:00
"execution_count": 22,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-06-29 22:31:43 +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": [
"Motion Prediction: using the state space model"
]
},
{
"cell_type": "code",
2020-06-29 22:31:43 +08:00
"execution_count": 23,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-06-29 22:31:43 +08:00
"CPU times: user 1.72 ms, sys: 0 ns, total: 1.72 ms\n",
"Wall time: 1.2 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-06-29 22:31:43 +08:00
"execution_count": 24,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-06-29 22:31:43 +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",
2020-05-04 20:36:18 +08:00
"plt.plot(np.degrees(x_bar[2,:]))\n",
2020-04-22 22:32:10 +08:00
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2020-05-04 20:36:18 +08:00
"The results are the same as expected, so the linearized model is equivalent as expected."
2020-04-22 22:32:10 +08:00
]
},
{
"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-06-29 22:31:43 +08:00
"execution_count": 25,
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",
2020-05-04 22:16:29 +08:00
" nn_idx=get_nn_idx(state,track)-1\n",
2020-04-22 22:32:10 +08:00
" 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",
2020-05-04 22:16:29 +08:00
"# def f(x,coeff):\n",
"# return round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)\n",
"\n",
2020-04-22 22:32:10 +08:00
"def df(x,coeff):\n",
2020-05-04 22:16:29 +08:00
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
"# def df(x,coeff):\n",
"# return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)"
2020-04-22 22:32:10 +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": [
"\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",
2020-06-29 22:31:43 +08:00
"$ \\delta x = x_{j,t,ref} - x_{j,t} $"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Problem Formulation: Study case\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"In this case, the objective function to minimize is given the sum of the **cross-track error** plus **heading error**\n",
2020-04-22 22:32:10 +08:00
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
2020-06-29 22:31:43 +08:00
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} cte_{j|t,ref}^TQcte_{j,t,ref} + \\psi_{j|t,ref}^TP\\psi_{j|t,ref} + u^T_{j|t}Ru_{j|t} \\\\\n",
2020-04-22 22:32:10 +08:00
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
2020-06-29 22:31:43 +08:00
" & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
2020-04-22 22:32:10 +08:00
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
2020-06-29 22:31:43 +08:00
"where R,P,Q are the cost matrices used to tune the response.\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"## Error Formulation\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"The track can be represented by fitting a curve trough its waypoints, using the vehicle position as reference\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"A fitted cubic poly has the form:\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"$\n",
"f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
"$\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"The derivative of a fitted cubic poly has the form:\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"$\n",
"f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
"$\n",
2020-05-02 00:05:24 +08:00
"\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"* **crosstrack error** cte: desired y-position - y-position of vehicle -> this is the value of the fitted polynomial\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"* **heading error** epsi: desired heading - heading of vehicle -> is the inclination of tangent to the fitted polynomial\n",
"\n",
"Then using the fitted polynomial representation in vehicle frame the errors can be easily computed as:\n",
"\n",
"$\n",
"cte = f(px) \\\\\n",
"\\psi = -atan(f`(px)) \\\\\n",
"$"
2020-04-22 22:32:10 +08:00
]
},
{
"cell_type": "code",
2020-06-29 22:31:43 +08:00
"execution_count": 26,
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",
2020-06-29 22:31:43 +08:00
" nnz(P) + nnz(A) = 753\n",
2020-04-22 22:32:10 +08:00
"settings: linear system solver = qdldl,\n",
2020-06-29 22:31:43 +08:00
" eps_abs = 1.0e-04, eps_rel = 1.0e-04,\n",
2020-04-22 22:32:10 +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",
2020-06-29 22:31:43 +08:00
" 1 0.0000e+00 1.00e+00 1.06e+02 1.00e-01 2.96e-04s\n",
" 125 1.8119e+03 6.13e-05 5.69e-04 4.76e+01 1.06e-03s\n",
2020-04-22 22:32:10 +08:00
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
"number of iterations: 125\n",
2020-06-29 22:31:43 +08:00
"optimal objective: 1811.9387\n",
"run time: 1.27e-03s\n",
"optimal rho estimate: 4.81e+01\n",
2020-04-22 22:32:10 +08:00
"\n",
2020-06-29 22:31:43 +08:00
"CPU times: user 237 ms, sys: 4 ms, total: 241 ms\n",
"Wall time: 236 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",
2020-06-29 22:31:43 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
2020-04-22 22:32:10 +08:00
" \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-06-29 22:31:43 +08:00
"execution_count": 27,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-06-29 22:31:43 +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-06-29 22:31:43 +08:00
"execution_count": 28,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
2020-05-01 23:40:00 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
2020-06-29 22:31:43 +08:00
"<ipython-input-25-0756f6b412cb>:29: RuntimeWarning: invalid value encountered in true_divide\n",
" v /= np.linalg.norm(v)\n",
"<ipython-input-28-79710852001d>:33: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
"<ipython-input-28-79710852001d>:33: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n"
2020-05-01 23:40:00 +08:00
]
},
2020-04-22 22:32:10 +08:00
{
"name": "stdout",
"output_type": "stream",
"text": [
2020-06-29 22:31:43 +08:00
"CVXPY Optimization Time: Avrg: 0.1897s Max: 0.2940s Min: 0.1666s\n"
2020-05-05 00:36:23 +08:00
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
2020-06-29 22:31:43 +08:00
"<ipython-input-28-79710852001d>:33: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n"
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
2020-05-04 22:16:29 +08:00
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
" [0,0,2,4,3,3],0.25)\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",
2020-04-22 22:32:10 +08:00
"\n",
2020-05-05 00:36:23 +08:00
"sim_duration = 80 #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-04 19:07:01 +08:00
"MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n",
2020-05-02 00:05:24 +08:00
"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",
2020-05-04 19:07:01 +08:00
" # dynamics starting state w.r.t vehicle frame\n",
2020-04-22 22:32:10 +08:00
" x_bar=np.zeros((N,T+1))\n",
" \n",
2020-05-04 19:07:01 +08:00
" #prediction for linearization of costrains\n",
2020-04-22 22:32:10 +08:00
" 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
" 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",
2020-06-29 22:31:43 +08:00
" #Prediction Horizon\n",
2020-04-22 22:32:10 +08:00
" for t in range(T):\n",
"\n",
2020-05-04 22:16:29 +08:00
" #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",
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-04 22:16:29 +08:00
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*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",
2020-05-04 19:07:01 +08:00
" # Kinrmatics Constrains (Linearized model)\n",
2020-04-22 22:32:10 +08:00
" A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n",
2020-06-29 22:31:43 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
2020-04-22 22:32:10 +08:00
"\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-06-29 22:31:43 +08:00
"execution_count": 29,
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-06-29 22:31:43 +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()"
]
},
2020-06-29 22:31:43 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## OBSTACLE AVOIDANCE\n",
"see dccp paper for reference"
]
},
{
"cell_type": "code",
"execution_count": 51,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"<ipython-input-25-0756f6b412cb>:29: RuntimeWarning: invalid value encountered in true_divide\n",
" v /= np.linalg.norm(v)\n",
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
" K=road_curve(x_sim[:,sim_time],track)\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"CVXPY Optimization Time: Avrg: 21.4061s Max: 49.1719s Min: 1.2366s\n"
]
}
],
"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))) "
]
},
{
"cell_type": "code",
"execution_count": 52,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAABC8AAALICAYAAABfINo9AAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4yLjIsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+WH4yJAAAgAElEQVR4nOzde5RkdX3v/c+vdlX1/TJ0M8AAA6IIInKTKAE0oCOPZz2aheYEcT1nmcRlckbUxCdZeaLm5CxznhMP6zni4RDEy2MCnjxG4zFRE4PRjGiC4lFwwECGCOiEy3CZG8N0z3R3Ve39e/7Ytat27dq7atf90u/XWq6+VFXXr4tuZH/7+/18jbXWCgAAAAAAYEhlBn0AAAAAAACARiheAAAAAACAoUbxAgAAAAAADDWKFwAAAAAAYKhRvAAAAAAAAEON4gUAAAAAABhq2UEfoBNPP/10359zeXlZBw8e7PvzjjNe0+7jNe0+XtPe4HXtPl7T7uM17b5heU23bds26CP0XDv/vTws/3yk4TqLNFzn4Szxhuks0nCdZ1TOkvTvZjovAAAAAADAUKN4AQAAAAAAhhrFCwAAAAAAMNQoXgAAAAAAgKFG8QIAAAAAAAw1ihcAAAAAAGCoUbwAAAAAAABDjeIFAAAAAAAYahQvAAAAAADAUKN4AQAAAAAAhhrFCwAAAAAAMNQoXgAAAAAAgKGWHfQBAAAAAAC95/3PP5V97mllrvo30nkXy2T4WzZGBz+tAAAAANAD1vPk/fknZZ9+YtBHkSTZB34g/fiH8v77H8r7g3fL+9bXZD2v/n4b6/K+dIdssTCAUwLxKF4AAAAAQC8cW5X99p2y/3z/oE/iK5VkXvVamXf9jjQ1I/uFT0txhZXHHpb9xl9Jex/t/xmBBBQvAAAAAKAXrOu/jeluGAi3JE1OKfPqX1DmzW/3PxfXXVEq+W89t39nA5qgeAEAAAAAvRAULYaleFEqSU459tBx/Lduqf5+weeG5dyAKF4AAAAAQG9UihdD0sFQKknZaPGi/mw2+NywnBsQxQsAAAAA6I2geGGHpIPBLYaKF+W3jTov3CE5NyCKFwAAAADQG0M0NmKtLY+N5PxPNOi8qHzO0nmB4UHxAgAAAAB6oYPihbW2u2cJChJB50WWzguMFooXAAAAANALtr3ihfU8eb//7+V9587unSUoSKTIvAg+Z2MyL6y1sg/+SHYIukmwuVC8AAAAAIBeaDf48tB+6cCz0pN7u3eWYP2pU5t5YUstbht54mfybvlD6aEfde9sQAoULwAAAACgF9odG9n3r5Iku/JC985SKvpvW+i8iL1tY90/23NPd+9sQAoULwAAAACgF9odG9n3hP9OV4sXwdhIENiZIvMirmMk+NyBZ7t3NiAFihcAAAAARor96b/I/c23y64eHfRRGmu78+Jx/+3RLhYv3DY6L+LOXS5eWIoX6DOKFwAAAABGit3/jLR2TDp6ZNBHaSy4+Letdl6UixerPei8cKKdF3HFi0adF+Xv5SDFC/QXxQsAAAAAo6WDFaR91cY5bakoPbfP75A4fsz/uBvKxQtT13kRNzbSoPMiWJ968Dk2jqCvKF4AAAAAGC1em1s8+q2dIsuz+/ziwVnn+h+vdGk0pm5VarudF+XPlUrSkUPdORuQAsULAAAAAKOlzSDMvvMabO1IEIyMmHMv8D/RrdDOulWpKTov3OTMC0nSgee6czYgBYoXAAAAAEZLcFHdQlFgINrJvNj3uOQ4Mi95mf9x14oX0cDONJ0X9ecOj4pYci/QRxQvAAAAAIyWNoMw+67B2Ij1PLl/cIO8e+6q/fy+x6WTT5O2LPkfd6t44dZ2XhhjpEymSeZFg00kkrSf4gX6h+IFAAAAgNFiG4w1DJNGmRduSXr2Kdnv3Fn7+X2Py2zbLs0t+h93e2wkm6t+zsk27ryIuy0oaDgOG0fQVxQvAAAAAIyWSlFgyMdGykWW2K0cQWFg7yP+6ldJdv24dGi/dOoZ0vSMXyBY6dI62OjYiOR//UadF3GdLcFtyyfLHqB4gf6heAEAAABgtLijEtjZ4Jzh7Ih77/bf2feEJMmceoY/1jG70LVtI7aFzgvbqPMiKGictE2ieIE+ongBAAAAYLRUVqWOSPEiroPB1hcvgk0jOvUM/+3cQvczL1rpvIjNvPDPbU4+VVo9Krt2vDvnA5qgeAEAAABgtNjRGBuxboMiS3DbKadL+x73CxdPPyFNTEpLW/3b5uZ7tyo1eD8286LBuYPX/KRt/lu6L9AnFC8AAAAAjJZRGRsJiiyxwZflDoafe41kMrL33i371L9K27bLZPzLNDO32IPAzjSdF8mrUoPvxZx0qv8xoZ3oE4oXAAAAAEbLqAR2NhobCW5bPEE69xWyP/xH6eknZIKREcnvvDjarbGRuMDOZp0XTTIvJNkDz3XnfEATFC8AAAAAjJZGWzyGScPAznJhIOP43RcHnvW7LE7dXr3P3IK0sSZb2Oj8LJWxkXBgp1MN5wyrBHY2GHeZmZdm5qQDz3R+NiCFbPO7AAAAAOil2267Tbt379bCwoJuuummutvvvvtuffWrX5UkTU5O6l3vepfOPPPMPp9yiHgNxjGGSaMOkeC2TEbmolfLfu6TkluSOfXM6n3mF/23K0elpRM7O0uleOFUP+c4rXdeBJ9zMtLySXReoG/ovAAAAAAG7KqrrtKHPvShxNu3bt2qD3/4w/roRz+qX/qlX9KnP/3pPp5uCI1K5kWaVamZjMzMrHT+Jf7Hoc4LMzfvv7NypPOzuCXJcSp5GpLKYyOtZl6UP2cyMltPofMCfUPnBQAAADBg5513nvbv3594+znnnFN5/+yzz9ahQ4f6cazhNSLbRqrnbDw2IkmZN79d9vSzZOa3VO8zF+q86FSpVLtpRGq/8yKTkTFGdvkkafc98aMnQJdRvAAAAABGyF133aWLL7449rZdu3Zp165dkqQbb7xRy8vLLX/9bDbb1uN6IeksR/N5rUmanZ7WdB/P2uprc3xqSiuSsk5GS5HHFY+9oMOS5hcXNLm8LC0vS698dc19SsUzdUjSrHU1FXl8q2c5mnO0nsvXPObw5JRkPZ0Q+ToHZeVKymezWozctjIxoeOO/9zHX/QSrbiuzJHDWl7emvosvTRMP7/ScJ1n1M9C8QIAAAAYEQ899JC+/e1v6z/9p/8Ue/uOHTu0Y8eOyscHDx5s+TmWl5fbelwvJJ3FO35MkrR69AUd7+NZW31tvKN+x0RpY6PucfaQ//HK6jGtJnxNW/I7Nlae2adjkfu0fJaVVdlMpuYxrudJG+t1X8ctFCRJhbW1utu8Y6tS+evYqVlJ0sa+J7RihyORYJh+fqXhOs+onGXbtm2xnx+OnzAAAAAADT3++OP61Kc+pd/93d/V3NzcoI8zWI2yJIZJw7GRIPPCqb8tMDklZXNdyrwoSrlc7ecSV6X6YyA2bmzE9cdGJEknnux/6tl9nZ8PaILiBQAAADDkDh48qI9+9KN673vfm/hXyU3Fa5DJMExSBnYmMcb461KPvtD5WRIzL+ICO4PXNyGrIyi4bFmSnKzc557u/HxAE4yNAAAAAAN28803a8+ePVpZWdHOnTt13XXXqVRebXnNNdfoS1/6klZXV/WZz3xGkuQ4jm688cZBHnmwRqXzIjifbRTY2eTvyXMLsqtdCuzMttZ5kVh0KZ/ZZBxpaSudF+gLihcAAADAgL3//e9vePvOnTu1c+fOPp1mBAQX1e6IFC/a7LyQJM0vSEc7Hxuxbn3nhXGc+E0hQUEjrrDheX7HRmD5JLn7WZeK3mNsBAAAAMBo8UZkVWqq4kWDzAtJZnZB6lrnRXRspEnnRVzHSDjzQpKmpmTX1zo/H9AExQsAAAAAI6USJDkqYyNJ2RFSus6LlSOy1nZ2FjemeJHNNs68iO28cGsKLiaXly1sdHY2IAWKFwAAAABGy8hkXjQKvkw5NjK7IBUK0sZ6Z2cpFRMCO2sLFNbaUGBnwthIuFskl5coXqAPKF4AAAAAGC0jNzaSUASQmo6NaH7Bf7vS4caRxLG
"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",
"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()"
]
},
2020-04-22 22:32:10 +08:00
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
2020-06-29 22:31:43 +08:00
"display_name": "Python [conda env:jupyter] *",
2020-04-22 22:32:10 +08:00
"language": "python",
2020-06-29 22:31:43 +08:00
"name": "conda-env-jupyter-py"
2020-04-22 22:32:10 +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",
2020-06-29 22:31:43 +08:00
"version": "3.8.3"
2020-04-22 22:32:10 +08:00
}
},
"nbformat": 4,
"nbformat_minor": 4
}
