update mpc cost and fast traj config

benchmarking_sim/quadrotor/config_overrides/gpmpc_acados_quadrotor_2D_attitude_tracking_100.yaml

@@ -4,19 +4,19 @@
     # q_mpc: [5, 0.1, 5, 0.1, 0.5, 0.001]
     # r_mpc: [1.5, 1.5]
     horizon: 25
-    q_mpc: [12, 0.1, 12, 0.5, 0.5, 0.0001]
-    r_mpc: [10., 3.]
+    q_mpc: [18, 0.1, 18, 0.5, 0.5, 0.0001]
+    r_mpc: [3., 3.]
     # gp_model_path: /home/mingxuan/Repositories/scg_mx/gpmpc_update/gpmpc_acados/results # null
     # gp_model_path: /home/mingxuan/Repositories/scg_tsung/benchmarking_sim/quadrotor/gpmpc_acados/results/200_300_rti/temp/seed1_Aug-08-12-27-05_efc4389
     test_data_ratio: 0.2
     soft_constraints: null
     optimization_iterations:
-      - 3000 # 1200
-      - 3000 # 1200
-      - 3000 # 1200
-      - 3000 # 1200
-      - 3000 # 1200
-      - 3000 # 1200
+      - 3000
+      - 3000
+      - 3000
+      - 3000
+      - 3000
+      - 3000
     learning_rate:
       - 0.01
       - 0.01

benchmarking_sim/quadrotor/config_overrides/linear_mpc_quadrotor_2D_attitude_tracking_100.yaml

@@ -13,8 +13,8 @@ algo_config:
   #   - 0.001
 
   horizon: 25
-  q_mpc: [12, 0.1, 12, 0.5, 0.5, 0.0001]
-  r_mpc: [10., 3.]
+  q_mpc: [18, 0.1, 18, 0.5, 0.5, 0.0001]
+  r_mpc: [3., 3.]
 
   # Prior info
   prior_info:

benchmarking_sim/quadrotor/config_overrides/mpc_acados_quadrotor_2D_attitude_tracking_100.yaml

@@ -12,8 +12,8 @@ algo_config:
   #   - 0.5
   #   - 0.001
   horizon: 25
-  q_mpc: [12, 0.1, 12, 0.5, 0.5, 0.0001]
-  r_mpc: [10., 3.]
+  q_mpc: [18, 0.1, 18, 0.5, 0.5, 0.0001]
+  r_mpc: [3., 3.]
   # Prior info
   prior_info:
     # prior_prop: null

benchmarking_sim/quadrotor/config_overrides/quadrotor_2D_attitude_tracking_fast.yaml

@@ -0,0 +1,87 @@
+task_config:
+  info_in_reset: True
+  ctrl_freq: 60
+  pyb_freq: 60
+  physics: dyn_si
+  # physics: pyb
+  quad_type: 4
+
+  init_state:
+    init_x: 0
+    init_x_dot: 0
+    init_z: 1.0
+    init_z_dot: 0
+    init_theta: 0
+    init_theta_dot: 0
+  randomized_init: True
+  randomized_inertial_prop: False
+
+  init_state_randomization_info:
+    init_x:
+      distrib: 'uniform'
+      low: -0.05
+      high: 0.05
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.05
+      high: 0.05
+    init_z:
+      distrib: 'uniform'
+      low: -0.05
+      high: 0.05
+    init_z_dot:
+      distrib: 'uniform'
+      low: -0.05
+      high: 0.05
+    init_theta:
+      distrib: 'uniform'
+      low: -0.05
+      high: 0.05
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.05
+      high: 0.05
+
+  task: traj_tracking
+  task_info:
+    trajectory_type: figure8
+    num_cycles: 3
+    trajectory_plane: 'xz'
+    trajectory_position_offset: [0, 1.]
+    trajectory_scale: 1.0
+    # ilqr_ref: True
+    # ilqr_traj_data: /home/mingxuan/Repositories/scg_tsung/examples/lqr/ilqr_ref_traj.npy
+
+  inertial_prop:
+    M: 0.033
+    Iyy: 1.4e-05
+    beta_1: 18.11
+    beta_2: 3.68
+    beta_3: 0.0
+    alpha_1: -140.8
+    alpha_2: -13.4
+    alpha_3: 124.8
+    pitch_bias: 0.0 # in radian
+
+  episode_len_sec: 11
+  cost: quadratic
+
+  constraints:
+    - constraint_form: default_constraint
+      constrained_variable: state
+    - constraint_form: default_constraint
+      constrained_variable: input
+      upper_bounds: [0.58212, 0.7]
+      lower_bounds: [0.09702, -0.7]
+
+  done_on_out_of_bound: True
+  done_on_violation: False
+  disturbances:
+    # dynamics: # disturbance force in newton
+    #   - disturbance_func: uniform
+    #     low:  1.
+    #     high: 1.
+    #     mask: [1, 0, 0, 0]
+    observation:
+      - disturbance_func: white_noise
+        std: [5.6e-05, 1.5e-02, 2.9e-05, 8.0e-03, 1.3e-03, 3.5e-03]