Added tests and removed bugs from gravity disturbance model

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

@@ -102,8 +102,10 @@ def calculate_disturbance_torque(self, position, velocity, euler_angles):
         if "aerodynamic" in self._actor.get_disturbances():
             T += calculate_aero_torque()
         if "gravitational" in self._actor.get_disturbances():
-            T += calculate_grav_torque(self._actor_pointing_vector_body,earth_rotation_vector_in_body,
-                                       self._actor._moment_of_inertia, position[0])
+            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)
+            T += calculate_grav_torque(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():
             T += calculate_magnetic_torque()
         return T

paseos/attitude/disturbance_calculations.py

@@ -12,7 +12,7 @@ def calculate_aero_torque():
     return np.array(T)
 
 
-def calculate_grav_torque(u_r, u_n, J, h):
+def calculate_grav_torque(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
@@ -24,7 +24,7 @@ def calculate_grav_torque(u_r, u_n, J, h):
         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
+        h (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
@@ -34,15 +34,12 @@ def calculate_grav_torque(u_r, u_n, J, h):
     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_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)
-    T = [tg[0,0], tg[1,1], tg[2,2]]
-    return np.array(T)
+    return np.array(tg)
 
 
 def calculate_magnetic_torque():

paseos/tests/attitude_test.py

@@ -0,0 +1,92 @@
+import numpy as np
+import pykep as pk
+import sys
+
+sys.path.append("../..")
+import paseos
+from paseos import ActorBuilder, SpacecraftActor, load_default_cfg
+
+
+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"""
+    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(sat1._attitude_model._actor_angular_acceleration == 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
+    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
+