fix receiding horizon tests

VBOC/Safe MPC/comparison.py

@@ -0,0 +1,64 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Load data
+data_dir = "data/"
+data_no = np.load(data_dir + "results_no_constraint.npz")
+
+use_data = 'receidinghard' # soft_term, soft_traj, hard_term, receidinghard, receidingsoft
+
+if use_data == 'soft_term':
+    data_vboc = np.load(data_dir + "results_softterm.npz")
+if use_data == 'soft_traj':
+    data_vboc = np.load(data_dir + "results_softtraj.npz")
+if use_data == 'hard_term':
+    data_vboc = np.load(data_dir + "results_hardterm.npz")
+if use_data == 'receidinghard':
+    data_vboc = np.load(data_dir + "results_receidinghard.npz")
+if use_data == 'receidingsoft':
+    data_vboc = np.load(data_dir + "results_receidingsoft.npz")
+
+dt = data_no["dt"]
+tot_time = data_no["tot_time"]
+times_no = data_no["times"]
+res_steps_no = data_no["res_steps"]
+
+times_hard = data_vboc["times"]
+res_steps_hard = data_vboc["res_steps_term"]
+better = data_vboc["better"]
+worse = data_vboc["worse"]
+equal = data_vboc["equal"]
+
+# Plot timing
+plt.figure()
+plt.plot(np.linspace(0, len(times_no), len(times_no)), times_no, label="naive MPC", color='red')
+plt.plot(np.linspace(0, len(times_hard), len(times_hard)), times_hard, label="MPC + VBOC", color='green')
+plt.plot(np.linspace(0, len(times_no), len(times_no)), np.ones(len(times_no)) * np.quantile(times_no, 0.9),
+         label="90% quantile naive MPC", color='fuchsia', linestyle='--')
+plt.plot(np.linspace(0, len(times_hard), len(times_hard)), np.ones(len(times_hard)) * np.quantile(times_hard, 0.9),
+         label="90% quantile MPC + VBOC", color='DarkBlue', linestyle='--')
+plt.xlabel('MPC Iteration')
+plt.ylabel('Solve time [s]')
+plt.legend()
+plt.title("Solve time comparison")
+plt.savefig(data_dir + 'times_' + use_data + '.png')
+
+# Barchart that states when the hard terminal constraint is better, equal or worse than the naive MPC
+plt.figure()
+plt.bar(["Better", "Equal", "Worse"], [better, equal, worse], color=["green", "blue", "red"])
+plt.ylabel("Number")
+plt.title("Comparison between MPC + VBOC and naive MPC")
+plt.savefig(data_dir + 'numbers_' + use_data + '.png')
+
+# Directly compare the number of iteration taken by the two MPCs before the first infeasible solution
+plt.figure()
+total = np.linspace(1, 100, 100)
+plt.plot(total, res_steps_no, label="naive MPC", color='red')
+plt.plot(total, np.ones(len(total)) * np.mean(res_steps_no), label="mean naive MPC", color='fuchsia', linestyle='--')
+plt.plot(total, res_steps_hard, label="MPC + VBOC", color='green')
+plt.plot(total, np.ones(len(total)) * np.mean(res_steps_hard), label="mean MPC + VBOC", color='DarkBlue', linestyle='--')
+plt.title("Comparison between MPC + VBOC and naive MPC")
+plt.xlabel("Number of problems")
+plt.ylabel("Number of iterations")
+plt.legend()
+plt.savefig(data_dir + 'iterations_' + use_data + '.png')

VBOC/Safe MPC/hard_terminal_constraints/3dof_sym.py

@@ -32,9 +32,8 @@ def simulate(p):
 
     for f in range(tot_steps):
 
-        temp = time.time()
-        status = ocp.OCP_solve(simX[f], x_sol_guess, u_sol_guess)
-        times[f] = time.time() - temp
+        status = ocp.OCP_solve(simX[f], x_sol_guess, u_sol_guess, ocp.thetamax-0.05, joint_vec[f])
+        times[f] = ocp.ocp_solver.get_stats('time_tot')
 
         if status != 0:
 
@@ -64,11 +63,12 @@ def simulate(p):
             x_sol_guess[N] = np.copy(x_sol_guess[N-1])
             u_sol_guess[N-1] = np.copy(u_sol_guess[N-2])
 
+        simU[f] += noise_vec[f]
+
         sim.acados_integrator.set("u", simU[f])
         sim.acados_integrator.set("x", simX[f])
         status = sim.acados_integrator.solve()
         simX[f+1] = sim.acados_integrator.get("x")
-        simU[f] = u_sol_guess[0]
 
     return f, times
 
@@ -86,19 +86,21 @@ def simulate(p):
 cpu_num = 1
 test_num = 100
 
-time_step = 4*1e-3
-tot_time = 0.16 #0.1 and 0.115 0.002s, 0.15 0.027s, 0.2 0.003s, 0.25 0.0035s, 0.3 0.0039s
+time_step = 5*1e-3
+tot_time = 0.16
 tot_steps = 100
 
 regenerate = True
 
 x_sol_guess_vec = np.load('../x_sol_guess.npy')
 u_sol_guess_vec = np.load('../u_sol_guess.npy')
+noise_vec = np.load('../noise.npy')
+noise_vec = np.load('../selected_joint.npy')
 
 quant = 10.
 r = 1
 
-while quant > 3*1e-3:
+while quant > 4*1e-3:
 
     ocp = OCPtriplependulumHardTerm("SQP_RTI", time_step, tot_time, list(model.parameters()), mean, std, regenerate)
     sim = SYMtriplependulum(time_step, tot_time, True)
@@ -120,17 +122,19 @@ def simulate(p):
 
     times = np.array([i for f in stats for i in f if i is not None])
 
-    quant = np.quantile(times, 0.9)
+    quant = np.quantile(times, 0.99)
 
     print('iter: ', str(r))
     print('tot time: ' + str(tot_time))
-    print('90 percent quantile solve time: ' + str(quant))
+    print('99 percent quantile solve time: ' + str(quant))
     print('Mean solve time: ' + str(np.mean(times)))
 
-    tot_time -= 2*1e-2
+    tot_time -= time_step
     r += 1
 
-print(np.array(res_steps_term).astype(int))
+    print(np.array(res_steps_term).astype(int))
+
+    del ocp
 
 np.save('res_steps_hardterm.npy', np.array(res_steps_term).astype(int))
 
@@ -151,4 +155,8 @@ def simulate(p):
 print('MPC standard vs MPC with hard term constraints')
 print('Percentage of initial states in which the MPC+VBOC behaves better: ' + str(better))
 print('Percentage of initial states in which the MPC+VBOC behaves equal: ' + str(equal))
-print('Percentage of initial states in which the MPC+VBOC behaves worse: ' + str(worse))
+print('Percentage of initial states in which the MPC+VBOC behaves worse: ' + str(worse))
+
+np.savez('../data/results_hardterm.npz', res_steps_term=res_steps_term,
+         better=better, worse=worse, equal=equal, times=times,
+         dt=time_step, tot_time=tot_time)