First attempt to merge everything

.gitignore

@@ -6,11 +6,13 @@ examples/mpsc/unsafe_rl_temp_data/
 #
 examples/pid/*data/
 #
-Results/
+experiments/mpsc/results*
+experiments/mpsc/models/rl_models*
+#
 results/
 z_docstring.py
 TODOs.md
-# 
+#
 hpo_study*/
 hp_study*/
 comparisons/
@@ -155,10 +157,4 @@ dmypy.json
 .idea/
 
 *c_generated_code/
-acados_ocp_nlp.json
-gpmpc_acados_ocp_solver.json
-gpmpc_update/
-temp
-models/
-benchmarking_sim/quadrotor/acados_ocp.json
-acados_ocp.json
+*acados_ocp*.json

benchmarking_sim/quadrotor/config_overrides/mpc_acados_quadrotor_2D_attitude_tracking_100.yaml

@@ -1,34 +1,10 @@
 algo: mpc_acados
 algo_config:
-  # horizon: 40
-  # r_mpc:
-  #   - 0.8
-  #   - 0.8
-  # q_mpc:
-  #   - 5.0
-  #   - 0.1
-  #   - 5.0
-  #   - 0.1
-  #   - 0.5
-  #   - 0.001
-  # horizon: 25
-  q_mpc: [18, 0.1, 18, 0.1, 0.5, 0.01]
-  r_mpc: [15., 5.]
-  # r_mpc: [3., 3.]
-  # horizon: 50
-  # q_mpc:
-  # - 7.920690650196039
-  # - 0.0001
-  # - 2.284725973739269
-  # - 0.00010069955204926803
-  # - 0.017010527262423716
-  # - 0.0001
-  # r_mpc:
-  # - 0.0001
-  # - 0.010206543973281433
+  horizon: 25
+  q_mpc: [10, 0.1, 10, 0.1, 0.1, 0.001]
+  r_mpc: [0.1, 0.1]
   prior_info:
-    # prior_prop: null
-    prior_prop: 
+    prior_prop:
       M: 0.033
       beta_1: 18.11
       beta_2: 3.68
@@ -40,9 +16,7 @@ algo_config:
     randomize_prior_prop: False
     prior_prop_rand_info: null
   warmstart: True
-  # use_lqr_gain_and_terminal_cost: True
-  # soft_constraints: True
-  # use_RTI: True
   output_dir: ./mpc_acados/results
 
-
+  soft_constraints: True
+  soft_penalty: 1000.0

benchmarking_sim/quadrotor/config_overrides/quadrotor_2D_attitude_tracking.yaml

@@ -3,7 +3,6 @@ task_config:
   ctrl_freq: 60
   pyb_freq: 60
   physics: dyn_si
-  # physics: pyb
   quad_type: 4
 
   init_state:
@@ -13,44 +12,16 @@ task_config:
     init_z_dot: 0
     init_theta: 0
     init_theta_dot: 0
-  randomized_init: True
+  randomized_init: False
   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: 2
     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
@@ -64,24 +35,21 @@ task_config:
     pitch_bias: 0.0 # in radian
 
   episode_len_sec: 11
-  cost: quadratic
+  cost: rl_reward
+  obs_goal_horizon: 0
+
+  # RL Reward
+  rew_state_weight: [10, 0.1, 10, 0.1, 0.1, 0.001]
+  rew_act_weight: [0.1, 0.1]
+  rew_exponential: True
 
   constraints:
     - constraint_form: default_constraint
       constrained_variable: state
+      upper_bounds: [ 0.9,  2, 1.45,  2,  0.75,  3]
+      lower_bounds: [-0.9, -2, 0.55, -2, -0.75, -3]
     - 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]