From f5ca35f72fe36ecb398fcfa400cd3215905f2400 Mon Sep 17 00:00:00 2001 From: Asia La Rocca Date: Mon, 31 Jul 2023 14:38:29 +0200 Subject: [PATCH] update MPC test settings --- .../hard_terminal_constraints/3dof_sym.py | 50 ++++++++++------ VBOC/Safe MPC/no_constraints/3dof_sym.py | 10 ++-- .../receiding_hard_constraints/3dof_sym.py | 50 ++++++++++------ .../soft_traj_constraints/3dof_sym.py | 59 ++++++++++++------- 4 files changed, 104 insertions(+), 65 deletions(-) diff --git a/VBOC/Safe MPC/hard_terminal_constraints/3dof_sym.py b/VBOC/Safe MPC/hard_terminal_constraints/3dof_sym.py index 1e3df68..2c75f78 100644 --- a/VBOC/Safe MPC/hard_terminal_constraints/3dof_sym.py +++ b/VBOC/Safe MPC/hard_terminal_constraints/3dof_sym.py @@ -83,11 +83,11 @@ def simulate(p): std = torch.load('../std_3dof_vboc') safety_margin = 2.0 -cpu_num = 10 +cpu_num = 1 test_num = 100 -time_step = 5*1e-3 -tot_time = 0.3 #0.1 and 0.115 0.002s, 0.15 0.027s, 0.2 0.003s, 0.25 0.0035s +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 tot_steps = 100 regenerate = True @@ -95,28 +95,40 @@ def simulate(p): x_sol_guess_vec = np.load('../x_sol_guess.npy') u_sol_guess_vec = np.load('../u_sol_guess.npy') -ocp = OCPtriplependulumHardTerm("SQP_RTI", time_step, tot_time, list(model.parameters()), mean, std, regenerate) -sim = SYMtriplependulum(time_step, tot_time, regenerate) +quant = 10. +r = 1 -# Generate low-discrepancy unlabeled samples: -sampler = qmc.Halton(d=ocp.ocp.dims.nu, scramble=False) -sample = sampler.random(n=test_num) -l_bounds = ocp.Xmin_limits[:ocp.ocp.dims.nu] -u_bounds = ocp.Xmax_limits[:ocp.ocp.dims.nu] -data = qmc.scale(sample, l_bounds, u_bounds) +while quant > 3*1e-3: -N = ocp.ocp.dims.N + ocp = OCPtriplependulumHardTerm("SQP_RTI", time_step, tot_time, list(model.parameters()), mean, std, regenerate) + sim = SYMtriplependulum(time_step, tot_time, True) -# MPC controller without terminal constraints: -with Pool(cpu_num) as p: - res = p.map(simulate, range(data.shape[0])) + N = ocp.ocp.dims.N -res_steps_term, stats = zip(*res) + # Generate low-discrepancy unlabeled samples: + sampler = qmc.Halton(d=ocp.ocp.dims.nu, scramble=False) + sample = sampler.random(n=test_num) + l_bounds = ocp.Xmin_limits[:ocp.ocp.dims.nu] + u_bounds = ocp.Xmax_limits[:ocp.ocp.dims.nu] + data = qmc.scale(sample, l_bounds, u_bounds) -times = np.array([i for f in stats for i in f if i is not None]) + # MPC controller without terminal constraints: + with Pool(cpu_num) as p: + res = p.map(simulate, range(data.shape[0])) -print('90 percent quantile solve time: ' + str(np.quantile(times, 0.9))) -print('Mean solve time: ' + str(np.mean(times))) + res_steps_term, stats = zip(*res) + + times = np.array([i for f in stats for i in f if i is not None]) + + quant = np.quantile(times, 0.9) + + print('iter: ', str(r)) + print('tot time: ' + str(tot_time)) + print('90 percent quantile solve time: ' + str(quant)) + print('Mean solve time: ' + str(np.mean(times))) + + tot_time -= 2*1e-2 + r += 1 print(np.array(res_steps_term).astype(int)) diff --git a/VBOC/Safe MPC/no_constraints/3dof_sym.py b/VBOC/Safe MPC/no_constraints/3dof_sym.py index d3e65eb..242beed 100644 --- a/VBOC/Safe MPC/no_constraints/3dof_sym.py +++ b/VBOC/Safe MPC/no_constraints/3dof_sym.py @@ -93,14 +93,14 @@ def init_guess(p): start_time = time.time() -cpu_num = 30 +cpu_num = 1 test_num = 100 -time_step = 5*1e-3 -tot_time = 0.11 +time_step = 4*1e-3 +tot_time = 0.16 tot_steps = 100 -ocp = OCPtriplependulumSTD("SQP", time_step, 0.5, True) +ocp = OCPtriplependulumSTD("SQP", time_step, 0.2, True) # Generate low-discrepancy unlabeled samples: sampler = qmc.Halton(d=ocp.ocp.dims.nu, scramble=False) @@ -111,7 +111,7 @@ def init_guess(p): N = ocp.ocp.dims.N -with Pool(cpu_num) as p: +with Pool(30) as p: res = p.map(init_guess, range(data.shape[0])) x_sol_guess_vec, u_sol_guess_vec = zip(*res) diff --git a/VBOC/Safe MPC/receiding_hard_constraints/3dof_sym.py b/VBOC/Safe MPC/receiding_hard_constraints/3dof_sym.py index fd0c8d2..406563c 100644 --- a/VBOC/Safe MPC/receiding_hard_constraints/3dof_sym.py +++ b/VBOC/Safe MPC/receiding_hard_constraints/3dof_sym.py @@ -106,11 +106,11 @@ def simulate(p): std = torch.load('../std_3dof_vboc') safety_margin = 2.0 -cpu_num = 10 +cpu_num = 1 test_num = 100 -time_step = 5*1e-3 -tot_time = 0.02 #0.1 0.011s, 0.06 0.008s, 0.04 0.006s, 0.03 0.0044s +time_step = 4*1e-3 +tot_time = 0.16 #0.1 0.011s, 0.06 0.008s, 0.04 0.006s, 0.03 0.0044s tot_steps = 100 regenerate = True @@ -120,28 +120,40 @@ def simulate(p): params = list(model.parameters()) -ocp = OCPtriplependulumSoftTraj("SQP_RTI", time_step, tot_time, params, mean, std, safety_margin, regenerate) -sim = SYMtriplependulum(time_step, tot_time, regenerate) +quant = 10. +r = 1 -# Generate low-discrepancy unlabeled samples: -sampler = qmc.Halton(d=ocp.ocp.dims.nu, scramble=False) -sample = sampler.random(n=test_num) -l_bounds = ocp.Xmin_limits[:ocp.ocp.dims.nu] -u_bounds = ocp.Xmax_limits[:ocp.ocp.dims.nu] -data = qmc.scale(sample, l_bounds, u_bounds) +while quant > 3*1e-3: -N = ocp.ocp.dims.N + ocp = OCPtriplependulumSoftTraj("SQP_RTI", time_step, tot_time, params, mean, std, safety_margin, regenerate) + sim = SYMtriplependulum(time_step, tot_time, regenerate) -# MPC controller without terminal constraints: -with Pool(cpu_num) as p: - res = p.map(simulate, range(data.shape[0])) + # Generate low-discrepancy unlabeled samples: + sampler = qmc.Halton(d=ocp.ocp.dims.nu, scramble=False) + sample = sampler.random(n=test_num) + l_bounds = ocp.Xmin_limits[:ocp.ocp.dims.nu] + u_bounds = ocp.Xmax_limits[:ocp.ocp.dims.nu] + data = qmc.scale(sample, l_bounds, u_bounds) -res_steps_traj, stats = zip(*res) + N = ocp.ocp.dims.N -times = np.array([i for f in stats for i in f if i is not None]) + # MPC controller without terminal constraints: + with Pool(cpu_num) as p: + res = p.map(simulate, range(data.shape[0])) -print('90 percent quantile solve time: ' + str(np.quantile(times, 0.9))) -print('Mean solve time: ' + str(np.mean(times))) + res_steps_traj, stats = zip(*res) + + times = np.array([i for f in stats for i in f if i is not None]) + + quant = np.quantile(times, 0.9) + + print('iter: ', str(r)) + print('tot time: ' + str(tot_time)) + print('90 percent quantile solve time: ' + str(quant)) + print('Mean solve time: ' + str(np.mean(times))) + + tot_time -= 2*1e-2 + r += 1 print(np.array(res_steps_traj).astype(int)) diff --git a/VBOC/Safe MPC/soft_traj_constraints/3dof_sym.py b/VBOC/Safe MPC/soft_traj_constraints/3dof_sym.py index 6a6cb61..d726a0f 100644 --- a/VBOC/Safe MPC/soft_traj_constraints/3dof_sym.py +++ b/VBOC/Safe MPC/soft_traj_constraints/3dof_sym.py @@ -83,11 +83,11 @@ def simulate(p): std = torch.load('../std_3dof_vboc') safety_margin = 2.0 -cpu_num = 10 +cpu_num = 1 test_num = 100 -time_step = 5*1e-3 -tot_time = 0.035 #0.1 0.018s, 0.05 0.005s, 0.045 0.0046s, 0.04 0.0043 +time_step = 4*1e-3 +tot_time = 0.16 #0.1 0.018s, 0.05 0.005s, 0.045 0.0046s, 0.04 0.0043 tot_steps = 100 regenerate = True @@ -95,32 +95,47 @@ def simulate(p): x_sol_guess_vec = np.load('../x_sol_guess.npy') u_sol_guess_vec = np.load('../u_sol_guess.npy') -ocp = OCPtriplependulumSoftTraj("SQP_RTI", time_step, tot_time, list(model.parameters()), mean, std, safety_margin, regenerate) -sim = SYMtriplependulum(time_step, tot_time, regenerate) +quant = 10. +r = 1 -# Generate low-discrepancy unlabeled samples: -sampler = qmc.Halton(d=ocp.ocp.dims.nu, scramble=False) -sample = sampler.random(n=test_num) -l_bounds = ocp.Xmin_limits[:ocp.ocp.dims.nu] -u_bounds = ocp.Xmax_limits[:ocp.ocp.dims.nu] -data = qmc.scale(sample, l_bounds, u_bounds) +while quant > 3*1e-3: -N = ocp.ocp.dims.N + ocp = OCPtriplependulumSoftTraj("SQP_RTI", time_step, tot_time, list(model.parameters()), mean, std, safety_margin, regenerate) + sim = SYMtriplependulum(time_step, tot_time, regenerate) -for i in range(1,N): - ocp.ocp_solver.cost_set(i, "Zl", 1e4*np.ones((1,))) -ocp.ocp_solver.cost_set(N, "Zl", 1e4*np.ones((1,))) + # Generate low-discrepancy unlabeled samples: + sampler = qmc.Halton(d=ocp.ocp.dims.nu, scramble=False) + sample = sampler.random(n=test_num) + l_bounds = ocp.Xmin_limits[:ocp.ocp.dims.nu] + u_bounds = ocp.Xmax_limits[:ocp.ocp.dims.nu] + data = qmc.scale(sample, l_bounds, u_bounds) -# MPC controller without terminal constraints: -with Pool(cpu_num) as p: - res = p.map(simulate, range(data.shape[0])) + N = ocp.ocp.dims.N -res_steps_traj, stats = zip(*res) + for i in range(1,N): + ocp.ocp_solver.cost_set(i, "Zl", 1e4*np.ones((1,))) + ocp.ocp_solver.cost_set(N, "Zl", 1e4*np.ones((1,))) -times = np.array([i for f in stats for i in f if i is not None]) + # MPC controller without terminal constraints: + with Pool(cpu_num) as p: + res = p.map(simulate, range(data.shape[0])) -print('90 percent quantile solve time: ' + str(np.quantile(times, 0.9))) -print('Mean solve time: ' + str(np.mean(times))) + res_steps_traj, stats = zip(*res) + + times = np.array([i for f in stats for i in f if i is not None]) + + quant = np.quantile(times, 0.9) + + print('90 percent quantile solve time: ' + str(np.quantile(times, 0.9))) + print('Mean solve time: ' + str(np.mean(times))) + + print('iter: ', str(r)) + print('tot time: ' + str(tot_time)) + print('90 percent quantile solve time: ' + str(quant)) + print('Mean solve time: ' + str(np.mean(times))) + + tot_time -= 2*1e-2 + r += 1 print(np.array(res_steps_traj).astype(int))