Adding updates for Quadrotor

.gitignore

@@ -7,6 +7,7 @@ examples/mpsc/unsafe_rl_temp_data/
 examples/pid/*data/
 #
 results/
+Results/
 z_docstring.py
 TODOs.md
 
@@ -147,3 +148,5 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
+
+.idea/

examples/lqr/config_overrides/quadrotor_2D_attitude/ilqr_quadrotor_2D_attitude_stabilization.yaml

@@ -0,0 +1,17 @@
+algo: ilqr
+algo_config:
+  # Cost parameters
+  q_lqr: [18.0, 0.0017, 18.0, 0.004, 18.0, 0.0017]
+  r_lqr: [0.001, 0.001]
+
+  # Model arguments
+  # Note: Higher simulation frequency is required if using controller designed
+  #       based on the continuous-time model
+  discrete_dynamics: True
+
+  # iLQR arguments
+  max_iterations: 15
+  lamb_factor: 10
+  lamb_max: 1000
+  epsilon: 0.01
+

examples/lqr/config_overrides/quadrotor_2D_attitude/ilqr_quadrotor_2D_attitude_tracking.yaml

@@ -0,0 +1,17 @@
+algo: ilqr
+algo_config:
+  # Cost parameters
+  q_lqr: [18.0, 0.0017, 18.0, 0.004, 18.0, 0.0017]
+  r_lqr: [0.001, 0.001]
+
+  # Model arguments
+  # Note: Higher simulation frequency is required if using controller designed
+  #       based on the continuous-time model
+  discrete_dynamics: True
+
+  # iLQR arguments
+  max_iterations: 15
+  lamb_factor: 10
+  lamb_max: 1000
+  epsilon: 0.01
+

examples/lqr/config_overrides/quadrotor_2D_attitude/lqr_quadrotor_2D_attitude_stabilization.yaml

@@ -0,0 +1,10 @@
+algo: lqr
+algo_config:
+  # Cost parameters
+  q_lqr: [1, 0.1, 1, 0.1, 0.1, 0.1]
+  r_lqr: [0.1]
+
+  # Model arguments
+  # Note: Higher simulation frequency is required if using controller designed
+  #       based on the continuous-time model
+  discrete_dynamics: True

examples/lqr/config_overrides/quadrotor_2D_attitude/lqr_quadrotor_2D_attitude_tracking.yaml

@@ -0,0 +1,10 @@
+algo: lqr
+algo_config:
+  # Cost parameters
+  q_lqr: [1, 0.1, 1, 0.1, 0.1, 0.1]
+  r_lqr: [0.1]
+
+  # Model arguments
+  # Note: Higher simulation frequency is required if using controller designed
+  #       based on the continuous-time model
+  discrete_dynamics: True

examples/lqr/config_overrides/quadrotor_2D_attitude/quadrotor_2D_attitude_stabilization.yaml

@@ -0,0 +1,44 @@
+task_config:
+  seed: 1337
+  info_in_reset: True
+  ctrl_freq: 60
+  pyb_freq: 60
+  physics: dyn_si
+  quad_type: 4
+
+  init_state_randomization_info:
+    init_x:
+      distrib: 'uniform'
+      low: -1
+      high: 1
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_z:
+      distrib: 'uniform'
+      low: 0.5
+      high: 1.5
+    init_z_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_theta:
+      distrib: 'uniform'
+      low: -0.2
+      high: 0.2
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+  randomized_init: True
+  randomized_inertial_prop: False
+
+  task: stabilization
+  task_info:
+    stabilization_goal: [0, 1]
+    stabilization_goal_tolerance: 0.0
+
+  episode_len_sec: 6
+  cost: quadratic
+  done_on_out_of_bound: True

examples/lqr/config_overrides/quadrotor_2D_attitude/quadrotor_2D_attitude_tracking.yaml

@@ -0,0 +1,82 @@
+task_config:
+  seed: 1337
+  info_in_reset: True
+  ctrl_freq: 60
+  pyb_freq: 60
+  physics: dyn_si
+  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: 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: 1
+    trajectory_plane: 'xz'
+    trajectory_position_offset: [0, 1.]
+    trajectory_scale: 0.5
+
+  inertial_prop:
+    M: 0.027
+    Iyy: 1.4e-05
+
+  episode_len_sec: 6
+  cost: quadratic
+  obs_goal_horizon: 0
+
+  # RL Reward
+  rew_state_weight: [10.0, 0.1, 10.0, 0.1, 0.1, 0.001]
+  rew_act_weight: 0.1
+  rew_exponential: True
+
+#  disturbances:
+#    observation:
+#      - disturbance_func: white_noise
+#        std: [5.6e-05, 1.5e-02, 2.9e-05, 8.0e-03, 1.3e-03, 3.6e-01]
+
+  constraints:
+    - constraint_form: default_constraint
+      constrained_variable: state
+#      upper_bounds: [2, 1, 2, 1, 0.2, 2.5]
+#      lower_bounds: [-2, -1, 0, -1, -0.2, -2.5]
+    - constraint_form: default_constraint
+      constrained_variable: input
+      upper_bounds: [0.47628, 0.4]
+      lower_bounds: [0.079, -0.4]
+
+  done_on_out_of_bound: True
+  done_on_violation: False

examples/lqr/lqr_experiment.sh

@@ -4,7 +4,8 @@
 
 # SYS='cartpole'
 # SYS='quadrotor_2D'
-SYS='quadrotor_3D'
+SYS='quadrotor_2D_attitude'
+# SYS='quadrotor_3D'
 
 # TASK='stabilization'
 TASK='tracking'

examples/mpc/config_overrides/quadrotor_2D_attitude/linear_mpc_quadrotor_2D_attitude_stabilization.yaml

@@ -0,0 +1,20 @@
+algo: linear_mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: qrqp

examples/mpc/config_overrides/quadrotor_2D_attitude/linear_mpc_quadrotor_2D_attitude_tracking.yaml

@@ -0,0 +1,20 @@
+algo: linear_mpc
+algo_config:
+  horizon: 40
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 1.0
+    - 0.1
+    - 1.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: qrqp

examples/mpc/config_overrides/quadrotor_2D_attitude/mpc_quadrotor_2D_attitude_stabilization.yaml

@@ -0,0 +1,20 @@
+algo: mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: ipopt

examples/mpc/config_overrides/quadrotor_2D_attitude/mpc_quadrotor_2D_attitude_tracking.yaml

@@ -0,0 +1,20 @@
+algo: mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: ipopt

examples/mpc/config_overrides/quadrotor_2D_attitude/quadrotor_2D_attitude_stabilization.yaml

@@ -0,0 +1,51 @@
+task_config:
+  seed: 1337
+  info_in_reset: True
+  ctrl_freq: 50
+  pyb_freq: 1000
+  gui: False
+  physics: pyb
+  quad_type: 4
+
+  init_state_randomization_info:
+    init_x:
+      distrib: 'uniform'
+      low: -1
+      high: 1
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_z:
+      distrib: 'uniform'
+      low: 0.5
+      high: 1.5
+    init_z_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_theta:
+      distrib: 'uniform'
+      low: -0.2
+      high: 0.2
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+  randomized_init: True
+  randomized_inertial_prop: False
+
+  task: stabilization
+  task_info:
+    stabilization_goal: [0, 1]
+    stabilization_goal_tolerance: 0.0
+
+  episode_len_sec: 6
+  cost: quadratic
+  done_on_out_of_bound: True
+
+  constraints:
+  - constraint_form: default_constraint
+    constrained_variable: input
+  - constraint_form: default_constraint
+    constrained_variable: state