Attempting to use far stricter constraints for the 3D quadrotor exper…

…iment
Federico-PizarroBejarano · Nov 2, 2023 · 21e3d86 · 21e3d86
1 parent 1cd7ab6
commit 21e3d86
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 61 deletions.
diff --git a/experiments/mpsc/config_overrides/quadrotor_3D/nl_mpsc_quadrotor_3D.yaml b/experiments/mpsc/config_overrides/quadrotor_3D/nl_mpsc_quadrotor_3D.yaml
@@ -2,20 +2,20 @@ safety_filter: nl_mpsc
 sf_config:
   # LQR controller parameters
   r_lin:
-    - 90
+    - 10000
   q_lin:
-    - 0.001
-    - 0.06
-    - 0.001
-    - 0.06
-    - 0.00025
-    - 80
-    - 0.00001
-    - 0.00001
-    - 0.75
-    - 1
-    - 1
-    - 1
+    - 1.0e-01
+    - 6.0e-03
+    - 3.0e-00
+    - 1.0e-05
+    - 8.0e-00
+    - 1.0e-5
+    - 1.0e-08
+    - 1.0e-08
+    - 1.0e-02
+    - 5.0e-01
+    - 5.0e-01
+    - 5.0e-01
 
   # MPC Parameters
   use_acados: True

diff --git a/experiments/mpsc/config_overrides/quadrotor_3D/quadrotor_3D_track.yaml b/experiments/mpsc/config_overrides/quadrotor_3D/quadrotor_3D_track.yaml
@@ -10,9 +10,9 @@ task_config:
   init_state:
     init_x: 0.4
     init_x_dot: 0
-    init_y: 0.4
+    init_y: -0.1
     init_y_dot: 0
-    init_z: 1.4
+    init_z: 0.9
     init_z_dot: 0
     init_phi: 0
     init_theta: 0
@@ -26,52 +26,52 @@ task_config:
   init_state_randomization_info:
     init_x:
       distrib: 'uniform'
-      low: -2
-      high: 2
+      low: -0.95
+      high: 0.95
     init_x_dot:
-      distrib: 'uniform'
-      low: -1
-      high: 1
-    init_y:
       distrib: 'uniform'
       low: -2
       high: 2
+    init_y:
+      distrib: 'uniform'
+      low: -0.475
+      high: 0
     init_y_dot:
       distrib: 'uniform'
-      low: -1
-      high: 1
+      low: -2
+      high: 2
     init_z:
       distrib: 'uniform'
-      low: 0.3  # Just so it doesn't crash into the ground
-      high: 2
+      low: 0.525  # Just so it doesn't crash into the ground
+      high: 0.95
     init_z_dot:
       distrib: 'uniform'
-      low: -1
-      high: 1
+      low: -2
+      high: 2
     init_phi:
       distrib: 'uniform'
-      low: -0.2
-      high: 0.2
+      low: -0.5
+      high: 0.5
     init_theta:
       distrib: 'uniform'
-      low: -0.2
-      high: 0.2
+      low: -0.5
+      high: 0.5
     init_psi:
       distrib: 'uniform'
-      low: -0.2
-      high: 0.2
+      low: -0.5
+      high: 0.5
     init_p:
       distrib: 'uniform'
-      low: -1
-      high: 1
+      low: -2
+      high: 2
     init_q:
       distrib: 'uniform'
-      low: -1
-      high: 1
+      low: -2
+      high: 2
     init_r:
       distrib: 'uniform'
-      low: -1
-      high: 1
+      low: -2
+      high: 2
 
   task: traj_tracking
   task_info:
@@ -103,31 +103,31 @@ task_config:
     - constraint_form: default_constraint
       constrained_variable: state
       upper_bounds:
+        - 0.95
+        - 2
+        - 0
+        - 2
+        - 0.95
+        - 2
+        - 0.5
+        - 0.5
+        - 0.5
         - 2
-        - 1
         - 2
-        - 1
         - 2
-        - 1
-        - 0.2
-        - 0.2
-        - 0.2
-        - 1
-        - 1
-        - 1
       lower_bounds:
+        - -0.95
+        - -2
+        - -0.475
+        - -2
+        - 0.525
+        - -2
+        - -0.5
+        - -0.5
+        - -0.5
+        - -2
         - -2
-        - -1
         - -2
-        - -1
-        - 0
-        - -1
-        - -0.2
-        - -0.2
-        - -0.2
-        - -1
-        - -1
-        - -1
     - constraint_form: default_constraint
       constrained_variable: input
       upper_bounds:

diff --git a/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_3D.pkl b/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_3D.pkl
diff --git a/experiments/mpsc/mpsc_experiment.py b/experiments/mpsc/mpsc_experiment.py
@@ -192,7 +192,7 @@ def determine_feasible_starting_points(num_points=100, num_steps=25):
 
     if config.algo in ['ppo', 'sac', 'safe_explorer_ppo']:
         # Load state_dict from trained.
-        ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_best.pt')
+        ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_latest.pt')
 
         # Remove temporary files and directories
         shutil.rmtree('./temp', ignore_errors=True)
@@ -334,7 +334,7 @@ def run_uncertified_trajectory(n_episodes=10):
 
     if config.algo in ['ppo', 'sac', 'safe_explorer_ppo']:
         # Load state_dict from trained.
-        ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_best.pt')
+        ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_latest.pt')
 
         # Remove temporary files and directories
         shutil.rmtree('./temp', ignore_errors=True)

diff --git a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py
@@ -1038,7 +1038,7 @@ def setup_acados_optimizer(self):
         ocp.solver_options.tf = self.dt * self.horizon
 
         solver_json = 'acados_ocp_mpsf.json'
-        ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=False, build=False)
+        ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=True, build=True)
 
         for stage in range(self.mpsc_cost_horizon):
             ocp_solver.cost_set(stage, 'W', (self.cost_function.decay_factor**stage) * ocp.cost.W)