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",
2024-10-23 16:26:31 +08:00
"本篇笔记包含了一个基于CVXPY的MPC实现\n",
"This is the simplest one\n",
"这是最简单的一个"
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2024-10-23 16:26:31 +08:00
"### MPC Problem formulation MPC问题的表述\n",
2021-04-13 18:30:08 +08:00
"\n",
2024-10-23 16:26:31 +08:00
"**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step.\n",
"**模型预测控制**是指在每个时间步上**数值**地解决一个优化问题的控制方法。 \n",
2021-04-13 18:30:08 +08:00
"\n",
2024-10-23 16:26:31 +08:00
"The controller generates a control signal over a fixed lenght T (Horizon) at each time step.\n",
"控制器在每个时间步上生成一个固定长度T( 视野) 的控制信号。"
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2024-10-23 16:26:31 +08:00
"#### Linear MPC Formulation 线性MPC表述\n",
2021-04-13 18:30:08 +08:00
"\n",
"Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n",
2024-10-23 16:26:31 +08:00
"线性MPC使用**LTI**(线性时不变)离散状态空间模型,该模型表示用于预测的运动模型。\n",
2021-04-13 18:30:08 +08:00
"\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",
2024-10-23 16:26:31 +08:00
"LTI表述意味着**未来状态**与当前状态和执行器信号之间是线性相关的。因此, MPC试图找到一个有限长度视野上的**控制策略**U。\n",
2021-04-13 18:30:08 +08:00
"\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",
2024-10-23 16:26:31 +08:00
"用于最小化( 将状态驱动至0) 的目标函数为: \n",
2021-04-13 18:30:08 +08:00
"\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",
2024-10-23 16:26:31 +08:00
"其他线性约束也可以应用,比如对控制变量的约束\n",
2021-04-13 18:30:08 +08:00
"\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",
2024-10-23 16:26:31 +08:00
"目标函数考虑了状态偏离0的二次误差以及控制输入序列的二次误差。矩阵Q和R是权重矩阵, 用于调整系统响应\n",
2021-04-13 18:30:08 +08:00
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
2024-10-23 16:26:31 +08:00
"由于目标是跟踪参考信号(例如轨迹),目标函数被重写为:\n",
2021-04-13 18:30:08 +08:00
"\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",
2024-10-23 16:26:31 +08:00
"其中考虑了相对于期望状态的误差:\n",
2021-04-13 18:30:08 +08:00
"\n",
"$ \\delta x = x_{j,t,ref} - x_{j,t} $"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2024-10-23 16:26:31 +08:00
"# Problem Formulation: Study case 研究案例问题表述\n",
2021-04-13 18:30:08 +08:00
"\n",
"In this case, the objective function to minimize is given by:\n",
2024-10-23 16:26:31 +08:00
"在这种情况下,要最小化的目标函数为:\n",
2021-04-13 18:30:08 +08:00
"\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",
2024-10-23 16:26:31 +08:00
"Where R,P,Q are the cost matrices used to tune the response.\n",
"R,P,Q是用于调整响应的成本矩阵。\n"
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2024-10-23 16:26:31 +08:00
"## PRELIMINARIES 准备工作\n",
2021-04-13 18:30:08 +08:00
"\n",
2024-10-23 16:26:31 +08:00
"* linearized system dynamics 线性化系统动力学\n",
"* function to represent the track 轨迹函数\n",
"* function to represent the **reference trajectory** to track 跟踪的**参考轨迹**函数"
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "code",
"execution_count": 1,
2024-10-23 16:26:31 +08:00
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T06:09:14.320605Z",
"start_time": "2024-10-23T06:09:13.499103Z"
}
},
2021-04-13 18:30:08 +08:00
"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,
2024-10-23 16:26:31 +08:00
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T06:09:15.094392Z",
"start_time": "2024-10-23T06:09:15.092291Z"
}
},
2021-04-13 18:30:08 +08:00
"outputs": [],
"source": [
"\"\"\"\n",
"Control problem statement.\n",
2024-10-23 16:26:31 +08:00
"控制问题陈述。\n",
2021-04-13 18:30:08 +08:00
"\"\"\"\n",
"\n",
2024-10-23 16:26:31 +08:00
"N = 4 # number of state variables (x, y, v, theta)\n",
"M = 2 # number of control variables (a, delta)\n",
"T = 20 # Prediction Horizon (time steps)\n",
"DT = 0.2 # discretization step [s]\n",
2022-08-02 16:33:49 +08:00
"\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",
2024-10-23 16:26:31 +08:00
" 计算LTI近似状态空间模型x' = Ax + Bu + C\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\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",
2024-10-23 16:26:31 +08:00
"ODE用于根据mpc结果更新仿真, 而不是两次使用LTI\n",
2021-04-13 18:30:08 +08:00
"\"\"\"\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",
2024-10-23 16:26:31 +08:00
" 返回车辆模型的ODE模型\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\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",
2024-10-23 16:26:31 +08:00
" \"\"\"\n",
" 预测车辆的真实轨迹, 使用ODE求解 \n",
" \"\"\"\n",
2022-08-02 16:33:49 +08:00
"\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",
2024-10-23 16:26:31 +08:00
" 给定一组路径点,计算参考路径\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\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",
2024-10-23 16:26:31 +08:00
" 计算最接近车辆的路径点的索引\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\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",
2024-10-23 16:26:31 +08:00
" 获取参考轨迹\n",
" 从当前位置开始, 截取路径上的一段作为参考轨迹, 其中v=固定的, 参考的方向转角为0\n",
2021-04-13 18:30:08 +08:00
" \"\"\"\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",
2024-10-23 16:26:31 +08:00
"test single iteration\n",
"测试单次迭代"
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "code",
2024-10-23 16:26:31 +08:00
"execution_count": 40,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T08:23:39.847687Z",
"start_time": "2024-10-23T08:23:39.780867Z"
}
},
2021-04-13 18:30:08 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2024-10-23 16:26:31 +08:00
"===============================================================================\n",
" CVXPY \n",
" v1.5.3 \n",
"===============================================================================\n",
"(CVXPY) Oct 23 04:23:39 PM: Your problem has 124 variables, 166 constraints, and 0 parameters.\n",
"(CVXPY) Oct 23 04:23:39 PM: It is compliant with the following grammars: DCP, DQCP\n",
"(CVXPY) Oct 23 04:23:39 PM: (If you need to solve this problem multiple times, but with different data, consider using parameters.)\n",
"(CVXPY) Oct 23 04:23:39 PM: CVXPY will first compile your problem; then, it will invoke a numerical solver to obtain a solution.\n",
"(CVXPY) Oct 23 04:23:39 PM: Your problem is compiled with the CPP canonicalization backend.\n",
"-------------------------------------------------------------------------------\n",
" Compilation \n",
"-------------------------------------------------------------------------------\n",
"(CVXPY) Oct 23 04:23:39 PM: Compiling problem (target solver=OSQP).\n",
"(CVXPY) Oct 23 04:23:39 PM: Reduction chain: CvxAttr2Constr -> Qp2SymbolicQp -> QpMatrixStuffing -> OSQP\n",
"(CVXPY) Oct 23 04:23:39 PM: Applying reduction CvxAttr2Constr\n",
"(CVXPY) Oct 23 04:23:39 PM: Applying reduction Qp2SymbolicQp\n",
"(CVXPY) Oct 23 04:23:39 PM: Applying reduction QpMatrixStuffing\n",
"(CVXPY) Oct 23 04:23:39 PM: Applying reduction OSQP\n",
"(CVXPY) Oct 23 04:23:39 PM: Finished problem compilation (took 2.905e-02 seconds).\n",
"-------------------------------------------------------------------------------\n",
" Numerical solver \n",
"-------------------------------------------------------------------------------\n",
"(CVXPY) Oct 23 04:23:39 PM: Invoking solver OSQP to obtain a solution.\n",
"-------------------------------------------------------------------------------\n",
" Summary \n",
"-------------------------------------------------------------------------------\n",
"(CVXPY) Oct 23 04:23:39 PM: Problem status: optimal\n",
"(CVXPY) Oct 23 04:23:39 PM: Optimal value: 5.630e+02\n",
"(CVXPY) Oct 23 04:23:39 PM: Compilation took 2.905e-02 seconds\n",
"(CVXPY) Oct 23 04:23:39 PM: Solver (including time spent in interface) took 3.982e-03 seconds\n",
"CPU times: user 60.9 ms, sys: 5.23 ms, total: 66.1 ms\n",
"Wall time: 62.7 ms\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:27: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[0, 2] = np.cos(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:28: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[0, 3] = -v * np.sin(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:29: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[1, 2] = np.sin(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:30: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[1, 3] = v * np.cos(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:31: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[3, 2] = v * np.tan(delta) / L\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:36: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n"
2021-04-13 18:30:08 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
2024-10-23 16:26:31 +08:00
"# 限制条件\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",
2024-10-23 16:26:31 +08:00
"REF_VEL = 1.0 # 目标路径参考速度\n",
2021-04-13 18:30:08 +08:00
"\n",
2024-10-23 16:26:31 +08:00
"#获取参考轨迹,线性插值,三个点[0,0],[3,0],[6,0]\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",
2024-10-23 16:26:31 +08:00
"# Starting Condition 初始条件\n",
2021-04-13 18:30:08 +08:00
"x0 = np.zeros(N)\n",
2022-08-02 16:33:49 +08:00
"x0[0] = 0 # x\n",
2024-10-23 16:26:31 +08:00
"x0[1] = -0.5 # y\n",
2022-08-02 16:33:49 +08:00
"x0[2] = 0.0 # v\n",
2024-10-23 16:26:31 +08:00
"x0[3] = np.radians(-80) # yaw\n",
2022-08-02 16:33:49 +08:00
"\n",
2024-10-23 16:26:31 +08:00
"# starting guess 开始猜测\n",
2022-08-02 16:33:49 +08:00
"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",
2024-10-23 16:26:31 +08:00
"# dynamics starting state w.r.t world frame 与世界坐标系相关的动力学起始状态\n",
"x_bar = np.zeros((N, T + 1)) # 4x21\n",
2022-08-02 16:33:49 +08:00
"x_bar[:, 0] = x0\n",
"\n",
2024-10-23 16:26:31 +08:00
"# prediction for linearization of costrains 用于约束线性化的预测\n",
"# 这部分应用线性模型,得到预测的轨迹\n",
2022-08-02 16:33:49 +08:00
"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",
2024-10-23 16:26:31 +08:00
" A, B, C = get_linear_model(xt, ut) # 获取在t - 1时刻的线性近似模型\n",
2022-08-02 16:33:49 +08:00
" xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
2024-10-23 16:26:31 +08:00
" x_bar[:, t] = xt_plus_one # 获取t时刻的状态\n",
2022-08-02 16:33:49 +08:00
"\n",
2024-10-23 16:26:31 +08:00
"# x_bar是根据猜测的u_bar获取的预测状态估计\n",
"\n",
"# CVXPY Linear MPC problem statement CVXPY线性MPC问题陈述\n",
"x = cp.Variable((N, T + 1)) # 4x21维, 状态向量\n",
"u = cp.Variable((M, T)) # 2x20维, 控制向量\n",
2021-04-13 18:30:08 +08:00
"cost = 0\n",
"constr = []\n",
"\n",
"# Cost Matrices\n",
2024-10-23 16:26:31 +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",
2024-10-23 16:26:31 +08:00
"# Get Reference_traj 获取参考轨迹,根据当前位置截取的路径上的一系列点,并赋值目标速度和转角\n",
"# x_ref 表示参考状态, d_ref表示参考转角\n",
2022-08-02 16:33:49 +08:00
"x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
2021-04-13 18:30:08 +08:00
"\n",
2024-10-23 16:26:31 +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",
2024-10-23 16:26:31 +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",
2024-10-23 16:26:31 +08:00
" # Actuation effort 执行努力\n",
2022-08-02 16:33:49 +08:00
" cost += cp.quad_form(u[:, t], R)\n",
"\n",
2024-10-23 16:26:31 +08:00
" # Actuation rate of change 变化率\n",
2021-04-13 18:30:08 +08:00
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
"\n",
2024-10-23 16:26:31 +08:00
" # 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",
2024-10-23 16:26:31 +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",
2024-10-23 16:26:31 +08:00
"# 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",
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",
2024-10-23 16:26:31 +08:00
"prob = cp.Problem(cp.Minimize(cost), constr) # 构建问题\n",
"solution = prob.solve(solver=cp.OSQP, verbose=True) # 求解"
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "code",
2024-10-23 16:26:31 +08:00
"execution_count": 41,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T08:23:43.320874Z",
"start_time": "2024-10-23T08:23:43.203207Z"
}
},
2021-04-13 18:30:08 +08:00
"outputs": [
{
"data": {
2024-10-23 16:26:31 +08:00
"text/plain": "<Figure size 640x480 with 4 Axes>",
"image/png": "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
2021-04-13 18:30:08 +08:00
},
"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",
2024-10-23 16:26:31 +08:00
"plt.plot(x_traj[0, :], x_traj[1, :]) #根据mpc优化后的a和delta, 预测的轨迹\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",
2024-10-23 16:26:31 +08:00
"plt.plot(theta_mpc) \n",
"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",
2024-10-23 16:26:31 +08:00
"plt.ylabel(\"d_in(t)\") # 前轮转角\n",
2021-04-13 18:30:08 +08:00
"\n",
"plt.tight_layout()\n",
2024-10-23 16:26:31 +08:00
"plt.show()\n",
"\n",
"# 下图展示的结果并不准确\n",
"# 这是因为在做约束条件中, xt+1 = Axt + But + C时, A, B, C是线性化的模型, 且是基于猜测的x_bar和u_bar来线性化的\n",
"# 所以需要通过滚动优化,获得更准确的猜测和状态猜测\n",
"# 这里有一个问题, 那是否要将这次计算的u, 作为下一次的猜测基础? "
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2024-10-23 16:26:31 +08:00
"## full track demo "
2021-04-13 18:30:08 +08:00
]
},
{
"cell_type": "code",
2024-10-23 16:26:31 +08:00
"execution_count": 38,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T08:19:27.567266Z",
"start_time": "2024-10-23T08:19:11.796354Z"
}
},
2021-04-13 18:30:08 +08:00
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
2024-10-23 16:26:31 +08:00
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:27: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[0, 2] = np.cos(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:28: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[0, 3] = -v * np.sin(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:29: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[1, 2] = np.sin(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:30: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[1, 3] = v * np.cos(theta)\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:31: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" A[3, 2] = v * np.tan(delta) / L\n",
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_12777/1770301921.py:36: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n"
2021-04-13 18:30:08 +08:00
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
2024-10-23 16:26:31 +08:00
"CVXPY Optimization Time: Avrg: 0.0791s Max: 0.1699s Min: 0.0546s\n"
2021-04-13 18:30:08 +08:00
]
}
],
"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",
2024-10-23 16:26:31 +08:00
"x_sim[0,0] = 0 # x\n",
"x_sim[1,0] = -0.5 # y\n",
"x_sim[2,0] = 0.0 # v\n",
"x_sim[3,0] = np.radians(-60.0) # yaw\n",
2022-08-02 16:33:49 +08:00
"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",
2024-10-23 16:26:31 +08:00
"x0 = x_sim[:, 0]\n",
2022-08-02 16:33:49 +08:00
"\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",
2024-10-23 16:26:31 +08:00
" # 获取当前时刻的状态, x_sim是通过ode计算出的真值\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",
2024-10-23 16:26:31 +08:00
" # 获取各参考点处的线性化模型参数\n",
2022-08-02 16:33:49 +08:00
" 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",
2024-10-23 16:26:31 +08:00
" # 构建MPC问题和求解器\n",
2022-08-02 16:33:49 +08:00
" 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",
2024-10-23 16:26:31 +08:00
" # 将本次计算出的u_bar作为下次猜测的起点\n",
2022-08-02 16:33:49 +08:00
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
2024-10-23 16:26:31 +08:00
" \n",
" # 本次的执行值\n",
2022-08-02 16:33:49 +08:00
" 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",
2024-10-23 16:26:31 +08:00
" # 用ode模型仿真车辆运动\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",
2024-10-23 16:26:31 +08:00
"execution_count": 39,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T08:19:28.719495Z",
"start_time": "2024-10-23T08:19:28.541069Z"
}
},
2021-04-13 18:30:08 +08:00
"outputs": [
{
"data": {
2024-10-23 16:26:31 +08:00
"text/plain": "<Figure size 1500x1000 with 5 Axes>",
"image/png": "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
2021-04-13 18:30:08 +08:00
},
"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",
2024-10-23 16:26:31 +08:00
"plt.plot(track[0, 0:200], track[1, 0:200], \"b+\")\n",
"plt.plot(x_sim[0, 0:50], x_sim[1, 0:50])\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()"
]
2024-10-23 16:26:31 +08:00
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [],
"metadata": {
"collapsed": false
}
2021-04-13 18:30:08 +08:00
}
],
"metadata": {
"kernelspec": {
2024-10-23 16:26:31 +08:00
"name": "python3",
2021-04-13 18:30:08 +08:00
"language": "python",
2024-10-23 16:26:31 +08:00
"display_name": "Python 3 (ipykernel)"
2021-04-13 18:30:08 +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",
"version": "3.8.5"
}
},
"nbformat": 4,
"nbformat_minor": 4
}