toc = time.perf_counter()
x_opt = out["xopt"]
J_opt = out["fopt"]
J_sample = out["F"]
X_sample = out["X"]
idx_opt = np.argmin(J_sample, axis=0)
time_optimization = time.perf_counter() - time_optimization_start
print(f"J_best_val: {J_opt:.3f}")
# In[Re-simulate with the optimal point]
simopt = get_simoptions_x(x_opt)
simout = simulate_pendulum_MPC(simopt)
tsim = simout['t']
xsim = simout['x']
usim = simout['u']
x_ref = simout['x_ref']
uref = get_parameter({}, 'uref')
fig, axes = plt.subplots(3, 1, figsize=(10, 10))
axes[0].plot(tsim, xsim[:, 0], "k", label='p')
axes[0].plot(tsim, x_ref[:, 0], "r--", label="p_ref")
axes[0].set_title("Position (m)")
axes[1].plot(tsim, xsim[:, 2] * 360 / 2 / np.pi, label="phi")
axes[1].plot(tsim, x_ref[:, 2] * 360 / 2 / np.pi, "r--", label="phi_ref")
def f_x(x, eps_calc=1.0, seed_val=None):
global N_eval
if seed_val is None:
seed_val = N_eval
simoptions = get_simoptions_x(x)
simoptions['seed_val'] = seed_val
sim_failed = False
try:
simout = simulate_pendulum_MPC(simoptions)
except ValueError as e:
print(e)
sim_failed = True
if not sim_failed:
t = simout['t']
y_meas = simout['y_meas']
x_ref = simout['x_ref']
p_meas = y_meas[:, 0]
phi_meas = y_meas[:, 1]
p_ref = x_ref[:, 0]
phi_ref = x_ref[:, 2]
J_perf = 10 * np.mean(np.abs(p_ref - p_meas)) + 0.0 * np.max(np.abs(p_ref - p_meas)) + \
30 * np.mean(np.abs(np.abs(phi_ref - phi_meas))) # + 15*np.max(np.abs(np.abs(phi_ref - phi_meas)))
# Computation of the barrier function
t_calc = simout['t_calc']
eps_margin = 0.8
t_calc = eps_calc * t_calc
t_calc_wc = np.max(
t_calc) # worst-case computational cost (max computational time)
Ts_MPC = simout['Ts_MPC']
t_available = Ts_MPC * eps_margin
delay_wc = (t_calc_wc - t_available)
delay_wc = delay_wc * (delay_wc >= 0)
J_calc = (delay_wc / t_available) * 1e3
emergency = simout['emergency_fast']
emergency_time, _ = np.where(emergency > 0)
if len(emergency_time) > 0:
J_emergency = (len(emergency) -
emergency_time[0]) / len(emergency) * 1e3
else:
J_emergency = 0.0
else:
J_perf = 1e3
J_calc = 1e3
J_emergency = 1e3 # (len(emergency) - emergency_time[0]) / len(emergency) * 1e3
# J_perf = 2e1
# J_fit = 2e1
J_cl = np.log(J_perf) + np.log(1 + J_calc) + np.log(1 + J_emergency)
print(
f"N_eval: {N_eval}, J_perf:{J_perf:.2f}, J_calc:{J_calc:.2f}, J_emergency:{J_emergency:.2f}, J_cl:{J_cl:.2f}"
)
N_eval += 1
return J_cl # + J_fit