Merge pull request #200 from aidotse/gravity-disturbances

Gravity disturbances

paseos/actors/actor_builder.py

@@ -333,6 +333,7 @@ def set_geometric_model(
             local_actor=actor, actor_mass=mass, vertices=vertices, faces=faces, scale=scale
         )
         actor._mesh = geometric_model.set_mesh()
+        actor._center_of_gravity = geometric_model.find_center_of_gravity()
         actor._moment_of_inertia = geometric_model.find_moment_of_inertia
 
     @staticmethod

paseos/attitude/attitude_model.py

@@ -14,6 +14,7 @@
     rodrigues_rotation,
     get_rpy_angles,
     rotate_body_vectors,
+    rpy_to_body,
 )
 
 
@@ -97,20 +98,36 @@ 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):
-        """Compute total torque due to user specified disturbances.
+
+    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
+
+
         T = np.array([0.0, 0.0, 0.0])
 
         if self._actor.has_attitude_disturbances:
             # TODO add solar disturbance
             if "aerodynamic" in self._actor.get_disturbances():
                 T += calculate_aero_torque()
             if "gravitational" in self._actor.get_disturbances():
-                T += calculate_grav_torque()
+                nadir_vector_in_rpy = eci_to_rpy(self.nadir_vector(), position, velocity)
+                nadir_vector_in_body = rpy_to_body(nadir_vector_in_rpy, euler_angles)
+                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 += calculate_grav_torque(self._actor.central_body.planet, nadir_vector_in_body,earth_rotation_vector_in_body,
+                                           self._actor._moment_of_inertia(), np.linalg.norm(position))
             if "magnetic" in self._actor.get_disturbances():
                 time = self._actor.local_time
                 T += calculate_magnetic_torque(
@@ -132,7 +149,10 @@ def _calculate_angular_acceleration(self):
         # Euler's equation for rigid body rotation: a = I^(-1) (T - w x (Iw))
         # with: a = angular acceleration, I = inertia matrix, T = torque vector, w = angular velocity
         self._actor_angular_acceleration = np.linalg.inv(I) @ (
-            self._calculate_disturbance_torque()
+
+            self.calculate_disturbance_torque(position=np.array(self._actor.get_position(self._actor.local_time)),
+                                              velocity=np.array(self._actor.get_position_velocity(self._actor.local_time)[1]),
+                                              euler_angles=self._actor_attitude_in_rad)
             - np.cross(self._actor_angular_velocity, I @ self._actor_angular_velocity)
         )
 

paseos/attitude/disturbance_calculations.py

@@ -13,11 +13,34 @@ 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(central_body, u_r, u_n, J, r):
+    """
+    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]]
+        r (float): The distance from the center of the Earth to the satellite
+
+    Returns:
+        np.array: total gravitational torques in Nm expressed in the spacecraft body frame
+    """
+    # Constants
+    mu = central_body.mu_self  # 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 = central_body.radius  # Earth's radius, [m]
+
+
+    tg_term_1 = (3 * mu / (r ** 3))*np.cross(u_r, np.dot(J,u_r))
+    tg_term_2 = 30 * np.dot(u_r, u_n)*(np.cross(u_n, np.dot(J,u_r)) + np.cross(u_r, np.dot(J,u_n)))
+    tg_term_3 = np.cross((15 - 105 * np.dot(u_r, u_n) ** 2) * u_r, np.dot(J,u_r)) + np.cross(6 * u_n, np.dot(J ,u_n))
+    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(m_earth, m_sat, position, velocity, attitude):

paseos/tests/attitude_test.py

@@ -1,15 +1,101 @@
 """Tests to see whether the attitude and disturbance models work as intended"""
-
 import numpy as np
 import pykep as pk
 import sys
 
 sys.path.append("../..")
 import paseos
-from paseos import ActorBuilder, SpacecraftActor
+from paseos import ActorBuilder, SpacecraftActor, load_default_cfg
 from paseos.utils.reference_frame_transfer import eci_to_rpy, rpy_to_body
 
 
+def gravity_disturbance_cube_test():
+    """This test checks whether a 3-axis symmetric, uniform body (a cube with constant density, and cg at origin)
+    creates no angular acceleration/velocity due to gravity. The spacecraft is initially positioned with the z-axis
+    aligned with the nadir vector."""
+    earth = pk.planet.jpl_lp("earth")
+
+    # Define local actor
+    sat1 = ActorBuilder.get_actor_scaffold("sat1", SpacecraftActor, pk.epoch(0))
+    ActorBuilder.set_orbit(sat1, [7000000, 0, 0], [0, 8000.0, 0], pk.epoch(0), earth)
+    ActorBuilder.set_geometric_model(sat1, mass=100)
+    ActorBuilder.set_attitude_model(sat1)
+    ActorBuilder.set_disturbances(sat1, gravitational=True)
+
+    # init simulation
+    sim = paseos.init_sim(sat1)
+
+    # Check initial conditions
+    assert np.all(sat1._attitude_model._actor_angular_velocity == 0.0)
+
+    # run simulation for 1 period
+    orbital_period = 2 * np.pi * np.sqrt((6371000 + 7000000) ** 3 / 3.986004418e14)
+    sim.advance_time(orbital_period, 0)
+    nadir = sat1._attitude_model.nadir_vector()
+
+    # check conditions after 1 orbit
+    assert np.all(np.round(sat1._attitude_model._actor_angular_acceleration,10) == 0.0)
+
+
+def gravity_disturbance_pole_test():
+    """This test checks whether a 2-axis symmetric, uniform body (a pole (10x1x1) with constant density, and cg at
+    origin) stabilises in orbit due to gravitational acceleration. The attitude at time zero is the z-axis pointing
+    towards earth. Hence, the accelerations should occur only in the y-axis.
+    It additionally checks the implementation of custom meshes of the geometric model"""
+
+    vertices = [
+                [-5, -0.5, -0.5],
+                [-5, -0.5, 0.5],
+        [-5, 0.5, -0.5],
+        [-5, 0.5, 0.5],
+        [5, -0.5, -0.5],
+        [5, -0.5, 0.5],
+        [5, 0.5, -0.5],
+        [5, 0.5, 0.5],
+    ]
+    faces = [
+        [0, 1, 3],
+        [0, 3, 2],
+        [0, 2, 6],
+        [0, 6, 4],
+        [1, 5, 3],
+        [3, 5, 7],
+        [2, 3, 7],
+        [2, 7, 6],
+        [4, 6, 7],
+        [4, 7, 5],
+        [0, 4, 1],
+        [1, 4, 5],
+    ]
+
+    earth = pk.planet.jpl_lp("earth")
+
+    # Define local actor
+    sat1 = ActorBuilder.get_actor_scaffold("sat1", SpacecraftActor, pk.epoch(0))
+    ActorBuilder.set_orbit(sat1, [7000000, 0, 0], [0, 8000.0, 0], pk.epoch(0), earth)
+    ActorBuilder.set_geometric_model(sat1, mass=100, vertices=vertices, faces=faces)
+    orbital_period = 2 * np.pi * np.sqrt((6371000 + 7000000) ** 3 / 3.986004418e14)
+    ActorBuilder.set_attitude_model(sat1)#, actor_initial_angular_velocity=[0,2*np.pi/orbital_period,0])
+    ActorBuilder.set_disturbances(sat1, gravitational=True)
+
+    # init simulation
+    cfg = load_default_cfg()  # loading cfg to modify defaults
+    cfg.sim.dt = 100.0  # setting higher timestep to run things quickly
+    sim = paseos.init_sim(sat1, cfg)
+
+
+    # Check initial conditions
+    assert np.all(sat1._attitude_model._actor_attitude_in_rad == 0.0)
+
+    # run simulation for 1 period
+    for i in range(11):
+        sim.advance_time(orbital_period*0.1, 0)
+
+    # check conditions after 0.1 orbit, satellite should have acceleration around y-axis to align pole towards earth
+    assert np.round(sat1._attitude_model._actor_angular_acceleration[0],10) == 0.0
+    assert not np.round(sat1._attitude_model._actor_angular_acceleration[1],10) == 0.0
+
+
 def test_attitude_model():
     """Testing the attitude model with no disturbances and known angular velocity.
     One actor has orbit in Earth inertial x-y plane (equatorial) with initial velocity which rotates the actor with 180°
@@ -88,7 +174,7 @@ def test_attitude_model():
     assert np.all(sat2._attitude_model._actor_angular_velocity == np.array([0.0, 0.0, 0.0]))
 
 
-def test_attitude_thermal_model():
+def attitude_thermal_model_test():
     """Testing the attitude model with no disturbances and no angular velocity, and ensuring the attitude model doesn't
     break the thermal model (or vice versa)"""
     earth = pk.planet.jpl_lp("earth")
@@ -130,7 +216,7 @@ def test_attitude_thermal_model():
     assert np.round(sat1.temperature_in_K, 3) == 278.522
 
 
-def test_attitude_and_orbit():
+def attitude_and_orbit_test():
     """This test checks both the orbit calculations, as well as the attitude.
     The input is a simple orbit, and the angular velocity if 2pi/period. This means the initial conditions should be
     the same as the conditions after one orbit"""