working attitude controller for quad 2d

examples/pid/config_overrides/quadrotor_2D_attitude_5s/quadrotor_2D_attitude_5s_tracking.yaml

@@ -0,0 +1,56 @@
+task_config:
+  seed: 1337
+  info_in_reset: True
+  ctrl_freq: 50
+  pyb_freq: 1000
+  gui: False
+  physics: pyb
+  quad_type: 5
+
+  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
+
+  episode_len_sec: 9
+  cost: quadratic
+  done_on_out_of_bound: False

examples/pid/pid_experiment.py

@@ -138,6 +138,8 @@ def run(gui=False, n_episodes=1, n_steps=None, save_data=False):
         system = f'quadrotor_{str(config.task_config.quad_type)}D'
         if config.task_config.quad_type == 4:
             system = 'quadrotor_2D_attitude'
+        elif config.task_config.quad_type == 5:
+            system = 'quadrotor_2D_attitude_5s'
     else:
         system = config.task
 
@@ -157,6 +159,11 @@ def run(gui=False, n_episodes=1, n_steps=None, save_data=False):
             graph1_2 = 5
             graph3_1 = 0
             graph3_2 = 2
+        elif system == 'quadrotor_2D_attitude_5s':
+            graph1_1 = 4
+            graph1_2 = 5
+            graph3_1 = 0
+            graph3_2 = 2
         elif system == 'quadrotor_3D':
             graph1_1 = 6
             graph1_2 = 9
@@ -249,6 +256,32 @@ def run(gui=False, n_episodes=1, n_steps=None, save_data=False):
             # save the plot
             plt.savefig(os.path.join(config.output_dir, 'pitch_action.png'))
 
+        if config.task_config.task == Task.TRAJ_TRACKING and system == "quadrotor_2D_attitude_5s":
+            _, ax4 = plt.subplots()
+            ax4.plot(trajs_data['timestamp'][0][0:] - trajs_data['timestamp'][0][0], trajs_data['obs'][0][1:, 4])
+            ax4.set_xlabel(r'time')
+            ax4.set_ylabel(r'pitch (rad) (obs)')
+            # ax4.set_ylim([-0.4, 0.4])
+            plt.tight_layout()
+            # save the plot
+            plt.savefig(os.path.join(config.output_dir, 'pitch_obs.png'))
+
+            _, ax6 = plt.subplots()
+            ax6.plot(trajs_data['timestamp'][0][0:] - trajs_data['timestamp'][0][0], trajs_data['action'][0][0:, 0])
+            ax6.set_xlabel(r'time')
+            ax6.set_ylabel(r'Thrust (rad) (action)')
+            plt.tight_layout()
+            # save the plot
+            plt.savefig(os.path.join(config.output_dir, 'thrust_action.png'))
+
+            _, ax7 = plt.subplots()
+            ax7.plot(trajs_data['timestamp'][0][0:] - trajs_data['timestamp'][0][0], trajs_data['action'][0][0:, 1])
+            ax7.set_xlabel(r'time')
+            ax7.set_ylabel(r'pitch (rad) (action)')
+            plt.tight_layout()
+            # save the plot
+            plt.savefig(os.path.join(config.output_dir, 'pitch_action.png'))
+
 
 if __name__ == '__main__':
     run()

examples/pid/pid_experiment.sh

@@ -4,6 +4,7 @@
 
 # SYS='quadrotor_2D'
 SYS='quadrotor_2D_attitude'
+#SYS='quadrotor_2D_attitude_5s'
 # SYS='quadrotor_3D'
 
 # TASK='stabilization'

examples/rl/rl_experiment.py

@@ -84,6 +84,13 @@ def run(gui=False, plot=True, n_episodes=1, n_steps=None, curr_path='.'):
             graph3_2 = 2
             graph4_1 = 0
             graph4_2 = 1
+        elif system == 'quadrotor_5D':
+            graph1_1 = 4
+            graph1_2 = 5
+            graph3_1 = 0
+            graph3_2 = 2
+            graph4_1 = 0
+            graph4_2 = 1
 
         _, ax = plt.subplots()
         ax.plot(results['obs'][0][:, graph1_1], results['obs'][0][:, graph1_2], 'r--', label='RL Trajectory')
@@ -122,6 +129,19 @@ def run(gui=False, plot=True, n_episodes=1, n_steps=None, curr_path='.'):
         ax3.set_box_aspect(0.5)
         ax3.legend(loc='upper right')
 
+        if config.task == Environment.QUADROTOR and system == 'quadrotor_2D':
+            # _, ax4 = plt.subplots()
+            # ax4.plot(results['timestamp'][0][:], results['action'][0][:, graph4_1], 'r', label='Thrust')
+            # ax4.set_ylabel(r'Thrust')
+            # _, ax5 = plt.subplots()
+            # ax5.plot(results['timestamp'][0][:], results['action'][0][:, graph4_2], 'r', label='Pitch')
+            # ax5.set_ylabel(r'Pitch')
+            _, ax6 = plt.subplots()
+            ax6.plot(results['timestamp'][0][:], results['obs'][0][1:, 4], 'r', label='Thrust')
+            ax6.set_ylabel(r'Pitch')
+            _, ax7 = plt.subplots()
+            ax7.plot(results['timestamp'][0][:], results['obs'][0][1:, 5], 'r', label='Pitch')
+            ax7.set_ylabel(r'Pitch rate')
         if config.task == Environment.QUADROTOR and system == 'quadrotor_4D':
             _, ax4 = plt.subplots()
             ax4.plot(results['timestamp'][0][:], results['action'][0][:, graph4_1], 'r', label='Action: Thrust')
@@ -135,20 +155,16 @@ def run(gui=False, plot=True, n_episodes=1, n_steps=None, curr_path='.'):
             _, ax7 = plt.subplots()
             ax7.plot(results['timestamp'][0][:], results['obs'][0][1:, 5], 'r', label='Obs: Pitch rate')
             ax7.set_ylabel(r'Obs: Pitch rate')
-
-        if config.task == Environment.QUADROTOR and system == 'quadrotor_2D':
-            # _, ax4 = plt.subplots()
-            # ax4.plot(results['timestamp'][0][:], results['action'][0][:, graph4_1], 'r', label='Thrust')
-            # ax4.set_ylabel(r'Thrust')
-            # _, ax5 = plt.subplots()
-            # ax5.plot(results['timestamp'][0][:], results['action'][0][:, graph4_2], 'r', label='Pitch')
-            # ax5.set_ylabel(r'Pitch')
+        if config.task == Environment.QUADROTOR and system == 'quadrotor_5D':
+            _, ax4 = plt.subplots()
+            ax4.plot(results['timestamp'][0][:], results['action'][0][:, graph4_1], 'r', label='Action: Thrust')
+            ax4.set_ylabel(r'Action: Thrust')
+            _, ax5 = plt.subplots()
+            ax5.plot(results['timestamp'][0][:], results['action'][0][:, graph4_2], 'r', label='Action: Pitch')
+            ax5.set_ylabel(r'Action: Pitch')
             _, ax6 = plt.subplots()
-            ax6.plot(results['timestamp'][0][:], results['obs'][0][1:, 4], 'r', label='Thrust')
-            ax6.set_ylabel(r'Pitch')
-            _, ax7 = plt.subplots()
-            ax7.plot(results['timestamp'][0][:], results['obs'][0][1:, 5], 'r', label='Pitch')
-            ax7.set_ylabel(r'Pitch rate')
+            ax6.plot(results['timestamp'][0][:], results['obs'][0][1:, 4], 'r', label='Obs: Pitch')
+            ax6.set_ylabel(r'Obs: Pitch')
 
         plt.tight_layout()
         plt.show()

examples/rl/rl_experiment.sh

@@ -2,8 +2,8 @@
 
 #SYS='cartpole'
 #SYS='quadrotor_2D'
-#SYS='quadrotor_2D_attitude'
-SYS='quadrotor_2D_attitude_5s'
+SYS='quadrotor_2D_attitude'
+#SYS='quadrotor_2D_attitude_5s'
 #SYS='quadrotor_3D'
 
 #TASK='stab'

examples/rl/train_rl_model.sh

@@ -2,8 +2,8 @@
 
 #SYS='cartpole'
 #SYS='quadrotor_2D'
-#SYS='quadrotor_2D_attitude'
-SYS='quadrotor_2D_attitude_5s'
+SYS='quadrotor_2D_attitude'
+#SYS='quadrotor_2D_attitude_5s'
 #SYS='quadrotor_3D'
 
 #TASK='stab'

safe_control_gym/envs/gym_pybullet_drones/base_aviary.py

@@ -314,6 +314,8 @@ def _advance_simulation(self, clipped_action, disturbance_force=None):
                 p.stepSimulation(physicsClientId=self.PYB_CLIENT)
             # Save the last applied action (e.g. to compute drag).
             self.last_clipped_action = clipped_action
+        # if self.PHYSICS == Physics.DYN_2D:
+        #     self._set_pybullet_information()
         # Update and store the drones kinematic information.
         self._update_and_store_kinematic_information()
 
@@ -356,6 +358,20 @@ def render(self, mode='human', close=False):
                     format(self.ang_v[i, 0], self.ang_v[i, 1], self.ang_v[i,
                                                                           2]))
 
+    def _set_pybullet_information(self):
+        """Set pybullet state information from external simulation"""
+        for i in range(self.NUM_DRONES):
+            # Set PyBullet's state.
+            p.resetBasePositionAndOrientation(self.DRONE_IDS[i],
+                                              self.pos[i, :],
+                                              p.getQuaternionFromEuler(self.rpy[i, :]),
+                                              physicsClientId=self.PYB_CLIENT)
+            # Note: the base's velocity only stored and not used #
+            p.resetBaseVelocity(self.DRONE_IDS[i],
+                                self.vel[i, :],
+                                self.ang_v[i, :],  # ang_vel not computed by DYN
+                                physicsClientId=self.PYB_CLIENT)
+
     def _update_and_store_kinematic_information(self):
         '''Updates and stores the drones kinematic information.
 
@@ -679,6 +695,7 @@ def _dynamics_2d(self, rpm, nth_drone):
         ang_v = self.ang_v[nth_drone, :]
         rpy_rates = self.rpy_rates[nth_drone, :]
         # rotation = np.array(p.getMatrixFromQuaternion(quat)).reshape(3, 3)
+
         # Compute forces and torques.
         forces = np.array(rpm ** 2) * self.KF
         # Update state with discrete time dynamics.
@@ -691,6 +708,7 @@ def _dynamics_2d(self, rpm, nth_drone):
         X_dot = self.X_dot_fun(state, action).full()[:, 0]
         next_state = state + X_dot*self.PYB_TIMESTEP
 
+        # Updated information
         pos = np.array([next_state[0], 0, next_state[4]])
         rpy = np.array([0, next_state[7], 0])
         vel = np.array([next_state[1], 0, next_state[5]])

safe_control_gym/envs/gym_pybullet_drones/quadrotor.py

@@ -18,7 +18,7 @@
 from safe_control_gym.envs.gym_pybullet_drones.quadrotor_utils import (AttitudeControl, QuadType, cmd2pwm,
                                                                        pwm2rpm)
 from safe_control_gym.math_and_models.symbolic_systems import SymbolicModel
-from safe_control_gym.math_and_models.transformations import csRotXYZ, transform_trajectory
+from safe_control_gym.math_and_models.transformations import csRotXYZ, transform_trajectory, get_quaternion_from_euler
 
 
 class Quadrotor(BaseAviary):
@@ -596,7 +596,12 @@ def _setup_symbolic(self, prior_prop={}, **kwargs):
                                z_dot,
                                (18.112984649321753 * T + 3.7613154938448576) * cs.cos(theta) - g,
                                theta_dot,
-                               -60.00143727772195 * theta + 60.00143727772195 * P)
+                               # 60 * (60 * (P - theta) - theta_dot)
+                               -143.9 * theta - 13.02 * theta_dot + 122.5 * P
+                               # 2.027 * (64.2 * P - 72.76 * theta) - 13.75 * theta_dot
+                               # -267.8 * theta - 13.41 * theta_dot + 187.5 * P
+                               # - 44.43 * theta - 10.33 * theta_dot + 32.81 * P
+                               )
             # Define observation.
             Y = cs.vertcat(x, x_dot, z, z_dot, theta, theta_dot)
         elif self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE_5S:
@@ -909,6 +914,7 @@ def _preprocess_control(self, action):
             # print(f"collective_thrust: {collective_thrust}, pitch: {pitch}")
 
             _, quat = p.getBasePositionAndOrientation(self.DRONE_IDS[0], physicsClientId=self.PYB_CLIENT)
+            # quat = get_quaternion_from_euler(self.rpy[0, :])
             thrust_action = self.attitude_control._dslPIDAttitudeControl(collective_thrust / 4,
                                                                          quat, np.array([0, pitch, 0]))
             # input thrust is in Newton

safe_control_gym/envs/gym_pybullet_drones/quadrotor_utils.py

@@ -5,6 +5,7 @@
 
 import numpy as np
 import pybullet as p
+from safe_control_gym.math_and_models.transformations import quaternion_rotation_matrix, euler_from_quaternion
 from scipy.spatial.transform import Rotation
 
 
@@ -157,6 +158,8 @@ def _dslPIDAttitudeControl(self,
         sim_timestep = self.sim_timestep
         cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat)).reshape(3, 3)
         cur_rpy = np.array(p.getEulerFromQuaternion(cur_quat))
+        # cur_rotation = quaternion_rotation_matrix(cur_quat)
+        # cur_rpy = euler_from_quaternion(cur_quat)
         target_quat = (Rotation.from_euler('XYZ', target_euler, degrees=False)).as_quat()
         w, x, y, z = target_quat
         target_rotation = (Rotation.from_quat([w, x, y, z])).as_matrix()