mpc_python_learn/notebooks/2.3-MPC-simplified.ipynb

561 lines
192 KiB
Plaintext
Raw Normal View History

2021-04-15 21:46:12 +08:00
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
2024-10-23 17:34:26 +08:00
"# Simplified MPC 简化MPC\n",
2021-04-15 21:46:12 +08:00
"\n",
2024-10-23 17:34:26 +08:00
"Checking the implementation from [Reuben Ferrante](https://github.com/arex18/rocket-lander)\n",
"参考[Reuben Ferrante]的实现 \n",
2021-04-15 21:46:12 +08:00
"\n",
"Here the system dynamics matrices are evaluated only once given the current state, input -> So no more need to keep track of **x_bar** and **u_bar**.\n",
2024-10-23 17:34:26 +08:00
"此处系统动力学矩阵仅在给定当前状态和输入时评估一次->因此不再需要跟踪**x_bar**和**u_bar**。\n",
2021-04-15 21:46:12 +08:00
"\n",
2024-10-23 17:34:26 +08:00
"This should give less precise results but the computation time should be gratly reduced and the overall code is slimmer, worth checking out!\n",
"这应该给出不太精确的结果,但计算时间应该大大减少,整体代码更简洁,值得一试!"
2021-04-15 21:46:12 +08:00
]
},
{
"cell_type": "code",
"execution_count": 1,
2024-10-23 17:34:26 +08:00
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T09:14:29.961291Z",
"start_time": "2024-10-23T09:14:29.224486Z"
}
},
2021-04-15 21:46:12 +08:00
"outputs": [],
"source": [
"import numpy as np\n",
"import cvxpy as opt\n",
"import time\n",
"from scipy.integrate import odeint\n",
"from scipy.interpolate import interp1d\n",
"import matplotlib.pyplot as plt\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-15 21:46:12 +08:00
"plt.style.use(\"ggplot\")\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 = 10 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"L = 0.3 # vehicle wheelbase\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",
2021-04-15 21:46:12 +08:00
"\n",
"\n",
2022-08-02 16:33:49 +08:00
"def get_linear_model_matrices(x_bar, u_bar):\n",
2021-04-15 21:46:12 +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",
2021-04-15 21:46:12 +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-15 21:46:12 +08:00
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-15 21:46:12 +08:00
" ct = np.cos(theta)\n",
" st = np.sin(theta)\n",
" cd = np.cos(delta)\n",
" td = np.tan(delta)\n",
2022-08-02 16:33:49 +08:00
"\n",
" A = np.zeros((N, N))\n",
" A[0, 2] = ct\n",
" A[0, 3] = -v * st\n",
" A[1, 2] = st\n",
" A[1, 3] = v * ct\n",
" A[3, 2] = v * td / 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 * cd**2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu = np.array([v * ct, v * st, a, v * td / L]).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",
" ) # .flatten()\n",
"\n",
" # return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)\n",
2021-04-15 21:46:12 +08:00
" return A_lin, B_lin, C_lin\n",
"\n",
2022-08-02 16:33:49 +08:00
"\n",
"class MPC:\n",
2021-04-19 23:18:08 +08:00
" def __init__(self, N, M, Q, R):\n",
2022-08-02 16:33:49 +08:00
" \"\"\" \"\"\"\n",
2021-04-15 21:46:12 +08:00
" self.state_len = N\n",
" self.action_len = M\n",
2021-04-19 23:18:08 +08:00
" self.state_cost = Q\n",
" self.action_cost = R\n",
2022-08-02 16:33:49 +08:00
"\n",
" def optimize_linearized_model(\n",
" self,\n",
" A,\n",
" B,\n",
" C,\n",
" initial_state,\n",
" target,\n",
" time_horizon=10,\n",
" Q=None,\n",
" R=None,\n",
" verbose=False,\n",
" ):\n",
2021-04-15 21:46:12 +08:00
" \"\"\"\n",
" Optimisation problem defined for the linearised model,\n",
2022-08-02 16:33:49 +08:00
" :param A:\n",
2021-04-15 21:46:12 +08:00
" :param B:\n",
2022-08-02 16:33:49 +08:00
" :param C:\n",
2021-04-15 21:46:12 +08:00
" :param initial_state:\n",
" :param Q:\n",
" :param R:\n",
" :param target:\n",
" :param time_horizon:\n",
" :param verbose:\n",
" :return:\n",
" \"\"\"\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-15 21:46:12 +08:00
" assert len(initial_state) == self.state_len\n",
2022-08-02 16:33:49 +08:00
"\n",
" if Q == None or R == None:\n",
2021-04-19 23:18:08 +08:00
" Q = self.state_cost\n",
" R = self.action_cost\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-15 21:46:12 +08:00
" # Create variables\n",
2022-08-02 16:33:49 +08:00
" x = opt.Variable((self.state_len, time_horizon + 1), name=\"states\")\n",
" u = opt.Variable((self.action_len, time_horizon), name=\"actions\")\n",
2021-04-15 21:46:12 +08:00
"\n",
" # Loop through the entire time_horizon and append costs\n",
" cost_function = []\n",
"\n",
" for t in range(time_horizon):\n",
"\n",
2022-08-02 16:33:49 +08:00
" _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) + opt.quad_form(\n",
" u[:, t], R\n",
" )\n",
"\n",
" _constraints = [\n",
" x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n",
" u[0, t] >= -MAX_ACC,\n",
" u[0, t] <= MAX_ACC,\n",
" u[1, t] >= -MAX_STEER,\n",
" u[1, t] <= MAX_STEER,\n",
" ]\n",
" # opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]\n",
"\n",
2021-04-15 21:46:12 +08:00
" # Actuation rate of change\n",
" if t < (time_horizon - 1):\n",
2022-08-02 16:33:49 +08:00
" _cost += opt.quad_form(u[:, t + 1] - u[:, t], R * 1)\n",
" _constraints += [opt.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC]\n",
" _constraints += [opt.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER]\n",
"\n",
2021-04-15 21:46:12 +08:00
" if t == 0:\n",
2022-08-02 16:33:49 +08:00
" # _constraints += [opt.norm(target[:, time_horizon] - x[:, time_horizon], 1) <= 0.01,\n",
2021-04-15 21:46:12 +08:00
" # x[:, 0] == initial_state]\n",
" _constraints += [x[:, 0] == initial_state]\n",
2022-08-02 16:33:49 +08:00
"\n",
" cost_function.append(\n",
" opt.Problem(opt.Minimize(_cost), constraints=_constraints)\n",
" )\n",
2021-04-15 21:46:12 +08:00
"\n",
" # Add final cost\n",
" problem = sum(cost_function)\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-15 21:46:12 +08:00
" # Minimize Problem\n",
" problem.solve(verbose=verbose, solver=opt.OSQP)\n",
" return x, u"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"jupyter": {
"source_hidden": true
2024-10-23 17:34:26 +08:00
},
"ExecuteTime": {
"end_time": "2024-10-23T09:14:34.623754Z",
"start_time": "2024-10-23T09:14:34.622136Z"
2021-04-15 21:46:12 +08:00
}
},
"outputs": [],
"source": [
"\"\"\"\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-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +08:00
"\n",
2022-08-02 16:33:49 +08:00
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
2021-04-15 21:46:12 +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-15 21:46:12 +08:00
" # solve ODE\n",
2022-08-02 16:33:49 +08:00
" for t in range(1, T + 1):\n",
2021-04-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +08:00
" return x_\n",
"\n",
"\n",
"\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
"\"\"\"\n",
2022-08-02 16:33:49 +08:00
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
2021-04-15 21:46:12 +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",
2021-04-15 21:46:12 +08:00
" # watch out to duplicate points!\n",
2022-08-02 16:33:49 +08:00
" final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
" final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
"\n",
2021-04-15 21:46:12 +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-15 21:46:12 +08:00
"\n",
"\n",
2022-08-02 16:33:49 +08:00
"def get_nn_idx(state, path):\n",
2021-04-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +08:00
"\n",
2022-08-02 16:33:49 +08:00
" if np.dot(d, v) > 0:\n",
2021-04-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +08:00
"def normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
" \"\"\"\n",
" while angle > np.pi:\n",
" angle -= 2.0 * np.pi\n",
"\n",
" while angle < -np.pi:\n",
" angle += 2.0 * np.pi\n",
"\n",
" return angle\n",
"\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-15 21:46:12 +08:00
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
2021-04-19 23:18:08 +08:00
" For each step in the time horizon\n",
2021-04-15 21:46:12 +08:00
" modified reference in robot frame\n",
" \"\"\"\n",
2022-08-02 16:33:49 +08:00
" 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",
2021-04-15 21:46:12 +08:00
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
2022-08-02 16:33:49 +08:00
" dx = path[0, ind] - state[0]\n",
" dy = path[1, ind] - state[1]\n",
"\n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y\n",
" xref[2, 0] = target_v # V\n",
" xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
"\n",
" dl = 0.05 # Waypoints spacing [m]\n",
2021-04-15 21:46:12 +08:00
" travel = 0.0\n",
2022-08-02 16:33:49 +08:00
"\n",
" for i in range(1, T + 1):\n",
" travel += abs(target_v) * DT # current V or target V?\n",
2021-04-15 21:46:12 +08:00
" dind = int(round(travel / dl))\n",
2022-08-02 16:33:49 +08:00
"\n",
2021-04-15 21:46:12 +08:00
" if (ind + dind) < ncourse:\n",
2022-08-02 16:33:49 +08:00
" dx = path[0, ind + dind] - state[0]\n",
" dy = path[1, ind + dind] - state[1]\n",
"\n",
2021-04-15 21:46:12 +08:00
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
2022-08-02 16:33:49 +08:00
" xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])\n",
2021-04-15 21:46:12 +08:00
" dref[0, i] = 0.0\n",
" else:\n",
2022-08-02 16:33:49 +08:00
" dx = path[0, ncourse - 1] - state[0]\n",
" dy = path[1, ncourse - 1] - state[1]\n",
"\n",
2021-04-15 21:46:12 +08:00
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
2022-08-02 16:33:49 +08:00
" xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])\n",
2021-04-15 21:46:12 +08:00
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
]
},
{
"cell_type": "code",
"execution_count": 3,
2024-10-23 17:34:26 +08:00
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T09:14:43.898791Z",
"start_time": "2024-10-23T09:14:36.364854Z"
}
},
2021-04-15 21:46:12 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2024-10-23 17:34:26 +08:00
"CVXPY Optimization Time: Avrg: 0.0378s Max: 0.1155s Min: 0.0311s\n"
2021-04-15 21:46:12 +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-15 21:46:12 +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",
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
2021-04-15 21:46:12 +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",
"x_sim[:, 0] = x0 # simulation_starting conditions\n",
2021-04-15 21:46:12 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"# starting guess\n",
2021-04-15 21:46:12 +08:00
"action = np.zeros(M)\n",
2022-08-02 16:33:49 +08:00
"action[0] = MAX_ACC / 2 # a\n",
"action[1] = 0.0 # delta\n",
"u_sim[:, 0] = action\n",
2021-04-15 21:46:12 +08:00
"\n",
2021-04-19 23:18:08 +08:00
"# Cost Matrices\n",
2022-08-02 16:33:49 +08:00
"Q = np.diag([20, 20, 10, 20]) # state error cost\n",
"Qf = np.diag([30, 30, 30, 30]) # 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-19 23:18:08 +08:00
"\n",
2022-08-02 16:33:49 +08:00
"mpc = MPC(N, M, Q, R)\n",
2021-04-15 21:46:12 +08:00
"REF_VEL = 1.0\n",
"\n",
2022-08-02 16:33:49 +08:00
"for sim_time in range(sim_duration - 1):\n",
"\n",
2021-04-15 21:46:12 +08:00
" iter_start = time.time()\n",
2022-08-02 16:33:49 +08:00
"\n",
" # dynamics starting state w.r.t. robot are always null except vel\n",
" start_state = np.array([0, 0, x_sim[2, sim_time], 0])\n",
"\n",
2021-04-19 23:18:08 +08:00
" # OPTIONAL: Add time delay to starting State- y\n",
2022-08-02 16:33:49 +08:00
"\n",
" current_action = np.array([u_sim[0, sim_time], u_sim[1, sim_time]])\n",
"\n",
2021-04-15 21:46:12 +08:00
" # State Matrices\n",
2022-08-02 16:33:49 +08:00
" A, B, C = get_linear_model_matrices(start_state, current_action)\n",
"\n",
" # Get Reference_traj -> inputs are in worldframe\n",
" target, _ = get_ref_trajectory(x_sim[:, sim_time], track, REF_VEL)\n",
"\n",
" x_mpc, u_mpc = mpc.optimize_linearized_model(\n",
" A, B, C, start_state, target, time_horizon=T, verbose=False\n",
" )\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u_mpc.value[0, :]).flatten(), (np.array(u_mpc.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
2021-04-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### RESUTS\n",
"\n",
"SCS -> Optimization Time: Avrg: 0.2139s Max: 0.3517s Min: 0.1913s\n",
"\n",
"OSQP -> Optimization Time: Avrg: 0.1035s Max: 0.1311s Min: 0.0959s\n",
"\n",
"ECOS -> Avrg: 0.2024s Max: 0.2313s Min: 0.1904s\n",
"\n",
"**Qualitative result** aka \"it drives?\" seems the same..."
]
},
{
"cell_type": "code",
"execution_count": 4,
2024-10-23 17:34:26 +08:00
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-23T09:14:55.715889Z",
"start_time": "2024-10-23T09:14:55.474003Z"
}
},
2021-04-15 21:46:12 +08:00
"outputs": [
{
"data": {
2024-10-23 17:34:26 +08:00
"text/plain": "<Figure size 1500x1000 with 5 Axes>",
"image/png": "iVBORw0KGgoAAAANSUhEUgAABdEAAAPeCAYAAADj01PlAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjkuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8hTgPZAAAACXBIWXMAAA9hAAAPYQGoP6dpAAEAAElEQVR4nOzdd3hUZfrG8e+ZzKRXUoFQAiQYSigqglhQV8CIBXddV2EV29pWYbHvruvqrgVcsKBr+a0rINhdXUssKGsDBKWGIqH3NNJ7JnN+f5xkSCAJQTKZJNyf6+KamTNnznnf5ADJPc88r2GapomIiIiIiIiIiIiIiBzB5u0BiIiIiIiIiIiIiIi0VwrRRURERERERERERESaoBBdRERERERERERERKQJCtFFRERERERERERERJqgEF1EREREREREREREpAkK0UVEREREREREREREmqAQXURERERERERERESkCQrRRURERERERERERESaoBBdRERERERERERERKQJCtFFRERERERERERERJpg9/YAvCU/Px+n0+ntYbS66OhocnJyvD0M6aB0/cjx0PUjx0PXjxwPXT9yvHQNyfHQ9SPHo7NcP3a7nYiICG8Po0NrSU7VWa4XzaP96SxzaW4ex/vv1AkbojudTqqrq709jFZlGAZgzc00TS+PRjoaXT9yPHT9yPHQ9SPHQ9ePHC9dQ3I8dP3I8dD1I/UdLafqLNeL5tH+dJa5eHoeauciIiIiIiIiIiIiItIEhegiIiIiIiIiIiIiIk04Ydu5iIiIiIiIiIhI57dx40Y++OADduzYQX5+PnfddRcjRow46mvmzZvH3r17iYiI4OKLL2bs2LEN9vn+++958803ycrKIjY2liuvvPKoxxWRjkmV6CIiIiIiIiIi0mlVVlbSu3dvrrvuuhbtn52dzWOPPUZycjIzZsxg4sSJvPLKK3z//ffufTIyMnjqqac466yzeOKJJzjrrLN48skn2bJli6emISJepEp0ERERERERERHptIYNG8awYcNavP/nn39OVFQUU6ZMASA+Pp5t27bx4YcfMnLkSAA+/vhjUlJSmDhxIgATJ05k48aNfPzxx0ybNq21pyAiXqYQXUREREREREREpNaWLVtISUlpsG3o0KH873//w+l0YrfbycjI4MILL2ywz5AhQ0hLS2v22NXV1VRXV7sfG4ZBQECA+35jXJ+/T803n5EZFIT5q2sxEgf+nGm1C3VzbGquHUVnmQd0nrl4eh4K0UVERERERERERGoVFBQQFhbWYFtYWBg1NTUUFxcTERFBQUEB4eHhDfYJDw+noKCg2WO/9957vPPOO+7HCQkJzJgxg+jo6KbHg4vizL1UA/7/+5jos35xrFNqd+Li4rw9hFbRWeYBnWcunpqHQnQREREREREREZF6Dq9mNU2z0e2H73O0KtiJEycyYcKEI86Tk5OD0+ls/LjDTsdm98X12otUrFnB/l07MXz9WjSP9sYwDOLi4sjMzHR/TTuizjIP6DxzOdo87HZ7s29WHY1CdBERERERERERkVqNVZQXFRXh4+NDcHBwk/sUFhYeUcF+OIfDgcPhaPS5JgPMqFiMcy7EZ9H71ORkYf60Dgaf0qK5tFemaXbowLZOZ5kHdJ65eGoetlY/ooiIiIiIiIiISAeVmJjIunXrGmxbu3Ytffr0wW636lGTkpJIT09vsM+6detISkryyJgMw8D/lNEAmD+lH2VvEWltCtFFRERERERERKTTqqioYOfOnezcuROA7Oxsdu7cSW5uLgCvvfYazz77rHv/sWPHkpuby7x589i7dy+LFy9m8eLFXHTRRe59UlNTWbt2Le+//z779u3j/fffJz09/YjFRluTT2RM7YTKPHYOEWmc2rmIiIiIiIiIiEintW3bNh566CH34/nz5wNw9tlnc9ttt5Gfn+8O1AFiYmK4//77mTdvHp999hkRERFce+21jBw50r1P//79mTZtGm+88QZvvvkmcXFxTJs2jcTERI/Nw6hrA1Nd7bFziEjjFKKLiIiIiIiIiEinNXDgQN56660mn7/tttuO2DZgwABmzJjR7HFHjhzZIFj3NMPha91xKkQXaWtq5yIiIiIiIiIiItLO1VWim6pEF2lzCtFFRERERERERETaO1Wii3iNQnQREREREREREZF2Tu1cRLxHIbqIiIiIiIiIiEg7515YVCG6SJtTiC4iIiIiIiIiItLOuSvR1RNdpM0pRBcREREREREREWnnVIku4j0K0UVERERERERERNo7d090p3fHIXICUoguIiIiIiIiIiLSzhl2LSwq4i0K0UVERERERERERNo5tXMR8R6F6CIiIiIiIiIiIu2cFhYV8R6F6CIiIiIiIiIiIu2c4at2LiLeohBdRERERERERESknXNXoitEF2lzCtFFRERERERERETau7qe6C4XpqvGu2MROcEoRBcREREREREREWnn3JXoANVO7w1E5ASkEF1ERERERERERKSdaxCiq6WLSJtSiC4iIiIiIiIiItLe+fiAYVj3FaKLtCmF6CIiIiIiIiIiIu2cYRhgr+2LrhBdpE0pRBcREREREREREekI6kL0aoXoIm1JIbqIiIiIiIiIiEhHoEp0Ea9QiC4iIiIiIiIiItIROOzWrUJ0kTZl9/YAREREREREREREPO2zzz7jgw8+oKCggPj4eKZMmUJycnKj+z733HN8/fXXR2yPj49n9uzZAHz11Vf885//PGKfBQsW4Ovr27qDr6NKdBGvUIguIiIiIiIiIiKd2tKlS5k7dy433HAD/fv354svvuDRRx/lySefJCoq6oj9r732WiZNmuR+XFNTw913383IkSMb7BcQEMDTTz/dYJvHAnRQT3QRL1E7FxERERERERER6dQ++ugjzj33XM477zx3FXpUVBSff/55o/sHBgYSHh7u/rNt2zZKS0s555xzGuxnGEaD/cLDwz07EXclutOz5xGRBlSJLiIiIiIiIiIinZbT6WT79u1ceumlDbanpKSwefPmFh1j8eLFDB48mOjo6AbbKyoquPXWW3G5XPTu3ZsrrriChISEJo9TXV1Ndb0qcsMwCAgIcN9vSt1zhsOBCVBT3ez+7ZV7Hh1w7PV1lnlA55mLp+ehEF1ERERERERERDqtoqIiXC4XYWFhDbaHhYVRUFBw1Nfn5+ezZs0a7rjjjgbbu3Xrxq233krPnj0pLy8nLS2NBx54gCeeeIKuXbs2eqz33nuPd955x/04ISGBGTNmHBHON8U3MIhKICI4mMAmztERxMXFeXsIraKzzAM6z1w8NQ+F6CIiIiIiIiIi0uk1VqHakqrVr776iqCgIEaMGNFge1JSEklJSe7H/fv359577+WTTz7huuuua/RYEydOZMKECUecPycnB2czLVoMwyAuLo4q0wQgPyeHwgMHjjr29qZuHpmZmZi1c+mIOss8oPPM5WjzsNvtLX6zqjEK0UVEREREREREpNMKDQ3FZrMdUXVeWFh4RHX64UzT5H//+x9nnnkmdnvzMZrNZqNv375kZmY2uY/D4cDhcDR5rqOq7YluVld16MDTNM0OPf46nWUe0Hnm4ql5aGFRERERERERERHptOx2O3369GHdunUNtq9bt47+/fs3+9qNGzeSmZnJueeee9TzmKbJrl27PLu4aF2QX6+vuoh4nirRRURERERERESkU5swYQJz5syhT58+JCUl8cUXX5Cbm8v5558PwGuvvUZeXh6///3vG7xu8eLFJCYm0rNnzyOO+fbbb5OYmEjXrl3dPdF37tzJ9ddf77mJ1Fai41SILtKWFKKLiIiIiIiIiEindvrpp1NcXMy7775Lfn4+PXr04P7773f3SM7Pzyc3N7fBa8rKyli+fDlTpkxp9JilpaW89NJLFBQUEBgYSEJCAg899BD9+vXz3EQUoot4hUJ0ERERERERERHp9MaNG8e4ceMafe622247YltgYCALFixo8nhTpkxpMmD3GIdCdBFvUE90ERERERERERGRjqCuEl090UXalEJ0ERERERERERGRjkDtXES8QiG6iIiIiIiIiIhIB2A
2021-04-15 21:46:12 +08:00
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
2022-08-02 16:33:49 +08:00
"# plot trajectory\n",
2021-04-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +08:00
"plt.axis(\"equal\")\n",
2022-08-02 16:33:49 +08:00
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
2021-04-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +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-15 21:46:12 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
2024-10-23 17:34:26 +08:00
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [],
"metadata": {
"collapsed": false
}
2021-04-15 21:46:12 +08:00
}
],
"metadata": {
"kernelspec": {
2024-10-23 17:34:26 +08:00
"name": "python3",
2021-04-15 21:46:12 +08:00
"language": "python",
2024-10-23 17:34:26 +08:00
"display_name": "Python 3 (ipykernel)"
2021-04-15 21:46:12 +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
}