update MPC test settings

idra-lab · Jul 31, 2023 · f5ca35f · f5ca35f
1 parent cfd8140
commit f5ca35f
Show file tree

Hide file tree

Showing 4 changed files with 104 additions and 65 deletions.
diff --git a/VBOC/Safe MPC/hard_terminal_constraints/3dof_sym.py b/VBOC/Safe MPC/hard_terminal_constraints/3dof_sym.py
@@ -83,40 +83,52 @@ 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
 
 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
@@ -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
@@ -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
@@ -83,44 +83,59 @@ 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
 
 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))