12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
mpc_python_learn/notebooks/2.0-MPC-base.ipynb

731 lines
150 KiB
Plaintext
Raw Normal View History

reorganizing my jupyter notebooks
2021-04-13 18:30:08 +08:00
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# 2 MPC\n",
"This notebook contains the CVXPY implementation of a MPC\n",
"This is the simplest one"
]
},
{
"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": [
"![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)"
]
},
{
"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",
"In this case, the objective function to minimize is given by:\n",
"\n",
"https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf\n",
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\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})^TP(u_{j} - u_{j-1}) \\\\\n",
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
" & 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",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"\n",
"Where R,P,Q are the cost matrices used to tune the response.\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## PRELIMINARIES\n",
"\n",
"* linearized system dynamics\n",
"* function to represent the track\n",
"* function to represent the **reference trajectory** to track"
]
},
{
"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": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"\"\"\"\n",
"Control problem statement.\n",
"\"\"\"\n",
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"DT = 0.2 #discretization step\n",
"\n",
"def get_linear_model(x_bar,u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
" \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" \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",
" A[1,2]=np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n",
" A_lin=np.eye(N)+DT*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
" B[3,1]=v/(L*np.cos(delta)**2)\n",
" B_lin=DT*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"def kinematics_model(x,t,u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
" \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" 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",
" x_ = np.zeros((N,T+1))\n",
" \n",
" x_[:,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_[:,t]=x_next[1]\n",
" \n",
" return x_\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
"\n",
" interp_range = np.linspace(0,1,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",
" 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",
"\n",
"\n",
"def get_nn_idx(state,path):\n",
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.hypot(dx,dy)\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 get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" Adapted from pythonrobotics\n",
" \"\"\"\n",
" 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",
" ind = get_nn_idx(state, path)\n",
"\n",
" xref[0, 0] = path[0,ind] #X\n",
" xref[1, 0] = path[1,ind] #Y\n",
" 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",
" \n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n",
"\n",
" for i in range(T + 1):\n",
" travel += abs(target_v) * DT #current V or target V?\n",
" 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",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
" xref[3, i] = path[2,ncourse - 1]\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## MPC \n",
"\n",
"test single iteration"
]
},
{
"cell_type": "code",
"execution_count": 3,
"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",
"problem: variables n = 326, constraints m = 408\n",
" nnz(P) + nnz(A) = 1063\n",
"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",
" 1 0.0000e+00 4.27e+00 4.67e+02 1.00e-01 4.07e-04s\n",
" 175 1.6965e+02 2.63e-05 3.49e-05 7.14e+00 1.86e-03s\n",
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
"number of iterations: 175\n",
"optimal objective: 169.6454\n",
"run time: 2.50e-03s\n",
"optimal rho estimate: 6.34e+00\n",
"\n",
"CPU times: user 122 ms, sys: 75 µs, total: 122 ms\n",
"Wall time: 119 ms\n"
]
}
],
"source": [
"%%time\n",
"\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_ACC = 1.0\n",
"REF_VEL=1.0\n",
"\n",
"track = compute_path_from_wp([0,3,6],\n",
" [0,0,0],0.05)\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.1 #delta\n",
"\n",
"# dynamics starting state w.r.t world frame\n",
"x_bar = np.zeros((N,T+1))\n",
"x_bar[:,0] = x0\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",
"x = cp.Variable((N, T+1))\n",
"u = cp.Variable((M, T))\n",
"cost = 0\n",
"constr = []\n",
"\n",
"# Cost Matrices\n",
"Q = np.diag([10,10,10,10]) #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",
"\n",
"#Prediction Horizon\n",
"for t in range(T):\n",
" \n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
" \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",
" 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",
"#Final Point tracking\n",
"cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
"# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
"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",
"\n",
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
"solution = prob.solve(solver=cp.OSQP, verbose=True)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
"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",
"delta_mpc=np.array(u.value[1, :]).flatten()\n",
"\n",
"#simulate robot state trajectory for optimized U\n",
"x_traj=predict(x0, np.vstack((a_mpc,delta_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_ref[0,:],x_ref[1,:],\"g+\")\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, 3)\n",
"plt.plot(a_mpc)\n",
"plt.ylabel('a_in(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(theta_mpc)\n",
"plt.ylabel('theta(t)')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(delta_mpc)\n",
"plt.ylabel('d_in(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## full track demo"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"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"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"CVXPY Optimization Time: Avrg: 0.1677s Max: 0.2731s Min: 0.1438s\n"
]
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
"sim_duration = 200 #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.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
" # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\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",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state error cost\n",
" Qf = np.diag([30,30,30,0]) #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",
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\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",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" 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",
" \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",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\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, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python [conda env:.conda-jupyter] *",
"language": "python",
"name": "conda-env-.conda-jupyter-py"
},
"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.8.5"
}
},
"nbformat": 4,
"nbformat_minor": 4
}