diff --git a/paseos/actors/actor_builder.py b/paseos/actors/actor_builder.py index 350ca88..51a6424 100644 --- a/paseos/actors/actor_builder.py +++ b/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 diff --git a/paseos/attitude/attitude_model.py b/paseos/attitude/attitude_model.py index 4c4610f..91d0dad 100644 --- a/paseos/attitude/attitude_model.py +++ b/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 diff --git a/paseos/attitude/disturbance_calculations.py b/paseos/attitude/disturbance_calculations.py index 4bdc9fe..3c1b6e2 100644 --- a/paseos/attitude/disturbance_calculations.py +++ b/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(): diff --git a/paseos/tests/attitude_test.py b/paseos/tests/attitude_test.py new file mode 100644 index 0000000..8a4b3d0 --- /dev/null +++ b/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 +