From 6281e1e71153144f27079b1dc085015198eef4fa Mon Sep 17 00:00:00 2001 From: Shambhuraj Sawant Date: Tue, 30 Jul 2024 11:19:36 +0200 Subject: [PATCH] Updated Quad dynamics with identified model --- .../envs/gym_pybullet_drones/base_aviary.py | 13 +++++++------ .../envs/gym_pybullet_drones/quadrotor.py | 9 ++------- 2 files changed, 9 insertions(+), 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 index 39d0125d8..ed722f4d5 100644 --- 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 index 1c30c184d..6c0ba4a92 100644 --- 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,))