fixed noise issue in attitude controller for quad 2d

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_tracking.yaml

@@ -0,0 +1,59 @@
+task_config:
+  seed: 1337
+  info_in_reset: True
+  ctrl_freq: 60
+  pyb_freq: 1200
+  physics: pyb
+  quad_type: 4
+
+  init_state:
+    init_x: 0
+    init_x_dot: 0
+    init_z: 1.15
+    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.01
+      high: 0.01
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.01
+      high: 0.01
+    init_z:
+      distrib: 'uniform'
+      low: -0.01
+      high: 0.01
+    init_z_dot:
+      distrib: 'uniform'
+      low: -0.01
+      high: 0.01
+    init_theta:
+      distrib: 'uniform'
+      low: -0.02
+      high: 0.02
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.02
+      high: 0.02
+
+  task: traj_tracking
+  task_info:
+    trajectory_type: figure8
+    num_cycles: 1
+    trajectory_plane: 'xz'
+    trajectory_position_offset: [ 0, 1.2 ]
+    trajectory_scale: 0.5
+
+  inertial_prop:
+    M: 0.027
+    Iyy: 1.4e-05
+
+  episode_len_sec: 10
+  cost: rl_reward
+  obs_goal_horizon: 1

examples/lqr/lqr_experiment.py

@@ -15,7 +15,7 @@
 from safe_control_gym.utils.registration import make
 
 
-def run(gui=True, n_episodes=1, n_steps=None, save_data=False):
+def run(gui=False, n_episodes=1, n_steps=None, save_data=False):
     '''The main function running LQR and iLQR experiments.
 
     Args:

examples/lqr/lqr_experiment.sh

@@ -2,12 +2,13 @@
 
 # LQR Experiment.
 
-SYS='cartpole'
+#SYS='cartpole'
 # SYS='quadrotor_2D'
+SYS='quadrotor_2D_attitude'
 # SYS='quadrotor_3D'
 
-TASK='stabilization'
-# TASK='tracking'
+#TASK='stabilization'
+TASK='tracking'
 
 ALGO='lqr'
 # ALGO='ilqr'

examples/pid/config_overrides/quadrotor_2D/quadrotor_2D_tracking.yaml

@@ -2,47 +2,55 @@ task_config:
   seed: 1337
   info_in_reset: True
   ctrl_freq: 60
-  pyb_freq: 840
+  pyb_freq: 1200
   gui: False
-  physics: pyb
+  physics: dyn_2d
   quad_type: 2
 
+  init_state:
+    init_x: 0
+    init_x_dot: 0
+    init_z: 1.15
+    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: -1
-      high: 1
+      low: -0.01
+      high: 0.01
     init_x_dot:
       distrib: 'uniform'
-      low: -0.1
-      high: 0.1
+      low: -0.01
+      high: 0.01
     init_z:
       distrib: 'uniform'
-      low: 0.5
-      high: 1.5
+      low: -0.01
+      high: 0.01
     init_z_dot:
       distrib: 'uniform'
-      low: -0.1
-      high: 0.1
+      low: -0.01
+      high: 0.01
     init_theta:
       distrib: 'uniform'
-      low: -0.2
-      high: 0.2
+      low: -0.02
+      high: 0.02
     init_theta_dot:
       distrib: 'uniform'
-      low: -0.1
-      high: 0.1
-  randomized_init: True
-  randomized_inertial_prop: False
+      low: -0.02
+      high: 0.02
 
   task: traj_tracking
   task_info:
     trajectory_type: figure8
     num_cycles: 1
     trajectory_plane: 'xz'
-    trajectory_position_offset: [0, 1]
+    trajectory_position_offset: [ 0, 1.2 ]
     trajectory_scale: 0.5
 
-  episode_len_sec: 9
+  episode_len_sec: 10
   cost: quadratic
-  done_on_out_of_bound: True
+  done_on_out_of_bound: False

examples/pid/config_overrides/quadrotor_2D_attitude/quadrotor_2D_attitude_tracking.yaml

@@ -1,44 +1,56 @@
 task_config:
   seed: 1337
   info_in_reset: True
-  ctrl_freq: 60
-  pyb_freq: 840
+  ctrl_freq: 50
+  pyb_freq: 1000
   gui: False
   physics: pyb
   quad_type: 4
 
+  init_state:
+    init_x: 0
+    init_x_dot: 0
+    init_z: 1.15
+    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: -1
-      high: 1
+      low: -0.01
+      high: 0.01
     init_x_dot:
       distrib: 'uniform'
-      low: -0.1
-      high: 0.1
+      low: -0.01
+      high: 0.01
     init_z:
       distrib: 'uniform'
-      low: 0.5
-      high: 1.5
+      low: -0.01
+      high: 0.01
     init_z_dot:
       distrib: 'uniform'
-      low: -0.1
-      high: 0.1
+      low: -0.01
+      high: 0.01
     init_theta:
       distrib: 'uniform'
-      low: -0.2
-      high: 0.2
-  randomized_init: True
-  randomized_inertial_prop: False
+      low: -0.02
+      high: 0.02
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.02
+      high: 0.02
 
   task: traj_tracking
   task_info:
     trajectory_type: figure8
     num_cycles: 1
     trajectory_plane: 'xz'
-    trajectory_position_offset: [0, 1]
+    trajectory_position_offset: [ 0, 1.2 ]
     trajectory_scale: 0.5
 
   episode_len_sec: 9
   cost: quadratic
-  done_on_out_of_bound: True
+  done_on_out_of_bound: False