Created disturbance model function and partly implemented it in the a…

…ttitude model

paseos/attitude/attitude_model.py

@@ -12,6 +12,7 @@
     rodrigues_rotation,
     get_rpy_angles,
     rotate_body_vectors,
+    rpy_to_body,
 )
 
 
@@ -80,18 +81,29 @@ def nadir_vector(self):
         u = np.array(self._actor.get_position(self._actor.local_time))
         return -u / np.linalg.norm(u)
 
-    def calculate_disturbance_torque(self):
+    def calculate_disturbance_torque(self, position, velocity, euler_angles):
         """Compute total torque due to user specified disturbances
 
+        Args:
+            position (np.ndarray): position vector of RPY reference frame wrt ECI frame
+            velocity (np.ndarray): velocity of the spacecraft in earth reference frame, centered on spacecraft
+            euler_angles (np.ndarray): [roll, pitch, yaw] in radians
+
         Returns:
             np.array([Tx, Ty, Tz]): total combined torques in Nm expressed in the spacecraft body frame
         """
 
+        # Transform the earth rotation vector to the body reference frame, assuming the rotation vector is the z-axis
+        # of the earth-centered-inertial (eci) frame
+        earth_rotation_vector_in_rpy = eci_to_rpy(np.array([0, 0, 1]), position, velocity)
+        earth_rotation_vector_in_body = rpy_to_body(earth_rotation_vector_in_rpy, euler_angles)
+
         T = np.array([0.0, 0.0, 0.0])
         if "aerodynamic" in self._actor.get_disturbances():
             T += calculate_aero_torque()
         if "gravitational" in self._actor.get_disturbances():
-            T += calculate_grav_torque()
+            T += calculate_grav_torque(self._actor_pointing_vector_body,earth_rotation_vector_in_body,
+                                       self._actor._moment_of_inertia, self._actor._previous_altitude)
         if "magnetic" in self._actor.get_disturbances():
             T += calculate_magnetic_torque()
         return T

paseos/attitude/disturbance_calculations.py

@@ -12,11 +12,36 @@ def calculate_aero_torque():
     return np.array(T)
 
 
-def calculate_grav_torque():
-    # calculations for torques
-    # T must be in actor body fixed frame (to be discussed)
-    T = [0, 0, 0]
-    return np.array(T)
+def calculate_grav_torque(u_r, u_n, J, h):
+    """
+    Equation for gravity gradient torque with up to J2 effect from:
+    https://doi.org/10.1016/j.asr.2018.06.025, chapter 3.3
+    This function currently only works for Earth centered orbits
+
+    Args:
+        u_r (np.array): Unit vector pointing from Satellite center of gravity to Earth's center of gravity
+        u_n (np.array): Unit vector along the Earth's rotation axis, in the spacecraft body frame
+        J (np.array): The satellites moment of inertia, in the form of [[Ixx Ixy Ixz]
+                                                                        [Iyx Iyy Iyx]
+                                                                        [Izx Izy Izz]]
+        h (float): The altitude of the spacecraft in m
+
+    Returns:
+        np.array: total gravitational torques in Nm expressed in the spacecraft body frame
+    """
+    # Constants
+    mu = 3.986004418e14  # Earth's gravitational parameter, [m^3/s^2]
+    J2 = 1.0826267e-3  # Earth's J2 coefficient, from https://ocw.tudelft.nl/wp-content/uploads/AE2104-Orbital-Mechanics-Slides_8.pdf
+    Re = 6371000  # Earth's radius, [m]
+
+    # Simulation parameters
+    r = h + Re  # Radial distance from Satellite center of gravity to Earth's center of gravity, [m]
+
+    tg_term_1 = np.cross((3 * mu * u_r / (r ** 3)), J * u_r)
+    tg_term_2 = 30 * np.dot(u_r, u_n) * (np.cross(u_n, J * u_r) + np.cross(u_r, J * u_n))
+    tg_term_3 = np.cross((15 - 105 * np.dot(u_r, u_n) ** 2 * u_r), J * u_r) + np.cross(6 * u_n, J * u_r)
+    tg = tg_term_1 + mu * J2 * Re ** 2 / (2 * r ** 5) * (tg_term_2 + tg_term_3)
+    return np.array(tg)
 
 
 def calculate_magnetic_torque():