def run_mpc(n_delay=1, M_weight=1e-3):
# Create models for simulation and planning
model = new_model(n_delay=n_delay)
model_p = new_model(n_delay=n_delay, M_weight=M_weight)
# Run MPC
X_all, U_all, Z_all, B_all, EB_all = Simulator.mpc(model, model_p)
# Cast simulation results for ease of use
x_all = model.x.repeated(X_all)
u_all = model.u.repeated(U_all)
z_all = model.z.repeated(Z_all)
b_all = model.b.repeated(B_all)
# Plot full simulation
plot_full(x_all, z_all, b_all)
# Plot heuristics
# model = new_model()
# fig = Plotter.plot_heuristics(model, x_all, u_all)
# plt.show()
return X_all, U_all, Z_all, B_all, EB_all, model
# ax_3D = fig_3D.add_subplot(111, projection='3d')
# Plotter.plot_trajectory_3D(ax_3D, x_all)
#
# plt.show()
# ============================================================================
# Model predictive control
# ============================================================================
# ----------------------------- Simulation --------------------------------- #
# Create models for simulation and planning
model = new_model()
model_p = new_model()
# Run MPC
X_all, U_all, Z_all, B_all, EB_all = Simulator.mpc(model, model_p)
# Cast simulation results for ease of use
x_all = model.x.repeated(X_all)
u_all = model.u.repeated(U_all)
z_all = model.z.repeated(Z_all)
b_all = model.b.repeated(B_all)
# ---------------------- Step-by-step plotting ----------------------------- #
fig, axes = plt.subplots(1, 2, figsize=(20, 8))
fig.tight_layout()
xlim = (-5, 35)
ylim = (-5, 20)
Plotter.plot_mpc(fig, axes, xlim, ylim,
model, X_all, Z_all, B_all, EB_all)
