Updated Quad dynamics with identified model

utiasDSL · Jul 30, 2024 · 6281e1e · 6281e1e
1 parent 51bb32c
commit 6281e1e
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 13 deletions.
diff --git a/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py b/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py
@@ -223,9 +223,11 @@ def _reset_simulation(self):
         self.rpy = np.zeros((self.NUM_DRONES, 3))
         self.vel = np.zeros((self.NUM_DRONES, 3))
         self.ang_v = np.zeros((self.NUM_DRONES, 3))
-        if (self.PHYSICS == Physics.DYN or self.PHYSICS == Physics.RK4
-                or self.PHYSICS == Physics.DYN_2D or self.PHYSICS == Physics.DYN_SI):
-            self.rpy_rates = np.zeros((self.NUM_DRONES, 3))
+        self.rpy_rates = np.zeros((self.NUM_DRONES, 3))
+        # if (self.PHYSICS == Physics.DYN or self.PHYSICS == Physics.RK4
+        #         or self.PHYSICS == Physics.DYN_2D or self.PHYSICS == Physics.DYN_SI):
+        #     self.rpy_rates = np.zeros((self.NUM_DRONES, 3))
+
         # Set PyBullet's parameters.
         p.resetSimulation(physicsClientId=self.PYB_CLIENT)
         p.setGravity(0, 0, -self.GRAVITY_ACC, physicsClientId=self.PYB_CLIENT)
@@ -288,7 +290,7 @@ def _advance_simulation(self, clipped_action, disturbance_force=None):
                 elif self.PHYSICS == Physics.DYN_2D:
                     self._dynamics_2d(rpm, i)
                 elif self.PHYSICS == Physics.DYN_SI:
-                    self._dynamics_si(clipped_action, i)
+                    self._dynamics_si(clipped_action[i, :], i)
                 elif self.PHYSICS == Physics.RK4:
                     self._dynamics_rk4(clipped_action[i, :], i)
                 elif self.PHYSICS == Physics.PYB_GND:
@@ -423,7 +425,7 @@ def _get_drone_state_vector(self, nth_drone):
         state = np.hstack([
             self.pos[nth_drone, :], self.quat[nth_drone, :],
             self.rpy[nth_drone, :], self.vel[nth_drone, :],
-            self.ang_v[nth_drone, :], self.last_clipped_action[nth_drone, :]
+            self.ang_v[nth_drone, :], self.rpy_rates[nth_drone, :], self.last_clipped_action[nth_drone, :]
         ])
         # state.reshape(20, )
         return state.copy()
@@ -844,7 +846,6 @@ def setup_dynamics_si_expression(self):
                            # 60 * (60 * (P - theta) - theta_dot)
                            -143.9 * theta - 13.02 * theta_dot + 122.5 * Pitch
                            )
-
         self.X_dot_fun = cs.Function("X_dot", [X, U], [X_dot])
 
     def _show_drone_local_axes(self, nth_drone):

diff --git a/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py b/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py
@@ -598,9 +598,6 @@ def _setup_symbolic(self, prior_prop={}, **kwargs):
                                theta_dot,
                                # 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)
@@ -883,6 +880,7 @@ def _setup_disturbances(self):
         self.DISTURBANCE_MODES['dynamics']['dim'] = int(self.QUAD_TYPE)
         super()._setup_disturbances()
 
+    # noinspection PyUnreachableCode
     def _preprocess_control(self, action):
         """Converts the action passed to .step() into motors' RPMs (ndarray of shape (4,)).
 
@@ -988,10 +986,7 @@ def _get_observation(self):
         """
 
         full_state = self._get_drone_state_vector(0)
-        pos, _, rpy, vel, ang_v, _ = np.split(full_state, [3, 7, 10, 13, 16])
-        # print(rpy)
-        # if rpy[1] > 0.2:
-        #     print([pos, rpy, vel, ang_v])
+        pos, _, rpy, vel, ang_v, rpy_rate, _ = np.split(full_state, [3, 7, 10, 13, 16, 19])
         if self.QUAD_TYPE == QuadType.ONE_D:
             # {z, z_dot}.
             self.state = np.hstack([pos[2], vel[2]]).reshape((2,))