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": [
"\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} $"
]
},
{
"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",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-13 18:30:08 +08:00
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"\"\"\"\n",
"Control problem statement.\n",
"\"\"\"\n",
"\n",
2022-08-02 16:33:49 +08:00
"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",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"def get_linear_model(x_bar, u_bar):\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
" \"\"\"\n",
2022-08-02 16:33:49 +08:00
"\n",
" L = 0.3 # vehicle wheelbase\n",
"\n",
2021-04-13 18:30:08 +08:00
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-13 18:30:08 +08:00
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
2022-08-02 16:33:49 +08:00
"\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(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
2021-04-13 18:30:08 +08:00
"\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",
2022-08-02 16:33:49 +08:00
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
" \"\"\"\n",
2022-08-02 16:33:49 +08:00
"\n",
" L = 0.3 # vehicle wheelbase\n",
" dxdt = x[2] * np.cos(x[3])\n",
" dydt = x[2] * np.sin(x[3])\n",
2021-04-13 18:30:08 +08:00
" dvdt = u[0]\n",
2022-08-02 16:33:49 +08:00
" dthetadt = x[2] * np.tan(u[1]) / L\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
2021-04-13 18:30:08 +08:00
"\n",
" return dqdt\n",
"\n",
2022-08-02 16:33:49 +08:00
"\n",
"def predict(x0, u):\n",
" \"\"\" \"\"\"\n",
"\n",
" x_ = np.zeros((N, T + 1))\n",
"\n",
" x_[:, 0] = x0\n",
"\n",
2021-04-13 18:30:08 +08:00
" # solve ODE\n",
2022-08-02 16:33:49 +08:00
" for t in range(1, T + 1):\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
2021-04-13 18:30:08 +08:00
"\n",
" x0 = x_next[1]\n",
2022-08-02 16:33:49 +08:00
" x_[:, t] = x_next[1]\n",
"\n",
2021-04-13 18:30:08 +08:00
" return x_\n",
"\n",
2022-08-02 16:33:49 +08:00
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
2022-08-02 16:33:49 +08:00
"\n",
" final_xp = []\n",
" final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\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",
2021-04-13 18:30:08 +08:00
" 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",
2022-08-02 16:33:49 +08:00
" return np.vstack((final_xp, final_yp, theta))\n",
2021-04-13 18:30:08 +08:00
"\n",
"\n",
2022-08-02 16:33:49 +08:00
"def get_nn_idx(state, path):\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
2022-08-02 16:33:49 +08:00
" dx = state[0] - path[0, :]\n",
" dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx, dy)\n",
2021-04-13 18:30:08 +08:00
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
2022-08-02 16:33:49 +08:00
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
2021-04-13 18:30:08 +08:00
" v /= np.linalg.norm(v)\n",
"\n",
2022-08-02 16:33:49 +08:00
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
" if np.dot(d, v) > 0:\n",
2021-04-13 18:30:08 +08:00
" target_idx = nn_idx\n",
" else:\n",
2022-08-02 16:33:49 +08:00
" target_idx = nn_idx + 1\n",
2021-04-13 18:30:08 +08:00
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx\n",
"\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-13 18:30:08 +08:00
"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",
2022-08-02 16:33:49 +08:00
"\n",
" # sp = np.ones((1,T +1))*target_v #speed profile\n",
"\n",
2021-04-13 18:30:08 +08:00
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
"\n",
2022-08-02 16:33:49 +08:00
" 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",
2021-04-13 18:30:08 +08:00
" travel = 0.0\n",
"\n",
" for i in range(T + 1):\n",
2022-08-02 16:33:49 +08:00
" travel += abs(target_v) * DT # current V or target V?\n",
2021-04-13 18:30:08 +08:00
" dind = int(round(travel / dl))\n",
"\n",
" if (ind + dind) < ncourse:\n",
2022-08-02 16:33:49 +08:00
" 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",
2021-04-13 18:30:08 +08:00
" dref[0, i] = 0.0\n",
" else:\n",
2022-08-02 16:33:49 +08:00
" 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",
2021-04-13 18:30:08 +08:00
" 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",
2022-08-02 16:33:49 +08:00
"MAX_SPEED = 1.5 # m/s\n",
"MAX_STEER = np.radians(30) # rad\n",
2021-04-13 18:30:08 +08:00
"MAX_ACC = 1.0\n",
2022-08-02 16:33:49 +08:00
"REF_VEL = 1.0\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"track = compute_path_from_wp([0, 3, 6], [0, 0, 0], 0.05)\n",
2021-04-13 18:30:08 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
2022-08-02 16:33:49 +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",
"\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",
2021-04-13 18:30:08 +08:00
"\n",
"# dynamics starting state w.r.t world frame\n",
2022-08-02 16:33:49 +08:00
"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",
2021-04-13 18:30:08 +08:00
"u = cp.Variable((M, T))\n",
"cost = 0\n",
"constr = []\n",
"\n",
"# Cost Matrices\n",
2022-08-02 16:33:49 +08:00
"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",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"# Get Reference_traj\n",
"x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"# Prediction Horizon\n",
2021-04-13 18:30:08 +08:00
"for t in range(T):\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-13 18:30:08 +08:00
" # Tracking Error\n",
2022-08-02 16:33:49 +08:00
" cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
2021-04-13 18:30:08 +08:00
"\n",
" # Actuation effort\n",
2022-08-02 16:33:49 +08:00
" cost += cp.quad_form(u[:, t], R)\n",
"\n",
2021-04-13 18:30:08 +08:00
" # 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",
2022-08-02 16:33:49 +08:00
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
" constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"# Final Point tracking\n",
2021-04-13 18:30:08 +08:00
"cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
"# sums problem objectives and concatenates constraints.\n",
2022-08-02 16:33:49 +08:00
"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",
2021-04-13 18:30:08 +08:00
"# 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": [
2022-08-02 16:33:49 +08:00
"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",
2021-04-13 18:30:08 +08:00
"plt.subplot(2, 2, 1)\n",
2022-08-02 16:33:49 +08:00
"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",
2021-04-13 18:30:08 +08:00
"plt.axis(\"equal\")\n",
2022-08-02 16:33:49 +08:00
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"# plot v(t)\n",
2021-04-13 18:30:08 +08:00
"plt.subplot(2, 2, 3)\n",
"plt.plot(a_mpc)\n",
2022-08-02 16:33:49 +08:00
"plt.ylabel(\"a_in(t)\")\n",
"# plt.xlabel('time')\n",
2021-04-13 18:30:08 +08:00
"\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(theta_mpc)\n",
2022-08-02 16:33:49 +08:00
"plt.ylabel(\"theta(t)\")\n",
2021-04-13 18:30:08 +08:00
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(delta_mpc)\n",
2022-08-02 16:33:49 +08:00
"plt.ylabel(\"d_in(t)\")\n",
2021-04-13 18:30:08 +08:00
"\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": [
2022-08-02 16:33:49 +08:00
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
2021-04-13 18:30:08 +08:00
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
2022-08-02 16:33:49 +08:00
"sim_duration = 200 # time steps\n",
"opt_time = []\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"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",
2021-04-13 18:30:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"REF_VEL = 1.0 # m/s\n",
2021-04-13 18:30:08 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
2022-08-02 16:33:49 +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",
"\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",
2021-04-13 18:30:08 +08:00
" iter_start = time.time()\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-13 18:30:08 +08:00
" # dynamics starting state\n",
2022-08-02 16:33:49 +08:00
" 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",
2021-04-13 18:30:08 +08:00
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
2022-08-02 16:33:49 +08:00
" 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",
2021-04-13 18:30:08 +08:00
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
2022-08-02 16:33:49 +08:00
" cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
2021-04-13 18:30:08 +08:00
"\n",
" # Actuation effort\n",
2022-08-02 16:33:49 +08:00
" cost += cp.quad_form(u[:, t], R)\n",
2021-04-13 18:30:08 +08:00
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
2022-08-02 16:33:49 +08:00
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
2021-04-13 18:30:08 +08:00
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
2022-08-02 16:33:49 +08:00
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
" constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
"\n",
" # Final Point tracking\n",
2021-04-13 18:30:08 +08:00
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
2022-08-02 16:33:49 +08:00
" 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",
2021-04-13 18:30:08 +08:00
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
2022-08-02 16:33:49 +08:00
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
2021-04-13 18:30:08 +08:00
" # Measure elpased time to get results from cvxpy\n",
2022-08-02 16:33:49 +08:00
" opt_time.append(time.time() - iter_start)\n",
"\n",
2021-04-13 18:30:08 +08:00
" # move simulation to t+1\n",
2022-08-02 16:33:49 +08:00
" tspan = [0, DT]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
2021-04-13 18:30:08 +08:00
]
},
{
"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": [
2022-08-02 16:33:49 +08:00
"# plot trajectory\n",
2021-04-13 18:30:08 +08:00
"grid = plt.GridSpec(4, 5)\n",
"\n",
2022-08-02 16:33:49 +08:00
"plt.figure(figsize=(15, 10))\n",
2021-04-13 18:30:08 +08:00
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
2022-08-02 16:33:49 +08:00
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
2021-04-13 18:30:08 +08:00
"plt.axis(\"equal\")\n",
2022-08-02 16:33:49 +08:00
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
2021-04-13 18:30:08 +08:00
"\n",
"plt.subplot(grid[0, 4])\n",
2022-08-02 16:33:49 +08:00
"plt.plot(u_sim[0, :])\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
2021-04-13 18:30:08 +08:00
"\n",
"plt.subplot(grid[1, 4])\n",
2022-08-02 16:33:49 +08:00
"plt.plot(x_sim[2, :])\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
2021-04-13 18:30:08 +08:00
"\n",
"plt.subplot(grid[2, 4])\n",
2022-08-02 16:33:49 +08:00
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
2021-04-13 18:30:08 +08:00
"\n",
"plt.subplot(grid[3, 4])\n",
2022-08-02 16:33:49 +08:00
"plt.plot(x_sim[3, :])\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
2021-04-13 18:30:08 +08:00
"\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
}
