diff --git a/paseos/actors/actor_builder.py b/paseos/actors/actor_builder.py index f6b0d8c4..8242156f 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 170f6bfd..7a4d27f0 100644 --- a/paseos/attitude/attitude_model.py +++ b/paseos/attitude/attitude_model.py @@ -14,6 +14,7 @@ rodrigues_rotation, get_rpy_angles, rotate_body_vectors, + rpy_to_body, ) @@ -97,12 +98,23 @@ 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: @@ -110,7 +122,12 @@ def _calculate_disturbance_torque(self): 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) ) diff --git a/paseos/attitude/disturbance_calculations.py b/paseos/attitude/disturbance_calculations.py index 7e527950..76d964bd 100644 --- a/paseos/attitude/disturbance_calculations.py +++ b/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): diff --git a/paseos/tests/attitude_test.py b/paseos/tests/attitude_test.py index e774d885..8f0a82b0 100644 --- a/paseos/tests/attitude_test.py +++ b/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"""