diff --git a/environment.yml b/environment.yml index 8fb1b7a..f0c1c99 100644 --- a/environment.yml +++ b/environment.yml @@ -9,7 +9,7 @@ dependencies: - myst-parser # for markdown math in docs - pykep>=2.6 # core non-optional dependency - pyquaternion>=0.9.9 # core non-optional dependency - - pytest # for tests + - pytest<8.0.0 # for tests. v8.0.0 is not supporting asyncio. To be checked later. - pytest-asyncio # for tests involving activities - python>=3.8 # core non-optional dependency - scikit-spatial>=6.5.0 # core non-optional dependency diff --git a/paseos/actors/actor_builder.py b/paseos/actors/actor_builder.py index 17d022a..e5b15af 100644 --- a/paseos/actors/actor_builder.py +++ b/paseos/actors/actor_builder.py @@ -13,7 +13,8 @@ from ..thermal.thermal_model import ThermalModel from ..power.power_device_type import PowerDeviceType from ..radiation.radiation_model import RadiationModel -from paseos.geometric_model.geometric_model import GeometricModel +from .spacecraft_body_model import SpacecraftBodyModel +from ..attitude.attitude_model import AttitudeModel, TorqueDisturbanceModel class ActorBuilder: @@ -307,32 +308,50 @@ def set_position(actor: BaseActor, position: list): logger.debug(f"Setting position {position} on actor {actor}") @staticmethod - def set_geometric_model( + def set_spacecraft_body_model( actor: SpacecraftActor, mass: float, vertices=None, faces=None, scale: float = 1 ): """Define geometry of the spacecraft actor. This is done in the spacecraft body reference frame, and can be - transformed to the inertial/PASEOS reference frame using the reference frane transformations in the attitude + transformed to the inertial/PASEOS reference frame using the reference frame transformations in the attitude model. When used in the attitude model, the geometric model is in the body reference frame. Args: actor (SpacecraftActor): Actor to update. mass (float): Mass of the spacecraft in kg. - vertices (list): List of all vertices of the mesh in terms of distance (in m) from origin of body frame. - Coordinates of the corners of the object. If not selected, it will default to a cube that can be scaled. + vertices (list): List of coordinates [x, y, z] of the vertices of the mesh w.r.t. the body frame [m]. + If not selected, it will default to a cube that can be scaled. by the scale. Uses Trimesh to create the mesh from this and the list of faces. faces (list): List of the indexes of the vertices of a face. This builds the faces of the satellite by defining the three vertices to form a triangular face. For a cuboid each face is split into two triangles. Uses Trimesh to create the mesh from this and the list of vertices. scale (float): Parameter to scale the cuboid by, defaults to 1. """ + # check for spacecraft actor + assert isinstance( + actor, SpacecraftActor + ), "Body model is only supported for SpacecraftActors." + + logger.trace("Checking mass values for sensibility.") assert mass > 0, "Mass is > 0" + # Check if the actor already has mass. + if actor.mass: + logger.warning("The actor already had a mass. Overriding old mass value.") + actor._mass = mass - geometric_model = GeometricModel( - local_actor=actor, actor_mass=mass, vertices=vertices, faces=faces, scale=scale + + # Check if the actor already had a has_spacecraft_body_model. + if actor.has_spacecraft_body_model: + logger.warning( + "The actor already had a spacecraft body model. Overriding old body bodel." + ) + + # Create a spacraft body model + actor._spacecraft_body_model = SpacecraftBodyModel( + actor_mass=mass, vertices=vertices, faces=faces, scale=scale ) - actor._mesh = geometric_model.set_mesh() - actor._moment_of_inertia = geometric_model.find_moment_of_inertia + # Logging + logger.debug(f"Added spacecraft body model to actor {actor}.") @staticmethod def set_power_devices( @@ -519,6 +538,127 @@ def set_thermal_model( power_consumption_to_heat_ratio=power_consumption_to_heat_ratio, ) + @staticmethod + def set_attitude_disturbances( + actor: SpacecraftActor, + aerodynamic: bool = False, + gravitational: bool = False, + magnetic: bool = False, + ): + """Enables the attitude disturbances to be considered in the attitude modelling for an actor. + + Args: + actor (SpacecraftActor): The actor to add to. + aerodynamic (bool): Whether to consider aerodynamic disturbances in the attitude model. Defaults to False. + gravitational (bool): Whether to consider gravity disturbances in the attitude model. Defaults to False. + magnetic (bool): Whether to consider magnetic disturbances in the attitude model. Defaults to False. + """ + # check for spacecraft actor + assert isinstance( + actor, SpacecraftActor + ), "Attitude disturbances are only supported for SpacecraftActors." + + assert ( + actor.has_attitude_model + ), "The actor has no attitude model. Impossible to set attitude disturbances." + + # Create a list with user specified disturbances which are considered in the attitude modelling. + disturbance_list = [] + + # Disturbance list name + disturbance_list_name = "" + + if aerodynamic: + disturbance_list.append(TorqueDisturbanceModel.Aerodynamic) + disturbance_list_name += "Aerodynamic, " + if gravitational: + disturbance_list.append(TorqueDisturbanceModel.Gravitational) + disturbance_list_name += "Gravitational, " + if magnetic: + disturbance_list.append(TorqueDisturbanceModel.Magnetic) + disturbance_list_name += "Magnetic, " + + if len(disturbance_list) > 0: + # Set attitude models. + actor._attitude_model._disturbances = disturbance_list + logger.debug( + f"Added {disturbance_list_name[:-2]} attitude torque disturbance models to actor {actor}." + ) + else: + logger.warning("No disturbance model was specified.") + + @staticmethod + def set_attitude_model( + actor: SpacecraftActor, + actor_initial_attitude_in_rad: list[float] = [0.0, 0.0, 0.0], + actor_initial_angular_velocity: list[float] = [0.0, 0.0, 0.0], + actor_pointing_vector_body: list[float] = [0.0, 0.0, 1.0], + actor_residual_magnetic_field_body: list[float] = [0.0, 0.0, 0.0], + accommodation_coefficient: float = 0.85, + ): + """Add an attitude model to the actor based on initial conditions: attitude (roll, pitch & yaw angles) + and angular velocity vector, modeling the evolution of the user specified pointing vector. + + Args: + actor (SpacecraftActor): Actor to model. + actor_initial_attitude_in_rad (list of floats): Actor's initial attitude. + Defaults to [0.0, 0.0, 0.0]. + actor_initial_angular_velocity (list of floats): Actor's initial angular velocity. + Defaults to [0.0, 0.0, 0.0]. + actor_pointing_vector_body (list of floats): Actor's pointing vector with respect to the body frame. + Defaults to [0.0, 0.0, 1.0]. + actor_residual_magnetic_field_body (list of floats): Actor's residual magnetic dipole moment vector + in the body frame. Only needed if magnetic torque disturbances are modelled. + Please, refer to [Tai L. Chow (2006) p. 148 - 149]. Defaults to [0.0, 0.0, 0.0]. + accommodation_coefficient (float): Accommodation coefficient used for Aerodynamic torque disturbance calculation. + Defaults to 0.85. + + """ + # check for spacecraft actor + assert isinstance( + actor, SpacecraftActor + ), "Attitude model is only supported for SpacecraftActors" + + # Check if actor has already an attitude model. + if actor.has_attitude_model: + logger.warning( + "The actor already had an attitude model. Overriding old attitude model." + ) + + assert ( + np.asarray(actor_initial_attitude_in_rad).shape[0] == 3 + and np.asarray(actor_initial_attitude_in_rad).ndim == 1 + ), "actor_initial_attitude_in_rad shall be [3] shaped." + + assert ( + np.asarray(actor_initial_angular_velocity).shape[0] == 3 + and np.asarray(actor_initial_angular_velocity).ndim == 1 + ), "actor_initial_angular_velocity shall be [3] shaped." + + assert ( + np.asarray(actor_pointing_vector_body).shape[0] == 3 + and np.asarray(actor_pointing_vector_body).ndim == 1 + ), "actor_pointing_vector_body shall be [3] shaped." + + assert ( + np.asarray(actor_residual_magnetic_field_body).shape[0] == 3 + and np.asarray(actor_residual_magnetic_field_body).ndim == 1 + ), "actor_residual_magnetic_field_body shall be [3] shaped." + + logger.trace("Checking accommodation coefficient for sensibility.") + assert accommodation_coefficient > 0, "Accommodation coefficient shall be positive." + + # Set attitude model. + actor._attitude_model = AttitudeModel( + local_actor=actor, + actor_initial_attitude_in_rad=actor_initial_attitude_in_rad, + actor_initial_angular_velocity=actor_initial_angular_velocity, + actor_pointing_vector_body=actor_pointing_vector_body, + actor_residual_magnetic_field_body=actor_residual_magnetic_field_body, + accommodation_coefficient=accommodation_coefficient, + ) + logger.debug(f"Added attitude model to actor {actor}.") + @staticmethod def add_comm_device(actor: BaseActor, device_name: str, bandwidth_in_kbps: float): """Creates a communication device. @@ -539,7 +679,10 @@ def add_comm_device(actor: BaseActor, device_name: str, bandwidth_in_kbps: float @staticmethod def add_custom_property( - actor: BaseActor, property_name: str, initial_value: Any, update_function: Callable + actor: BaseActor, + property_name: str, + initial_value: Any, + update_function: Callable, ): """Adds a custom property to the actor. This e.g. allows tracking any physical the user would like to track. diff --git a/paseos/actors/base_actor.py b/paseos/actors/base_actor.py index bae022c..d6bdac4 100644 --- a/paseos/actors/base_actor.py +++ b/paseos/actors/base_actor.py @@ -47,6 +47,9 @@ class BaseActor(ABC): # Tracks the current activity _current_activity = None + # Mass + _mass = None + # The following variables are used to track last evaluated state vectors to avoid recomputation. _previous_position = None _time_of_previous_position = None @@ -148,6 +151,15 @@ def has_radiation_model(self) -> bool: """ return hasattr(self, "_radiation_model") and self._radiation_model is not None + @property + def has_spacecraft_body_model(self) -> bool: + """Returns true if actor's body is modeled, else false. + + Returns: + bool: bool indicating presence. + """ + return hasattr(self, "_spacecraft_body_model") and self._spacecraft_body_model is not None + @property def has_thermal_model(self) -> bool: """Returns true if actor's temperature is modeled, else false. @@ -157,6 +169,15 @@ def has_thermal_model(self) -> bool: """ return hasattr(self, "_thermal_model") and self._thermal_model is not None + @property + def has_attitude_model(self) -> bool: + """Returns true if actor's attitude is modeled, else false. + + Returns: + bool: bool indicating presence. + """ + return hasattr(self, "_attitude_model") and self._attitude_model is not None + @property def mass(self) -> float: """Returns actor's mass in kg. diff --git a/paseos/actors/spacecraft_actor.py b/paseos/actors/spacecraft_actor.py index 30f7527..eadfa0e 100644 --- a/paseos/actors/spacecraft_actor.py +++ b/paseos/actors/spacecraft_actor.py @@ -1,5 +1,6 @@ -from loguru import logger +import numpy as np import pykep as pk +from loguru import logger from paseos.actors.base_actor import BaseActor from paseos.power import discharge_model @@ -19,8 +20,10 @@ class SpacecraftActor(BaseActor): # Actor's mass in kg _mass = None + _spacecraft_body_model = None _thermal_model = None _radiation_model = None + _attitude_model = None # If radiation randomly restarted the device _was_interrupted = False @@ -28,6 +31,9 @@ class SpacecraftActor(BaseActor): # If radiation permanently killed this device _is_dead = False + # Default temperature [K] + _default_temperature_in_K = 300 + def __init__( self, name: str, @@ -50,6 +56,48 @@ def set_was_interrupted(self): """Sets this device to "was_interrupted=True" indicating current activities were interrupted.""" self._was_interrupted = True + def set_body_attitude(self, attitude_in_rad): + """Sets the spacecraft attitude in [roll, pitch, yaw] angles. + + Args: + actor_attitude_in_rad (numpy array): actor's attitude in [roll, pitch, yaw] angles [rad]. + """ + assert self.has_attitude_model, "The actor has no attitude model." + assert ( + isinstance(attitude_in_rad, np.array) + and attitude_in_rad.shape[1] == 3 + and attitude_in_rad.ndim == 1 + ), "attitude_in_rad shall be a numpy array [3] shaped." + self._attitude_model._actor_attitude_in_rad = attitude_in_rad + + def set_body_pointing_vector(self, pointing_vector_body): + """Sets the spacecraft angular velocity in body frame. + + Args: + pointing_vector_body (numpy array): actor's pointing vector in body frame [m]. + """ + assert self.has_attitude_model, "The actor has no attitude model." + assert ( + isinstance(pointing_vector_body, np.array) + and pointing_vector_body.shape[1] == 3 + and pointing_vector_body.ndim == 1 + ), "pointing_vector_body shall be a numpy array [3] shaped." + self._attitude_model._actor_pointing_vector_body = pointing_vector_body + + def set_body_angular_velocity(self, angular_velocity_body): + """Sets the spacecraft angular velocity in body frame. + + Args: + angular_velocity_body (numpy array): actor's angular velocity in body frame [rad/s]. + """ + assert self.has_attitude_model, "The actor has no attitude model." + assert ( + isinstance(angular_velocity_body, np.array) + and angular_velocity_body.shape[1] == 3 + and angular_velocity_body.ndim == 1 + ), "angular_velocity_body shall be a numpy array [3] shaped." + self._attitude_model._actor_angular_velocity = angular_velocity_body + @property def was_interrupted(self) -> bool: """Returns whether the actor was interrupted in its activity. @@ -104,6 +152,24 @@ def mass(self) -> float: """ return self._mass + @property + def body_mesh(self) -> np.array: + """Gives the mesh of the satellite. + + Returns: + np.array: Mesh of the satellite. + """ + return self._spacecraft_body_model._body_mesh + + @property + def attitude_disturbances(self) -> list: + """Gives attitude disturbances list. + + Returns: + list: attitude disturbances list. + """ + return self._attitude_model._disturbances + @property def temperature_in_K(self) -> float: """Returns the current temperature of the actor in K. @@ -113,6 +179,15 @@ def temperature_in_K(self) -> float: """ return self._thermal_model.temperature_in_K + @property + def default_temperature_in_K(self) -> float: + """Returns the default temperature of the actor in K. + + Returns: + float: Actor default temperature in Kelvin. + """ + return self._default_temperature_in_K + @property def temperature_in_C(self) -> float: """Returns the current temperature of the actor in C. @@ -164,3 +239,69 @@ def charge(self, duration_in_s: float): self = charge_model.charge(self, duration_in_s) logger.debug(f"New battery level is {self.battery_level_in_Ws}") + + @property + def attitude_in_rad(self): + """Returns the current attitude of the actor in radians. + + Returns: + list[floats]: actor attitude in radians. + """ + if type(self._attitude_model._actor_attitude_in_rad) is np.ndarray: + return np.ndarray.tolist(self._attitude_model._actor_attitude_in_rad) + else: + return self._attitude_model._actor_attitude_in_rad + + @property + def attitude_in_deg(self): + """Returns the current attitude of the actor in degrees. + + Returns: + list[floats]: actor attitude in degrees. + """ + if type(self._attitude_model._actor_attitude_in_rad) is np.ndarray: + return np.ndarray.tolist(self._attitude_model._actor_attitude_in_rad * 180 / np.pi) + else: + return np.ndarray.tolist( + np.array(self._attitude_model._actor_attitude_in_rad) * 180 / np.pi + ) + + @property + def pointing_vector(self): + """Returns the spacecraft pointing vector in the Earth-centered inertial frame. + + Returns: + np.ndarray (x, y, z). + """ + return self._attitude_model._actor_pointing_vector_eci + + @property + def angular_velocity(self): + """Returns the spacecraft angular velocity vector in the Earth-centered inertial frame. + + Returns: + np.ndarray (owega_x, omega_y, omega_z). + """ + return self._attitude_model._actor_angular_velocity_eci + + @property + def body_moment_of_inertia_body(self) -> np.array: + """Gives the moment of inertia of the actor, assuming constant density, with respect to the body frame. + + Returns: + np.array: Mass moments of inertia for the actor + + I is the moment of inertia, in the form of [[Ixx Ixy Ixz] + [Iyx Iyy Iyx] + [Izx Izy Izz]] + """ + return self._spacecraft_body_model._body_moment_of_inertia_body + + @property + def body_center_of_gravity_body(self) -> np.array: + """Gives the volumetric center of mass of the actor with respect to the body frame. + + Returns: + np.array: Coordinates of the center of gravity of the mesh. + """ + return self._spacecraft_body_model._body_center_of_gravity_body diff --git a/paseos/actors/spacecraft_body.py b/paseos/actors/spacecraft_body.py new file mode 100644 index 0000000..2555fa1 --- /dev/null +++ b/paseos/actors/spacecraft_body.py @@ -0,0 +1,51 @@ +import trimesh + + +def create_body_mesh(vertices=None, faces=None, scale=1): + """Creates the mesh of the satellite. If no vertices input is given, it defaults to a cuboid scaled by the + scale value. The default without scale values is a cube with 1m sides. This uses the python module Trimesh. + Args: + vertices (list): List of all vertices of the mesh in terms of distance (in m) from origin of body frame. + Coordinates of the corners of the object. If not selected, it will default to a cube that can be scaled + by the scale. Uses Trimesh to create the mesh from this and the list of faces. + faces (list): List of the indexes of the vertices of a face. This builds the faces of the satellite by + defining the three vertices to form a triangular face. For a cuboid each face is split into two + triangles. Uses Trimesh to create the mesh from this and the list of vertices. + scale (float): Parameter to scale the cuboid by, defaults to 1. + + Returns: + mesh: Trimesh mesh of the satellite + """ + if vertices is None: + # Defines the corners of the mesh, values are in meters, from the origin of the body frame. + vertices = [ + [-0.5, -0.5, -0.5], + [-0.5, -0.5, 0.5], + [-0.5, 0.5, -0.5], + [-0.5, 0.5, 0.5], + [0.5, -0.5, -0.5], + [0.5, -0.5, 0.5], + [0.5, 0.5, -0.5], + [0.5, 0.5, 0.5], + ] + # List of three vertices to form a triangular face of the satellite. + # Two triangular faces are used per side of the cuboid. + 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], + ] + + # Set mesh + mesh = trimesh.Trimesh(vertices, faces).apply_scale(scale) + + return mesh diff --git a/paseos/actors/spacecraft_body_model.py b/paseos/actors/spacecraft_body_model.py new file mode 100644 index 0000000..99f80ee --- /dev/null +++ b/paseos/actors/spacecraft_body_model.py @@ -0,0 +1,157 @@ +from loguru import logger +import numpy as np +import trimesh + + +class SpacecraftBodyModel: + """This model describes the geometry of the spacecraft + Currently it assumes the spacecraft to be a cuboid shape, with width, length and height + """ + + _body_mesh = None + _body_center_of_gravity_body = None + _body_moment_of_inertia_body = None + + def __init__(self, actor_mass, vertices=None, faces=None, scale=1) -> None: + """Describes the geometry of the spacecraft and outputs relevant parameters related to the spacecraft body. + If no vertices or faces are provided, defaults to a cube with unit length sides. This is in the spacecraft body + reference frame and can be transformed to the inertial/PASEOS reference frame by using the transformations in the + attitude model. + + Args: + actor_mass (float): Actor's mass in kg. + vertices (list): List of all vertices of the mesh in terms of distance (in m) from origin of body frame. + Coordinates of the corners of the object. If not selected, it will default to a cube that can be scaled + by the scale. Uses Trimesh to create the mesh from this and the list of faces. + faces (list): List of the indexes of the vertices of a face. This builds the faces of the satellite by + defining the three vertices to form a triangular face. For a cuboid each face is split into two + triangles. Uses Trimesh to create the mesh from this and the list of vertices. + scale (float): Parameter to scale the cuboid by, defaults to 1. + """ + logger.trace("Initializing cuboid geometrical model.") + + self._body_mass = actor_mass + self._body_mesh = self._create_body_mesh(vertices=vertices, faces=faces, scale=scale) + self._body_moment_of_inertia_body = self._body_mesh.moment_inertia * self._body_mass + self._body_center_of_gravity_body = self._body_mesh.center_mass + + @staticmethod + def _is_cuboid(vertices, faces): + """Check if the mesh corresponds to a cuboid. + + Args: + vertices (list): List of all vertices of the mesh in terms of distance (in m) from origin of body frame. + faces (list): List of the indexes of the vertices of a face. This builds the faces of the satellite by + defining the three vertices to form a triangular face. For a cuboid each face is split into two + triangles. Uses Trimesh to create the mesh from this and the list of vertices. + Returns: + bool: True, if the mesh corresponds to a cuboid. + """ + # Convert to numpy + vertices = np.array(vertices) + # Checking expected number of vertices and faces + if len(vertices) != 8 or len(faces) != 12: + return False + + # Minimum norm vectors in faces + min_vectors_in_faces = [] + + # Taking the two vectors for each face having the smallest norm. + for face in faces: + v1 = vertices[face[0]] - vertices[face[1]] + v2 = vertices[face[0]] - vertices[face[2]] + v3 = vertices[face[1]] - vertices[face[2]] + n1 = np.linalg.norm(v1) + n2 = np.linalg.norm(v2) + n3 = np.linalg.norm(v3) + + # norm sorted + norm_sorted = sorted([n1, n2, n3]) + + # mininum norm vectors + min_norm_vectors = [] + if n1 in norm_sorted[:2]: + min_norm_vectors.append(v1) + + if n2 in norm_sorted[:2]: + min_norm_vectors.append(v2) + + if n3 in norm_sorted[:2]: + min_norm_vectors.append(v3) + + min_vectors_in_faces.append(min_norm_vectors) + + # The vectors in min_vectors_in_faces shall be orthogonal. + + # Angles between vectors + for min_norm_vector_in_face in min_vectors_in_faces: + if np.dot(min_norm_vector_in_face[0], min_norm_vector_in_face[1]): + return False + + return True + + def _create_body_mesh(self, vertices=None, faces=None, scale=1): + """Creates the mesh of the satellite. If no vertices input is given, it defaults to a cuboid scaled by the + scale value. The default without scale values is a cube with 1m sides. This uses the python module Trimesh. + Args: + vertices (list): List of all vertices of the mesh in terms of distance (in m) from origin of body frame. + Coordinates of the corners of the object. If not selected, it will default to a cube that can be scaled + by the scale. Uses Trimesh to create the mesh from this and the list of faces. + faces (list): List of the indexes of the vertices of a face. This builds the faces of the satellite by + defining the three vertices to form a triangular face. For a cuboid each face is split into two + triangles. Uses Trimesh to create the mesh from this and the list of vertices. + scale (float): Parameter to scale the cuboid by, defaults to 1. + + Returns: + mesh: Trimesh mesh of the satellite + """ + if vertices is None: + # Defines the corners of the mesh, values are in meters, from the origin of the body frame. + vertices = [ + [-0.5, -0.5, -0.5], + [-0.5, -0.5, 0.5], + [-0.5, 0.5, -0.5], + [-0.5, 0.5, 0.5], + [0.5, -0.5, -0.5], + [0.5, -0.5, 0.5], + [0.5, 0.5, -0.5], + [0.5, 0.5, 0.5], + ] + # List of three vertices to form a triangular face of the satellite. + # Two triangular faces are used per side of the cuboid. + 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], + ] + + else: + assert ( + len(np.asarray(vertices).shape) == 2 and np.asarray(vertices).shape[1] == 3 + ), "Vertices shall be [N, 3] shaped." + if not (SpacecraftBodyModel._is_cuboid(vertices=vertices, faces=faces)): + raise NotImplementedError("Only cuboid meshes are currently supported.") + + # Create mesh + logger.trace("Creating the spacecraft body mesh.") + mesh = trimesh.Trimesh(vertices, faces).apply_scale(scale) + + return mesh + + @property + def body_mesh(self) -> np.array: + """Gives the mesh of the satellite. + + Returns: + np.array: Mesh of the satellite. + """ + return self._body_mesh diff --git a/paseos/attitude/attitude_model.py b/paseos/attitude/attitude_model.py new file mode 100644 index 0000000..a234e96 --- /dev/null +++ b/paseos/attitude/attitude_model.py @@ -0,0 +1,326 @@ +import numpy as np +import pykep as pk +from ..actors.spacecraft_actor import SpacecraftActor +from loguru import logger + +from .disturbance_torques_utils import ( + compute_magnetic_torque, + compute_gravity_gradient_torque, +) +from ..utils.reference_frame_transfer import ( + eci_to_rpy, + rpy_to_eci, + body_to_rpy, + rodrigues_rotation, + get_rpy_angles, + rotate_body_vectors, + rpy_to_body, + frame_rotation, +) + +from enum import Enum + + +class TorqueDisturbanceModel(Enum): + """Describes the different torque disturbance models supported. + 1 - SpacePlot + """ + + Aerodynamic = 1 + Gravitational = 2 + Magnetic = 3 + + +class AttitudeModel: + """This model describes the attitude (Roll, Pitch and Yaw) evolution of a spacecraft actor. + Starting from an initial attitude and angular velocity, the spacecraft response to disturbance torques is simulated. + + The model allows for one pointing vector to be defined in the actor body frame for visualization and possibly + could be used for future antenna pointing applications. Its position in time within the Earth-centered inertial + frame is also calculated alongside the general body attitude. + + The attitude calculations are based in three reference frames, refer to reference_frame_transfer.py in utils folder. + """ + + # Spacecraft actor. + _actor = None + # Actor attitude in rad. + _actor_attitude_in_rad = None + # Actor angular velocity expressed in body frame [rad/s]. + _actor_angular_velocity = None + # Actor angular acceleation expressed in body frame [rad/s^2]. + _actor_angular_acceleration = None + # Actor pointing vector expressed in the body frame. + _actor_pointing_vector_body = None + # Actor pointing vector expressed in intertial frame. + _actor_pointing_vector_eci = None + # Attitude disturbances experienced by the actor. + _disturbances = None + # Accommodation parameter for Aerodynamic torque disturbance calculations. + # Please refer to: "Roto-Translational Spacecraft Formation Control Using Aerodynamic Forces"; + # Ran. S, Jihe W., et al.; 2017. + _accommodation_coefficient = None + # Earth J2 coefficient. + _J2_coefficient = 1.0826267e-3 # Dimensionless constant. + # Universal gas constant + _R_gas = 8.314462 # [J/(K mol)] + + def __init__( + self, + local_actor, + # initial conditions: + actor_initial_attitude_in_rad: list[float] = [0.0, 0.0, 0.0], + actor_initial_angular_velocity: list[float] = [0.0, 0.0, 0.0], + # pointing vector in body frame: (defaults to body z-axis) + actor_pointing_vector_body: list[float] = [0.0, 0.0, 1.0], + actor_residual_magnetic_field_body: list[float] = [0.0, 0.0, 0.0], + accommodation_coefficient: float = 0.85, + ): + """Creates an attitude model to model actor attitude based on + initial conditions (initial attitude and angular velocity) and + external disturbance torques. + + Args: + actor (SpacecraftActor): Actor to model. + actor_initial_attitude_in_rad (list of floats, optional): Actor's initial attitude ([roll, pitch, yaw]) angles. + Defaults to [0., 0., 0.]. + actor_initial_angular_velocity (list of floats, optional) in the body frame: Actor's initial angular velocity + vector. Defaults to [0., 0., 0.]. + actor_pointing_vector_body (list of floats, optional): User defined vector in the Actor body. + Defaults to [0., 0., 1]. + actor_residual_magnetic_field_body (list of floats, optional): Actor's own magnetic field modeled + as dipole moment vector. Defaults to [0., 0., 0.]. + accommodation_coefficient (float): Accommodation coefficient used for Aerodynamic torque disturbance calculation. + Defaults to 0.85. + """ + assert isinstance(local_actor, SpacecraftActor), ( + "local_actor must be a " "SpacecraftActor" "." + ) + + logger.trace("Initializing thermal model.") + self._actor = local_actor + # convert to np.ndarray + self._actor_attitude_in_rad = np.array(actor_initial_attitude_in_rad) + self._actor_angular_velocity = np.array(actor_initial_angular_velocity) + + # normalize inputted pointing vector & convert to np.ndarray + self._actor_pointing_vector_body = np.array(actor_pointing_vector_body) / np.linalg.norm( + np.array(actor_pointing_vector_body) + ) + + # pointing vector expressed in Earth-centered inertial frame + self._actor_pointing_vector_eci = rpy_to_eci( + body_to_rpy(self._actor_pointing_vector_body, self._actor_attitude_in_rad), + np.array(self._actor.get_position(self._actor.local_time)), + np.array(self._actor.get_position_velocity(self._actor.local_time)[1]), + ) + + # angular velocity vector expressed in Earth-centered inertial frame + self._actor_angular_velocity_eci = rpy_to_eci( + body_to_rpy(self._actor_angular_velocity, self._actor_attitude_in_rad), + np.array(self._actor.get_position(self._actor.local_time)), + np.array(self._actor.get_position_velocity(self._actor.local_time)[1]), + ) + + # actor residual magnetic field (modeled as dipole) + self._actor_residual_magnetic_field_body = np.array(actor_residual_magnetic_field_body) + + # Accommodation coefficient + self._accommodation_coefficient = accommodation_coefficient + + def _nadir_vector(self): + """Compute unit vector pointing towards earth, in the inertial frame. + + Returns: + np array ([x, y, z]): unit nadir vector in ECIF (Earth-centered inertial frame) + """ + u = np.array(self._actor.get_position(self._actor.local_time)) + nadir_vector = -u / np.linalg.norm(u) + logger.trace(f"Nadir vector: {nadir_vector}") + return nadir_vector + + def compute_disturbance_torque(self, position, velocity, euler_angles, current_temperature_K): + """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. + current_temperature_K (float): current temperature in Kelvin. + 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._disturbances is not None: + if self._actor.central_body.planet.name != "earth": + raise NotImplementedError( + "Models for torque disturbances are implemented only for Earth as a central body." + ) + + # TODO Add solar disturbances. Refer to issue: https://github.com/aidotse/PASEOS/issues/206 + + if TorqueDisturbanceModel.Aerodynamic in self._actor.attitude_disturbances: + # TODO improve integration and testing of aerodynamic model. + # Refer to issue: https://github.com/aidotse/PASEOS/issues/207 + raise ValueError("Aerodynamic torque disturbance model is not implemented.") + + if TorqueDisturbanceModel.Gravitational in self._actor.attitude_disturbances: + # Extract nadir vectors in different reference systems + 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) + # Extract Earth rotation vector in different reference systems + 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 + ) + # Computing gravitational torque + gravitational_torque = compute_gravity_gradient_torque( + self._actor.central_body.planet, + nadir_vector_in_body, + earth_rotation_vector_in_body, + self._actor.body_moment_of_inertia_body, + np.linalg.norm(position), + self._J2_coefficient, + ) + logger.debug(f"Gravitational torque: f{gravitational_torque}") + # Accumulate torque due to gravity gradients + T += gravitational_torque + + if TorqueDisturbanceModel.Magnetic in self._actor.attitude_disturbances: + time = self._actor.local_time + # Computing magnetic torque + magnetic_torque = compute_magnetic_torque( + m_earth=self._actor.central_body.magnetic_dipole_moment(time), + m_sat=self._actor_residual_magnetic_field_body, + position=self._actor.get_position(time), + velocity=self._actor.get_position_velocity(time)[1], + attitude=self._actor_attitude_in_rad, + ) + logger.debug(f"Magnetic torque: f{magnetic_torque}") + # Accumulating magnetic torque + T += magnetic_torque + + logger.trace(f"Disturbance torque: f{T}") + return T + + def _calculate_angular_acceleration(self, current_temperature_K): + """Calculate the spacecraft angular acceleration (external disturbance torques and gyroscopic accelerations). + Args: + current_temperature_K (float): current temperature in Kelvin. + """ + + # TODO in the future control torques could be added + # Euler's equation for rigid body rotation: a = I^(-1) (T - w x (Iw)) + # with: a = angular acceleration, body_moment_of_inertia = inertia matrix, T = torque vector, w = angular velocity + self._actor_angular_acceleration = np.linalg.inv( + self._actor.body_moment_of_inertia_body + ) @ ( + self.compute_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, + current_temperature_K=current_temperature_K, + ) + - np.cross( + self._actor_angular_velocity, + self._actor.body_moment_of_inertia_body @ self._actor_angular_velocity, + ) + ) + + def _body_rotation(self, dt, current_temperature_K): + """Calculates the rotation vector around which the spacecraft body rotates + based on angular acceleration, velocity, and current temperature. + + Args: + dt (float): time to advance. + current_temperature_K (float): current temperature in Kelvin. + + Returns: rotation vector of spacecraft body expressed in the RPY frame. + """ + # Calculate angular acceleration + self._calculate_angular_acceleration(current_temperature_K) + + # Add angular velocity + self._actor_angular_velocity += self._actor_angular_acceleration * dt + + # Body rotation vector: + body_rotation = self._actor_angular_velocity * dt + + # Return rotation vector in RPY frame + return body_to_rpy(body_rotation, self._actor_attitude_in_rad) + + def _body_axes_in_rpy(self): + """Transforms vectors expressed in the spacecraft body frame to the roll pitch yaw frame. + Vectors: - x, y, z axes + - user specified pointing vector + + Returns: transformed vectors. + """ + # principal axes: + x = body_to_rpy(np.array([1, 0, 0]), self._actor_attitude_in_rad) + y = body_to_rpy(np.array([0, 1, 0]), self._actor_attitude_in_rad) + z = body_to_rpy(np.array([0, 0, 1]), self._actor_attitude_in_rad) + + # pointing vector: + p = body_to_rpy(self._actor_pointing_vector_body, self._actor_attitude_in_rad) + return x, y, z, p + + def update_attitude(self, dt, current_temperature_K): + """Updates the actor attitude based on initial conditions and disturbance torques over time. + + Args: + dt (float): how far to advance the attitude computation. + current_temperature_K (float): current actor temperature in Kelvin. + """ + # time + t = self._actor.local_time + + # next position + next_position = np.array( + self._actor.get_position(pk.epoch(t.mjd2000 + dt * pk.SEC2DAY, "mjd2000")) + ) + + # position, velocity + position, velocity = np.array(self._actor.get_position_velocity(self._actor.local_time)) + + # Initial body vectors expressed in rpy frame: (x, y, z, custom pointing vector) + xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy = self._body_axes_in_rpy() + + # attitude change due to two rotations + # rpy frame rotation, in inertial frame: + theta_1 = frame_rotation(position, next_position, velocity) + # body rotation due to its physical rotation + theta_2 = self._body_rotation(dt, current_temperature_K) + + # rotate the body vectors in rpy frame with frame rotation + xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy = rotate_body_vectors( + xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy, theta_1 + ) + + # rotate the body rotation vector as well + theta_2 = rodrigues_rotation(theta_2, theta_1) + # rotate the body vectors in rpy frame with body rotation + xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy = rotate_body_vectors( + xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy, theta_2 + ) + + # update new attitude in ECI: + self._actor_attitude_in_rad = get_rpy_angles(xb_rpy, yb_rpy, zb_rpy) + + # update new angular velocity vector in ECI: + self._actor_angular_velocity_eci = rpy_to_eci( + body_to_rpy(self._actor_angular_velocity, self._actor_attitude_in_rad), + next_position, + velocity, + ) + # update pointing vector + self._actor_pointing_vector_eci = rpy_to_eci(pointing_vector_rpy, next_position, velocity) + + logger.trace(f"Actor's new attitude (Yaw, Pitch, Roll) is f{self._actor_attitude_in_rad}") + logger.trace( + f"Actor's new angular velocity (ECI) vector is f{self._actor_angular_velocity_eci}" + ) + logger.trace(f"Actor's new pointing vector (ECI) is f{self._actor_pointing_vector_eci}") diff --git a/paseos/attitude/disturbance_torques_utils.py b/paseos/attitude/disturbance_torques_utils.py new file mode 100644 index 0000000..7fd4d89 --- /dev/null +++ b/paseos/attitude/disturbance_torques_utils.py @@ -0,0 +1,316 @@ +"""this file contains functions that return attitude disturbance torque vectors expressed in the actor body frame.""" + +import numpy as np +from ..utils.reference_frame_transfer import ( + rpy_to_body, + eci_to_rpy, + compute_transformation_matrix_rpy_body, +) + + +def compute_aerodynamic_torque( + position, + velocity, + mesh, + actor_attitude_in_rad, + current_spacecraft_temperature_K, + central_body_radius_m, + gas_temperature_K, + R_gas, + accommodation_coefficient, +): + """Calculates the aerodynamic torque on the satellite. + The model used is taken from "Roto-Translational Spacecraft Formation Control Using Aerodynamic Forces"; Ran. S, + Jihe W., et al.; 2017. The mass density of the atmosphere is calculated from the best linear fit of the data + extracted from "Thermospheric mass density: A review"; Emmert J.T.; 2015. This function only works for Earth + centered orbits and satellites defined by a cuboid mesh as in the geometrical model. + + Future addition to the model can be the temperature of the spacecraft as an input parameter. At the moment the + temperature of the spacecraft is assumed in the model. The sensitivity of the torque with respect to the + temperature of the spacecraft and of the gas is low. + + Args: + position (np.array): distance from the satellite and the Earth's center of mass [m]. + velocity (np.array): velocity of the satellite in ECI reference frame [m/s]. + mesh (trimesh): mesh of the satellite from the geometric model. + actor_attitude_in_rad (np.array): spacecraft actor in rad. + current_spacecraft_temperature_K (float): current temperature in Kelvin. + central_body_radius_m (float): central body radius [m]. + gas_temperature_K (float): gas temperature [K]. + R_gas (float): Universal Gas Constant [J/(K mol)]. + accommodation_coefficient (float): accommodation coefficient. + Returns: + T (np.array): torque vector in the spacecraft body frame. + """ + + altitude = np.linalg.norm(position) - central_body_radius_m # [km] + density = 10 ** ( + -(altitude + 1285e3) / 151e3 + ) # equation describing the best linear fit for the data, [kg/m^3] + molecular_speed_ratio_t = np.linalg.norm(velocity) / np.sqrt( + 2 * R_gas * gas_temperature_K + ) # Used in the Cd and Cl calculation + molecular_speed_ratio_r = np.linalg.norm(velocity) / np.sqrt( + 2 * R_gas * current_spacecraft_temperature_K + ) # Used in the Cd and Cl calculation + + # Get the normal vectors of all the faces of the mesh in the spacecraft body reference frame, and then they get + # translated in the Roll Pitch Yaw frame with a transformation from paseos.utils.reference_frame_transfer.py + face_normals_sbf = mesh.face_normals[0:12] + # Get ntransformation matrix + transformation_matrix_rpy_body = compute_transformation_matrix_rpy_body(actor_attitude_in_rad) + face_normals_rpy = np.dot(transformation_matrix_rpy_body, face_normals_sbf.T).T + + # Get the velocity and transform it in the Roll Pitch Yaw frame. Get the unit vector associated with the latter + v_rpy = eci_to_rpy(velocity, position, velocity) + unit_v_rpy = v_rpy / np.linalg.norm(v_rpy) + + # Loop to get the normals, the areas, the angle of attack and the centroids of the faces with airflow, confronting them + # with the velocity vector direction. + # If the dot product between velocity and normal is positive, the plate corresponding to the normal is receiving + # airflow and is stored in the variable normals_faces_with_airflow. Similarly, the areas of the faces is stored in + # area_faces_airflow, and the angles of attack of the respective faces in alpha. The last three lines calculate the + # centroids of the faces that receive airflow, for later use in the torque calculation. + + # Initialize parameters and variables for the for loop. + j = 0 + normals_faces_with_airflow = np.zeros((6, 3)) # Maximum six faces have airflow + alpha = [ + 0, + 0, + 0, + 0, + 0, + 0, + ] # angle of attack matrix, stores the angles of attack in radiant + area_all_faces_mesh = mesh.area_faces # ordered list of all faces' areas + area_faces_airflow = [0, 0, 0, 0, 0, 0] + centroids_faces_airflow = [0, 0, 0, 0, 0, 0] + + for i in range(len(mesh.area_faces)): + if np.dot(face_normals_rpy[i], v_rpy) > 0: + normals_faces_with_airflow[j] = face_normals_rpy[i] + # get the area of the plate [i] which is receiving airflow + area_faces_airflow[j] = area_all_faces_mesh[i] + alpha[j] = np.arccos(np.dot(normals_faces_with_airflow[j], unit_v_rpy)) + face_vertices = mesh.vertices[mesh.faces[i]] + face_vertices_rpy = np.dot( + np.linalg.inv(transformation_matrix_rpy_body), face_vertices.T + ).T + # get the centroids of the face[i] with airflow + centroids_faces_airflow[j] = np.mean(face_vertices_rpy, axis=0) + j += 1 + + # Get the aerodynamic coefficient Cd and Cl for every plate and calculate the corresponding drag and lift forces + # for every plate [k] with airflow. Calculate the torque associated with the forces with the previously calculated + # centroid position of every plate. + + # Initialization of the variables + force_drag = [0, 0, 0, 0, 0, 0] + force_lift = [0, 0, 0, 0, 0, 0] + torque_aero = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]] + alpha = np.array(alpha) + + for k in range(j): + alpha_scalar = alpha[k] + + # C_d first term + C_d_term_1 = ( + molecular_speed_ratio_t + / np.sqrt(np.pi) + * ( + 4 * np.sin(alpha_scalar) ** 2 + + 2 * accommodation_coefficient * np.cos(2 * alpha_scalar) + ) + * np.exp(-((molecular_speed_ratio_t * np.sin(alpha_scalar)) ** 2)) + ) + + # C_d second term + C_d_term_2 = ( + np.sin(alpha_scalar) + * ( + 1 + + 2 * molecular_speed_ratio_t**2 + + (1 - accommodation_coefficient) + * (1 - 2 * molecular_speed_ratio_t**2 * np.cos(2 * alpha_scalar)) + ) + * np.math.erf(molecular_speed_ratio_t * np.sin(alpha_scalar)) + ) + + # C_d third term + C_d_term_3 = ( + accommodation_coefficient + * np.sqrt(np.pi) + * molecular_speed_ratio_t**2 + / molecular_speed_ratio_r + * np.sin(alpha_scalar) ** 2 + ) + + # C_d (computed according to Eq 2a of "Roto-Translational Spacecraft Formation Control Using Aerodynamic Forces"; + # Ran. S, Jihe W., et al.; 2017. ) + C_d = 1 / molecular_speed_ratio_t**2 * (C_d_term_1 + C_d_term_2 + C_d_term_3) + # C_l first term + C_l_term_1 = ( + 2 + / np.sqrt(np.pi) + * (2 - 2 * accommodation_coefficient) + * molecular_speed_ratio_t + * np.sin(alpha_scalar) + * np.exp(-((molecular_speed_ratio_t * np.sin(alpha_scalar)) ** 2)) + ) + + # C_l second term + C_l_term_2 = ( + 2 + * (2 - 2 * accommodation_coefficient) + * (molecular_speed_ratio_t * np.sin(alpha_scalar)) ** 2 + + (2 - accommodation_coefficient) + ) * np.math.erf(molecular_speed_ratio_t * np.sin(alpha_scalar)) + + # C_l third term + C_l_term_3 = ( + accommodation_coefficient + * np.sqrt(np.pi) + * molecular_speed_ratio_t**2 + / molecular_speed_ratio_r + * np.sin(alpha_scalar) + ) + + # C_l (computed according to Eq 2b of "Roto-Translational Spacecraft Formation Control Using Aerodynamic Forces"; + # Ran. S, Jihe W., et al.; 2017. ) + C_l = ( + np.cos(alpha[k]) / molecular_speed_ratio_t**2 * (C_l_term_1 + C_l_term_2 + C_l_term_3) + ) + + # Drag force on the plate [k]. Direction along the velocity vector. + force_drag[k] = ( + -0.5 + * density + * C_d + * area_faces_airflow[k] + * np.linalg.norm(velocity) ** 2 + * unit_v_rpy + ) + # Lift force on the plate [k]. Direction along the (v x n) x v direction, lift vector defined to be in that + # direction. Intermediate step to get v x n. + v_x_n_vector = np.cross(unit_v_rpy, normals_faces_with_airflow[k]) + not_norm_lift_vector = np.cross(v_x_n_vector, unit_v_rpy) + lift_vector = not_norm_lift_vector / np.linalg.norm(not_norm_lift_vector) + force_lift[k] = ( + -0.5 + * density + * C_l + * area_faces_airflow[k] + * np.linalg.norm(velocity) ** 2 + * lift_vector + ) + + # Torque calculated as the product between the distance of the centroid from the geometric center of the + # satellite and the sum of the forces. + torque_aero[k] = np.cross( + centroids_faces_airflow[k] - mesh.centroid, + (np.array(force_drag[k]) + np.array(force_lift[k])), + ) + + # Compute aero torque in the timestep as the vector sum of all the torques + torque_aero = np.array(torque_aero) + T_rpy = torque_aero.sum(axis=0) + T = transformation_matrix_rpy_body @ T_rpy + # substitutes 0 to any NaN value + T[np.isnan(T)] = 0 + + return np.array(T) + + +def compute_gravity_gradient_torque( + central_body, satellite_to_earth_unit_vector_body, roatation_axis_unit_vector_body, J, r, J2 +): + """ + 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: + satellite_to_earth_unit_vector_body (np.array): Unit vector pointing from Satellite center + of gravity to Earth's center of gravity. + roatation_axis_unit_vector_body (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. + J2 (float): Earth'sJ2 coefficient [https://ocw.tudelft.nl/wp-content/uploads/AE2104-Orbital-Mechanics-Slides_8.pdf]. + 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]. + + Re = central_body.radius # Earth's radius, [m] + + tg_term_1 = (3 * mu / (r**3)) * np.cross( + satellite_to_earth_unit_vector_body, np.dot(J, satellite_to_earth_unit_vector_body) + ) + tg_term_2 = ( + 30 + * np.dot(satellite_to_earth_unit_vector_body, roatation_axis_unit_vector_body) + * ( + np.cross( + roatation_axis_unit_vector_body, np.dot(J, satellite_to_earth_unit_vector_body) + ) + + np.cross( + satellite_to_earth_unit_vector_body, np.dot(J, roatation_axis_unit_vector_body) + ) + ) + ) + tg_term_3 = np.cross( + ( + 15 + - 105 + * np.dot(satellite_to_earth_unit_vector_body, roatation_axis_unit_vector_body) ** 2 + ) + * satellite_to_earth_unit_vector_body, + np.dot(J, satellite_to_earth_unit_vector_body), + ) + np.cross(6 * roatation_axis_unit_vector_body, np.dot(J, roatation_axis_unit_vector_body)) + tg = tg_term_1 + mu * J2 * Re**2 / (2 * r**5) * (tg_term_2 + tg_term_3) + return np.array(tg) + + +def compute_magnetic_torque(m_earth, m_sat, position, velocity, attitude): + """Calculates the external disturbance torque acting on the actor due to the magnetic field of the earth. + a dipole magnetic field flux density is described by the formula: B = μ0/(4πr³) * [3 r_hat(r_hat ⋅ m) − m]. + With μ0 = 4 π 1e-7 H/m (vacuum permeability), r = actor distance from dipole center, r_hat = unit position vector, + and m the magnetic dipole moment vector of the Earth (magnitude in the year 2000 = 7.79 x 10²² Am²). + + The disturbance torque is then calculated by: T = m_sat x B. + With m_sat the (residual) magnetic dipole moment vector of the actor, magnitude usually between 0.1 - 20 Am² (SMAD). + + https://en.wikipedia.org/wiki/Magnetic_dipole (or Chow, Tai L. (2006). Introduction to electromagnetic theory: + a modern perspective, used formular on p. 149). + + Args: + m_earth (np.ndarray): magnetic dipole moment vector of the Earth in Am². + m_sat (np.ndarray): magnetic dipole moment vector of the actor, magnitude usually between 0.1 - 20 Am². + position (tuple or np.ndarray): actor position. + velocity (tuple or np.ndarray): actor velocity (used for frame transformation). + attitude (np.ndarray): actor velocity (used for frame transformation). + + Returns: Disturbance torque vector T (nd.array) in Nm in the actor body frame. + """ + # convert to np.ndarray. + position = np.array(position) + velocity = np.array(velocity) + + # actor distance. + r = np.linalg.norm(position) + # actor unit position vector. + r_hat = position / r + + # magnetic field flux density at actor's position in Earth inertial frame. + B = 1e-7 * (3 * np.dot(m_earth, r_hat) * r_hat - m_earth) / (r**3) + + # transform field vector to body frame. + B = rpy_to_body(eci_to_rpy(B, position, velocity), attitude) + + # disturbance torque. + T = np.cross(m_sat, B) + return T diff --git a/paseos/central_body/central_body.py b/paseos/central_body/central_body.py index d844525..88883e5 100644 --- a/paseos/central_body/central_body.py +++ b/paseos/central_body/central_body.py @@ -5,6 +5,8 @@ from loguru import logger from pyquaternion import Quaternion from skspatial.objects import Sphere +from skyfield.api import wgs84, load + import pykep as pk import numpy as np @@ -210,3 +212,51 @@ def _apply_rotation(self, point, epoch: pk.epoch): # Rotate point q = Quaternion(axis=self._rotation_axis, angle=angle) return q.rotate(point) + + def magnetic_dipole_moment( + self, + epoch: pk.epoch, + strength_in_Am2=7.79e22, + pole_latitude_in_deg=79.6, + pole_longitude_in_deg=-71.6, + ): + """Returns the time-dependent magnetic dipole moment vector of central body. + Default values are for Earth. Earth dipole moment vector determined from the northern geomagnetic pole position + using skyfield api, and actor epoch. To model the simplified Earth magnetic field as a magnetic dipole with an + offset from the Earth rotational axis, at a specific point in time. + + Earth geomagnetic pole position and dipole moment strength values from the year 2000: + Latitude: 79.6° N + Longitude: 71.6° W + Dipole moment: 7.79 x 10²² Am² + https://wdc.kugi.kyoto-u.ac.jp/poles/polesexp.html + + (The same method used as ground station actor position determination) + + Args: + epoch (pk.epoch): Epoch at which to get the dipole + strength_in_Am2 (float): dipole strength in Am². Defaults to 7.79e22 + pole_latitude_in_deg (float): latitude of the Northern geomagnetic pole in degrees. Defaults to 79.6 + pole_longitude_in_deg (float): longitude of the Northern geomagnetic pole in degrees. Defaults to -71.6 + + Returns: Time-dependent dipole moment vector in inertial frame (np.ndarray): [mx, my, mz] + """ + if self.planet.name == "earth": + # Converting time to skyfield to use its API + t_skyfield = load.timescale().tt_jd(epoch.jd) + + # North geomagnetic pole location on Earth surface in cartesian coordinates + dipole_north_direction = np.array( + wgs84.latlon(pole_latitude_in_deg, pole_longitude_in_deg).at(t_skyfield).position.m + ) + # Multiply geomagnetic pole position unit vector with dipole moment strength + magnetic_dipole_moment = -( + dipole_north_direction / np.linalg.norm(dipole_north_direction) * strength_in_Am2 + ) + else: + # TODO add other planets' magnetic fields + raise NotImplementedError("Magnetic dipole moment only modeled for Earth.") + # For now: assume alignment with rotation axis + magnetic_dipole_moment = np.array([0, 0, 1]) * strength_in_Am2 + + return magnetic_dipole_moment diff --git a/paseos/central_body/mesh_between_points.py b/paseos/central_body/mesh_between_points.py index 157404f..bfd40ab 100644 --- a/paseos/central_body/mesh_between_points.py +++ b/paseos/central_body/mesh_between_points.py @@ -3,7 +3,10 @@ def mesh_between_points( - point_1: np.array, point_2: np.array, mesh_vertices: np.array, mesh_triangles: np.array + point_1: np.array, + point_2: np.array, + mesh_vertices: np.array, + mesh_triangles: np.array, ) -> bool: """Checks whether the mesh is between the two points using ray-triangle intersection with Möller-Trumbore algorithm. @@ -28,7 +31,11 @@ def mesh_between_points( # Iterate over triangles to find any intersection for t in mesh_triangles: intersect, intersect_t = _rays_triangle_intersect( - point_1, direction, mesh_vertices[t[0]], mesh_vertices[t[1]], mesh_vertices[t[2]] + point_1, + direction, + mesh_vertices[t[0]], + mesh_vertices[t[1]], + mesh_vertices[t[2]], ) # If / When found break the loop if intersect: diff --git a/paseos/geometric_model/geometric_model.py b/paseos/geometric_model/geometric_model.py deleted file mode 100644 index 7cd9216..0000000 --- a/paseos/geometric_model/geometric_model.py +++ /dev/null @@ -1,102 +0,0 @@ -from loguru import logger -import trimesh - - -class GeometricModel: - """This model describes the geometry of the spacecraft - Currently it assumes the spacecraft to be a cuboid shape, with width, length and height - """ - - _actor = None - _actor_mesh = None - _actor_center_of_gravity = None - _actor_moment_of_inertia = None - - def __init__(self, local_actor, actor_mass, vertices=None, faces=None, scale=1) -> None: - """Describes the geometry of the spacecraft and outputs relevant parameters related to the spacecraft body. - If no vertices or faces are provided, defaults to a cube with unit length sides. This is in the spacecraft body - reference frame and can be transformed to the inertial/PASEOS reference frame by using the transformations in the - attitude model. - - Args: - local_actor (SpacecraftActor): Actor to model. - actor_mass (float): Actor's mass in kg. - vertices (list): List of all vertices of the mesh in terms of distance (in m) from origin of body frame. - Coordinates of the corners of the object. If not selected, it will default to a cube that can be scaled - by the scale. Uses Trimesh to create the mesh from this and the list of faces. - faces (list): List of the indexes of the vertices of a face. This builds the faces of the satellite by - defining the three vertices to form a triangular face. For a cuboid each face is split into two - triangles. Uses Trimesh to create the mesh from this and the list of vertices. - scale (float): Parameter to scale the cuboid by, defaults to 1. - """ - logger.trace("Initializing cuboid geometrical model.") - - self._actor = local_actor - self._actor_mass = actor_mass - self.vertices = vertices - self.faces = faces - self.scale = scale - - def set_mesh(self): - """Creates the mesh of the satellite. If no vertices input is given, it defaults to a cuboid scaled by the - scale value. The default without scale values is a cube with 1m sides. This uses the python module Trimesh. - - Returns: - mesh: Trimesh mesh of the satellite - """ - if self.vertices is None: - # Defines the corners of the mesh, values are in meters, from the origin of the body frame. - self.vertices = [ - [-0.5, -0.5, -0.5], - [-0.5, -0.5, 0.5], - [-0.5, 0.5, -0.5], - [-0.5, 0.5, 0.5], - [0.5, -0.5, -0.5], - [0.5, -0.5, 0.5], - [0.5, 0.5, -0.5], - [0.5, 0.5, 0.5], - ] - # List of three vertices to form a triangular face of the satellite. - # Two triangular faces are used per side of the cuboid. - self.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], - ] - - mesh = trimesh.Trimesh(self.vertices, self.faces) - # Scales the mesh by the scale factor. - self._actor_mesh = mesh.apply_scale(self.scale) - return self._actor_mesh - - @property - def find_moment_of_inertia(self): - """Gives the moment of inertia of the actor, assuming constant density. - - Returns: - np.array: Mass moments of inertia for the actor - - I is the moment of inertia, in the form of [[Ixx Ixy Ixz] - [Iyx Iyy Iyx] - [Izx Izy Izz]] - """ - self._actor_moment_of_inertia = self._actor_mesh.moment_inertia - return self._actor_moment_of_inertia - - def find_center_of_gravity(self): - """Gives the volumetric center of mass of the actor. - - Returns: - np.array: Coordinates of the center of gravity of the mesh. - """ - self._actor_center_of_gravity = self._actor_mesh.center_mass - return self._actor_center_of_gravity diff --git a/paseos/paseos.py b/paseos/paseos.py index 94661f0..8e6c32c 100644 --- a/paseos/paseos.py +++ b/paseos/paseos.py @@ -159,14 +159,24 @@ def advance_time( # Update actor temperature if self.local_actor.has_thermal_model: + # Get current temperature to update attitude model, if needed. + current_temperature_K = self.local_actor.temperature_in_K + self.local_actor._thermal_model.update_temperature( dt, current_power_consumption_in_W ) + else: + # Setting current temperature to default temperatue to update the attitude model, if needed. + current_temperature_K = self.local_actor.default_temperature_in_K # Update state of charge if self.local_actor.has_power_model: self.local_actor.discharge(current_power_consumption_in_W, dt) + # Update actor attitude + if self.local_actor.has_attitude_model: + self.local_actor._attitude_model.update_attitude(dt, current_temperature_K) + # Update user-defined properties in the actor for property_name in self.local_actor.custom_properties.keys(): update_function = self.local_actor.get_custom_property_update_function( diff --git a/paseos/tests/actor_builder_test.py b/paseos/tests/actor_builder_test.py index 7dafab5..bacf801 100644 --- a/paseos/tests/actor_builder_test.py +++ b/paseos/tests/actor_builder_test.py @@ -95,3 +95,19 @@ def test_add_comm_device(): assert len(sat1.communication_devices) == 2 assert sat1.communication_devices["dev1"].bandwidth_in_kbps == 10 assert sat1.communication_devices["dev2"].bandwidth_in_kbps == 42 + + +def test_set_spacecraft_body_model(): + """Check if we can set the spacecraft body model, and if the moments of inertia are calculated correctly""" + _, sat1, _ = get_default_instance() + ActorBuilder.set_spacecraft_body_model(sat1, mass=100) + + assert sat1.mass == 100 + assert all( + sat1.body_center_of_gravity_body == np.array([0, 0, 0]) + ) # check the default mesh is centered + assert sat1.body_mesh.volume == 1 # check the default volume is correct + assert round(sat1.body_moment_of_inertia_body[0, 0], 4) == 16.6667 # for the default mesh + assert ( + sat1.body_moment_of_inertia_body[0, 1] == 0.0 + ) # Should be zero if the mass distribution is even diff --git a/paseos/tests/attitude_disturbances_test.py b/paseos/tests/attitude_disturbances_test.py new file mode 100644 index 0000000..3b6e27c --- /dev/null +++ b/paseos/tests/attitude_disturbances_test.py @@ -0,0 +1,254 @@ +"""Tests to see whether the attitude 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.utils.reference_frame_transfer import eci_to_rpy, rpy_to_body +from paseos.utils.load_default_cfg import load_default_cfg + + +def test_gravity_disturbance_cube(): + """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_spacecraft_body_model(sat1, mass=100) + ActorBuilder.set_attitude_model(sat1) + ActorBuilder.set_attitude_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) + + # check conditions after 1 orbit + assert np.all(np.round(sat1._attitude_model._actor_angular_acceleration, 10) == 0.0) + + +def test_gravity_disturbance_pole(): + """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_spacecraft_body_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_attitude_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_magnetic_disturbance(): + """Tests the magnetic disturbance torques applied in the attitude model. + Put two spacecraft actors in a geostationary orbit (disregarding the relative magnetic field rotation of the Earth). + Both actor's own magnetic dipole moment aligned with the local magnetic flux density vector of the Earth + magnetic field. One is non-magnetic and is expected to have a fixed attitude in the Earth inertial frame. + The other (magnetic) actor should stay aligned with the Earth magnetic field. + """ + + def flux_density_vector(actor, frame="eci"): + """Function to calculate the local magnetic field flux density vector (B) at the actor's location. + B vector is calculated multiple times. Use of this function for code clarity. + + Args: + actor (SpacecraftActor): Spacecraft actor + frame (string): B expressed in which frame (actor body frame or Earth-centered inertial frame) + + Returns: B vector (np.ndarray) + """ + # get Earth B vector at specific timestep + # Earth magnetic dipole moment: + m_earth = actor.central_body.magnetic_dipole_moment(actor.local_time) + + # parameters to calculate local B vector: + actor_position = np.array(actor.get_position(actor.local_time)) + r = np.linalg.norm(actor_position) + r_hat = actor_position / r + + # local B vector: + B = 1e-7 * (3 * np.dot(m_earth, r_hat) * r_hat - m_earth) / (r**3) + if frame == "eci": + # return B vector in ECI frame + return B + elif frame == "body": + # transform B vector to the actor body frame and return + actor_velocity = np.array(actor.get_position_velocity(actor.local_time)[1]) + actor_attitude = np.array(actor.attitude_in_rad) + return rpy_to_body(eci_to_rpy(B, actor_position, actor_velocity), actor_attitude) + + # Define central body + earth = pk.planet.jpl_lp("earth") + + # Define spacecraft actors + # magnetic: + sat1 = ActorBuilder.get_actor_scaffold("sat1", SpacecraftActor, pk.epoch(0)) + # non-magnetic: + sat2 = ActorBuilder.get_actor_scaffold("sat2", SpacecraftActor, pk.epoch(0)) + + # Geostationary orbital parameters: + r = 6371000 + 35786000 # radius [km] + v = 3074.66 # velocity [m/s] + + # To have a more symmetric case, let the actors be on same longitude as Earth magnetic dipole vector + longitude = -71.6 * np.pi / 180 + + # Set orbits: + ActorBuilder.set_orbit( + sat1, + position=[ + r * np.cos(np.pi / 2 + longitude), + r * np.sin(np.pi / 2 + longitude), + 0, + ], + velocity=[ + -v * np.sin(np.pi / 2 + longitude), + v * np.cos(np.pi / 2 + longitude), + 0, + ], + epoch=pk.epoch(0), + central_body=earth, + ) + ActorBuilder.set_orbit( + sat2, + position=[ + r * np.cos(np.pi / 2 + longitude), + r * np.sin(np.pi / 2 + longitude), + 0, + ], + velocity=[ + -v * np.sin(np.pi / 2 + longitude), + v * np.cos(np.pi / 2 + longitude), + 0, + ], + epoch=pk.epoch(0), + central_body=earth, + ) + + # Set geometric model + ActorBuilder.set_spacecraft_body_model(sat1, mass=100) + ActorBuilder.set_spacecraft_body_model(sat2, mass=100) + + # Now, align the body magnetic dipole with the local Earth magnetic flux density vector + # Earth magnetic flux density vector at start position is approximately: + B0 = np.array([3.18159529e-09, -1.02244882e-07, 3.72362170e-08]) + + B_direction = B0 / np.linalg.norm(B0) + + # Define a very large dipole moment for magnetic actor to compensate for the low magnetic field at GEO orbit + m_body = 500 # Am² + actor_dipole = np.ndarray.tolist(B_direction * m_body) + initial_pointing_vector_body = np.ndarray.tolist(B_direction) + + # Set attitude models + ActorBuilder.set_attitude_model( + sat1, + actor_initial_angular_velocity=[0.0, 0.0, 0.0], + actor_pointing_vector_body=initial_pointing_vector_body, + actor_initial_attitude_in_rad=[0.0, 0.0, 0.0], + actor_residual_magnetic_field_body=actor_dipole, # magnetic + ) + + ActorBuilder.set_attitude_model( + sat2, + actor_initial_angular_velocity=[0.0, 0.0, 0.0], + actor_pointing_vector_body=initial_pointing_vector_body, + actor_initial_attitude_in_rad=[0.0, 0.0, 0.0], + actor_residual_magnetic_field_body=[0.0, 0.0, 0.0], # non-magnetic + ) + + # Disturbances: + ActorBuilder.set_attitude_disturbances(sat1, magnetic=True) + ActorBuilder.set_attitude_disturbances(sat2, magnetic=True) + + # Initial pointing vector in Earth inertial frame + initial_pointing_vector_eci = np.array(sat1.pointing_vector) + # Check if pointing vectors in Earth inertial frame are equal + assert np.all(sat1.pointing_vector == sat2.pointing_vector) + + # Start simulation + sim = paseos.init_sim(sat1) + sim.add_known_actor(sat2) + + # Check if B0, used to define the actors' magnetic field orientations is the initial B vector orientation in sim. + assert np.all(np.isclose(B0, flux_density_vector(sat1, "body"))) + assert np.all(np.isclose(B0, flux_density_vector(sat2, "body"))) + + # The magnetic actor will oscillate slightly, following the Earth's magnetic field lines. (check for multiple steps) + # The actor's magnetic field will not stay perfectly aligned with the Earth's field but needs to stay close. + for i in range(20): + sim.advance_time(200, 0) + + # Get the magnetic flux density vector: + B = flux_density_vector(sat1) + + # B vector direction: + B_direction = B / np.linalg.norm(B) + + # Angle between the B vector and the actor's magnetic dipole vector (which is in the pointing vector direction): + angle = np.arccos(np.dot(B_direction, sat1.pointing_vector)) * 180 / np.pi + + # Check if the magnetic actor dipole moment vector does not deviate more than 1° from the B vector. + assert angle < 1 + + # Check if the non-magnetic actor didn't rotate + assert np.all(sat2.pointing_vector == initial_pointing_vector_eci) diff --git a/paseos/tests/attitude_test.py b/paseos/tests/attitude_test.py new file mode 100644 index 0000000..a6f759c --- /dev/null +++ b/paseos/tests/attitude_test.py @@ -0,0 +1,166 @@ +"""Tests to see whether the attitude model works as intended""" + +import numpy as np +import pykep as pk +import sys + +sys.path.append("../..") +import paseos +from paseos import ActorBuilder, SpacecraftActor + + +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° + in 20 steps advancing time 100 seconds (pi/ (20 * 100)). + Another one has zero initial angular velocity. + This test mainly checks whether the model correctly models constant angular velocity without disturbances + """ + + earth = pk.planet.jpl_lp("earth") + + # First actor constant angular acceleration + omega = np.pi / 2000 + + # Define first local actor with angular velocity + 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_spacecraft_body_model(sat1, mass=100) + ActorBuilder.set_attitude_model( + sat1, + actor_initial_angular_velocity=[0.0, omega, 0.0], + actor_pointing_vector_body=[0, 0, 1], + ) + + # Define second local actor without angular velocity + sat2 = ActorBuilder.get_actor_scaffold("sat2", SpacecraftActor, pk.epoch(0)) + ActorBuilder.set_orbit(sat2, [7000000, 0, 0], [0, 8000.0, 0], pk.epoch(0), earth) + ActorBuilder.set_spacecraft_body_model(sat2, mass=100) + ActorBuilder.set_attitude_model( + sat2, + actor_initial_angular_velocity=[0.0, 0.0, 0.0], + actor_pointing_vector_body=[0, 0, 1], + ) + + # Check Initial values + + # sat1 + assert np.all(sat1._attitude_model._actor_pointing_vector_body == [0.0, 0.0, 1.0]) + assert np.all(sat1._attitude_model._actor_pointing_vector_eci == [-1.0, 0.0, 0.0]) + assert np.all(sat1._attitude_model._actor_attitude_in_rad == [0.0, 0.0, 0.0]) + # positive angular velocity in body y direction is negative angular velocity in Earth inertial z direction: + assert np.all(sat1._attitude_model._actor_angular_velocity_eci == [0.0, 0.0, -omega]) + + # sat2 + assert np.all(sat2._attitude_model._actor_pointing_vector_body == [0.0, 0.0, 1.0]) + assert np.all(sat2._attitude_model._actor_pointing_vector_eci == [-1.0, 0.0, 0.0]) + assert np.all(sat2._attitude_model._actor_attitude_in_rad == [0.0, 0.0, 0.0]) + + # Initialise simulation + sim = paseos.init_sim(sat1) + sim.add_known_actor(sat2) + # Run simulation 20 steps + for i in range(20): + sim.advance_time(100, 0) + + # Testing the simulation went as intended + # Pointing vector from sat1 must be rotated from [-1, 0, 0] to [1, 0, 0]: + assert np.all(np.isclose(sat1.pointing_vector, np.array([1.0, 0.0, 0.0]))) + # Sat1 angular velocity in the body frame must stay constant: + assert np.all( + np.isclose( + sat1._attitude_model._actor_angular_velocity, + np.array([0.0, omega, 0.0]), + ) + ) + # Sat1 angular velocity in the Earth inertial frame must stay constant: + assert np.all( + np.isclose( + sat1.angular_velocity, + np.array([0.0, 0.0, -omega]), + ) + ) + + # Pointing vector from sat2 must not be rotated. + assert np.all(sat2.pointing_vector == np.array([-1.0, 0.0, 0.0])) + # Sat2 angular velocity in the body frame must stay zero: + assert np.all(sat2._attitude_model._actor_angular_velocity == np.array([0.0, 0.0, 0.0])) + + +def attitude_thermal_model_test(): + """Testing the attitude model with no disturbances and no angular velocity, and ensuring the attitude model does not + break the thermal model (or vice versa)""" + 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_spacecraft_body_model(sat1, mass=100) + ActorBuilder.set_thermal_model( + actor=sat1, + actor_mass=sat1.mass, + actor_initial_temperature_in_K=273.15, + actor_sun_absorptance=1.0, + actor_infrared_absorptance=1.0, + actor_sun_facing_area=1.0, + actor_central_body_facing_area=1.0, + actor_emissive_area=1.0, + actor_thermal_capacity=1000, + ) + ActorBuilder.set_attitude_model(sat1, actor_pointing_vector_body=[0, 0, 1]) + + # Check Initial values + assert np.all(sat1._attitude_model._actor_pointing_vector_body == [0.0, 0.0, 1.0]) + assert np.all(sat1._attitude_model._actor_attitude_in_rad == [0.0, 0.0, 0.0]) + assert sat1.temperature_in_K == 273.15 + + # Initialise simulation + sim = paseos.init_sim(sat1) + + # Run simulation 20 steps + for i in range(21): + vector = sat1.pointing_vector + sim.advance_time(100, 0) + + # Testing the simulation went as intended + assert vector[0] == -1.0 + assert sat1._attitude_model._actor_angular_velocity[1] == 0.0 + assert np.round(sat1._attitude_model._actor_attitude_in_rad[0], 3) == 3.142 + assert np.round(sat1.temperature_in_K, 3) == 278.522 + + +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""" + + 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, 5460.0, 0], pk.epoch(0), earth) + ActorBuilder.set_spacecraft_body_model(sat1, mass=100) + orbit_period = 2 * np.pi * np.sqrt((6371000 + 7000000) ** 3 / 3.986004418e14) + ActorBuilder.set_attitude_model( + sat1, + actor_initial_angular_velocity=[0.0, 2 * np.pi / orbit_period, 0.0], + actor_pointing_vector_body=[0, 0, 1], + ) + + # Check Initial values + assert np.all(sat1._attitude_model._actor_pointing_vector_body == [0.0, 0.0, 1.0]) + assert np.all(sat1._attitude_model._actor_attitude_in_rad == [0.0, 0.0, 0.0]) + vector = sat1.pointing_vector + assert vector[0] == -1.0 + + # Initialise simulation + sim = paseos.init_sim(sat1) + + # Run simulation 10 steps + for i in range(11): + vector = sat1.pointing_vector + sim.advance_time(orbit_period / 10, 0) + + # Testing the simulation went as intended + assert sat1._attitude_model._actor_pointing_vector_body[2] == 1.0 + assert vector[0] == -1.0 diff --git a/paseos/tests/communication_window_test.py b/paseos/tests/communication_window_test.py deleted file mode 100644 index 8b1fc52..0000000 --- a/paseos/tests/communication_window_test.py +++ /dev/null @@ -1,176 +0,0 @@ -"""Test to check the communication function(s)""" -from paseos import ( - SpacecraftActor, - GroundstationActor, - ActorBuilder, - find_next_window, - get_communication_window, -) - -import pykep as pk -import numpy as np - - -def setup_sentinel_example(t0): - """Sets up the example with sentinel2B and maspolamas ground station.""" - """Tests the find_next_window function""" - earth = pk.planet.jpl_lp("earth") - - # Define Sentinel 2 orbit - sentinel2B = ActorBuilder.get_actor_scaffold("Sentinel2B", SpacecraftActor, t0) - sentinel2B_line1 = "1 42063U 17013A 22300.18652110 .00000099 00000+0 54271-4 0 9998" - sentinel2B_line2 = "2 42063 98.5693 13.0364 0001083 104.3232 255.8080 14.30819357294601" - s2b = pk.planet.tle(sentinel2B_line1, sentinel2B_line2) - - # Calculating S2B ephemerides. - sentinel2B_eph = s2b.eph(t0) - - ActorBuilder.set_orbit( - actor=sentinel2B, - position=sentinel2B_eph[0], - velocity=sentinel2B_eph[1], - epoch=t0, - central_body=earth, - ) - - # Define ground station - maspalomas_groundstation = ActorBuilder.get_actor_scaffold( - name="maspalomas_groundstation", actor_type=GroundstationActor, epoch=t0 - ) - - ActorBuilder.set_ground_station_location( - maspalomas_groundstation, 27.7629, -15.6338, 205.1, minimum_altitude_angle=5 - ) - - # Add communication link - ActorBuilder.add_comm_device(sentinel2B, device_name="link1", bandwidth_in_kbps=1) - return sentinel2B, maspalomas_groundstation - - -def test_find_next_window(): - # Test window from other test is found - t0 = pk.epoch_from_string("2022-Oct-27 22:57:00") - sentinel2B, maspalomas_groundstation = setup_sentinel_example(t0) - - start, length, transmittable_data = find_next_window( - sentinel2B, - local_actor_communication_link_name="link1", - target_actor=maspalomas_groundstation, - search_window_in_s=360, - t0=t0, - ) - - assert np.isclose(start.mjd2000, 8335.957060185183) - assert np.isclose(length, 740.0000001071021, rtol=0.01, atol=3.0) - assert np.isclose(transmittable_data, 740000, rtol=0.01, atol=3000) - - # Test correct return if no window found - t0 = pk.epoch_from_string("2022-Oct-27 20:00:00") - sentinel2B, maspalomas_groundstation = setup_sentinel_example(t0) - - start, length, transmittable_data = find_next_window( - sentinel2B, - local_actor_communication_link_name="link1", - target_actor=maspalomas_groundstation, - search_window_in_s=360, - t0=t0, - ) - - assert start is None - assert length == 0 - assert transmittable_data == 0 - - -def test_communication_link_sat_to_ground(): - """This test checks if the communication window between Sentinel - and one of it's ground stations matches - """ - t0 = pk.epoch_from_string("2022-Oct-27 22:58:09") - sentinel2B, maspalomas_groundstation = setup_sentinel_example(t0) - - # Check again after communication_window_end_time - ( - communication_window_start_time, - communication_window_end_time, - _, - ) = get_communication_window( - sentinel2B, - local_actor_communication_link_name="link1", - target_actor=maspalomas_groundstation, - dt=1, - t0=t0, - data_to_send_in_b=1000000, - ) - window_in_s = ( - communication_window_end_time.mjd2000 - communication_window_start_time.mjd2000 - ) * pk.DAY2SEC - expected_window_in_s = 739.0000000305008 - assert np.isclose(expected_window_in_s, window_in_s) - - -def test_communication_link_sat_to_sat(): - # create satellites where sat1 and sat2 starts from the same point but move along different orbit. - # At t=1470s they will not be in line of sight anymore. - - earth = pk.planet.jpl_lp("earth") - sat1 = ActorBuilder.get_actor_scaffold("sat1", SpacecraftActor, pk.epoch(0)) - sat2 = ActorBuilder.get_actor_scaffold("sat2", SpacecraftActor, pk.epoch(0)) - - ActorBuilder.set_orbit( - sat1, - position=[10000000, 1e-3, 1e-3], - velocity=[1e-3, 8000, 1e-3], - epoch=pk.epoch(0), - central_body=earth, - ) - ActorBuilder.set_orbit( - sat2, - position=[10000000, 1e-3, 1e-3], - velocity=[1e-3, -8000, 1e-3], - epoch=pk.epoch(0), - central_body=earth, - ) - - # Add communication link - ActorBuilder.add_comm_device(sat1, device_name="link1", bandwidth_in_kbps=1) - - # Check communication link at == 0. Satellites shall be in line of sight - ( - _, - communication_window_end_time, - transmitted_data_in_b, - ) = get_communication_window( - sat1, - local_actor_communication_link_name="link1", - target_actor=sat2, - dt=10, - t0=pk.epoch(0), - data_to_send_in_b=10000, - ) - - assert (communication_window_end_time.mjd2000 * pk.DAY2SEC >= 10) and ( - transmitted_data_in_b == 10000 - ) - - # Check again after communication_window_end_time - ( - communication_window_start_time, - communication_window_end_time, - transmitted_data_in_b, - ) = get_communication_window( - sat1, - local_actor_communication_link_name="link1", - target_actor=sat2, - dt=10, - t0=communication_window_end_time, - data_to_send_in_b=10000, - ) - - assert (transmitted_data_in_b == 0) and np.isclose( - communication_window_end_time.mjd2000, communication_window_start_time.mjd2000 - ) - - -if __name__ == "__main__": - # test_communication_link_sat_to_sat() - test_communication_link_sat_to_ground() diff --git a/paseos/tests/compare_communication_windows.py b/paseos/tests/compare_communication_windows.py new file mode 100644 index 0000000..a47a79d --- /dev/null +++ b/paseos/tests/compare_communication_windows.py @@ -0,0 +1,113 @@ +import pykep as pk +import sys + +sys.path.append("../..") +import paseos +from paseos.utils.load_default_cfg import load_default_cfg + +from paseos import ( + SpacecraftActor, + GroundstationActor, + ActorBuilder, + find_next_window, +) + +import numpy as np + + +def setup_sentinel_example(t0): + """Sets up the example with sentinel2B and maspolamas ground station.""" + """Tests the find_next_window function""" + earth = pk.planet.jpl_lp("earth") + + # Define Sentinel 2 orbit + sentinel2B = ActorBuilder.get_actor_scaffold("Sentinel2B", SpacecraftActor, t0) + sentinel2B_line1 = "1 40697U 15028A 23193.19358829 .00000126 00000-0 64636-4 0 9993" + sentinel2B_line2 = "2 40697 98.5693 267.3615 0001325 90.2705 269.8630 14.30818726420585" + s2b = pk.planet.tle(sentinel2B_line1, sentinel2B_line2) + + # Calculating S2B ephemerides. + sentinel2B_eph = s2b.eph(t0) + + ActorBuilder.set_orbit( + actor=sentinel2B, + position=sentinel2B_eph[0], + velocity=sentinel2B_eph[1], + epoch=t0, + central_body=earth, + ) + + # sentinel2B.set_central_body_shape(Sphere([0, 0, 0], _PASEOS_TESTS_EARTH_RADIUS)) + + # Define ground station + maspalomas_groundstation = ActorBuilder.get_actor_scaffold( + name="maspalomas_groundstation", actor_type=GroundstationActor, epoch=t0 + ) + + ActorBuilder.set_ground_station_location( + maspalomas_groundstation, 27.7629, -15.6338, 205.1, minimum_altitude_angle=5 + ) + + # Add communication link + ActorBuilder.add_comm_device(sentinel2B, device_name="link1", bandwidth_in_kbps=1) + return sentinel2B, maspalomas_groundstation + + +def main(): + t0 = pk.epoch_from_string("2019-02-25 08:40:17.000") + sentinel2B, maspalomas_groundstation = setup_sentinel_example(t0) + cfg = load_default_cfg() # loading cfg to modify defaults + cfg.sim.start_time = t0.mjd2000 * pk.DAY2SEC # convert epoch to seconds + sim = paseos.init_sim(sentinel2B, cfg) + + lengths = [] + starts = [] + n = 4 + while n: + start, length, _ = find_next_window( + sentinel2B, + local_actor_communication_link_name="link1", + target_actor=maspalomas_groundstation, + search_window_in_s=3600, + t0=sentinel2B.local_time, + ) + if start is not None: + if len(starts) != 0: + if not (start in starts): + lengths.append(length) + starts.append(start) + print("n: ", n) + n -= 1 + n_sec_advance = ( + start.mjd2000 * pk.DAY2SEC + - sentinel2B.local_time.mjd2000 * pk.DAY2SEC + + length + ) + else: + n_sec_advance = 500 + else: + lengths.append(length) + starts.append(start) + print("n: ", n) + n -= 1 + n_sec_advance = ( + start.mjd2000 * pk.DAY2SEC - sentinel2B.local_time.mjd2000 * pk.DAY2SEC + length + ) + else: + n_sec_advance = 500 + + sim.advance_time(n_sec_advance, 0) + print(sentinel2B.local_time) + + print("Lengths: ", lengths) + print("Starts: ", starts) + lengths_expected = np.array([31032 - 30426, 37026 - 36321, 75297 - 74811, 81396 - 80661]) + errors = np.abs(lengths_expected - np.array(lengths)) + errors_percentage = errors / np.array(lengths) + print("Error percentages: ", errors_percentage) + print("Mean percentage error: ", np.mean(errors_percentage)) + + +if __name__ == "__main__": + print("START") + main() diff --git a/paseos/tests/custom_property_test.py b/paseos/tests/custom_property_test.py index 839ee31..144cdf3 100644 --- a/paseos/tests/custom_property_test.py +++ b/paseos/tests/custom_property_test.py @@ -26,7 +26,10 @@ def prop_update_fn(actor, dt, power_consumption): return actor.get_custom_property(prop_name) + power_consumption * dt ActorBuilder.add_custom_property( - actor=sat1, property_name=prop_name, update_function=prop_update_fn, initial_value=0 + actor=sat1, + property_name=prop_name, + update_function=prop_update_fn, + initial_value=0, ) print(f"Actor custom properties are now {sat1.custom_properties}") diff --git a/paseos/tests/mesh_test.py b/paseos/tests/mesh_test.py index 974b395..b729153 100644 --- a/paseos/tests/mesh_test.py +++ b/paseos/tests/mesh_test.py @@ -127,7 +127,10 @@ def test_mesh_eclipse(): # Add a power device ActorBuilder.set_power_devices( - actor=sat1, battery_level_in_Ws=500, max_battery_level_in_Ws=1000, charging_rate_in_W=10 + actor=sat1, + battery_level_in_Ws=500, + max_battery_level_in_Ws=1000, + charging_rate_in_W=10, ) assert not sat1.is_in_eclipse(epoch) diff --git a/paseos/utils/reference_frame_transfer.py b/paseos/utils/reference_frame_transfer.py new file mode 100644 index 0000000..c7ae04e --- /dev/null +++ b/paseos/utils/reference_frame_transfer.py @@ -0,0 +1,306 @@ +"""This file contains functions to transform vectors between three reference frames: + +- Earth-centered inertial frame (ECI) + +- Roll-Pitch-Yaw frame (RPY): x: along track direction + y: -ĥ (angular momentum of the orbit). perpendicular to the orbital plane + z: -r̂ (nadir) + +- Body fixed frame: when body is unperturbed, frame coincides with RPY frame. Perturbations result in non-zero roll + pitch and yaw angles, rotating the body fixed frame w.r.t the RPY frame. + +It also provides functions for rotating vectors around another vector and determining roll, pitch and yaw angles between +two reference frames. + +Note: the order of rotation yaw -> pitch -> roll is used for transforming from "space frame (RPY here)" to body frame +""" + +import numpy as np + + +def compute_transformation_matrix_eci_rpy(r, v): + """ + Creates the transformation matrix to transform a vector in the Earth-Centered Inertial Frame (ECI) to the + Roll-Pitch-Yaw (RPY) reference frame of the spacecraft (variant to Gaussian reference frame, useful for attitude + disturbance modelling). + + To go from ECI to RPY, this transformation matrix is used. + To go from RPY to ECI, the inverse is used. + + Args: + r (np.ndarray): position vector of RPY reference frame wrt ECI frame + v (np.ndarray): velocity of the spacecraft in earth reference frame, centered on spacecraft + Returns: + T (np.ndarray): transformation matrix + """ + + # y' coincides with -ĥ vector which is perpendicular to the orbital plane. Thus, perpendicular to v and p vectors + # determine y' base by use of the cross product: (V x r)/||(V x r)|| + cross_vr = np.cross(v, r) + y = cross_vr / np.linalg.norm(cross_vr) + + # z' coincides with the nadir pointing vector + # determine z' base by use of the position vector of the RPY frame + z = -r / np.linalg.norm(r) + + # x' completes the right-handed frame + # determine x' base by use of the cross product: (y' x z')/||(y' x z')|| + cross_yz = np.cross(y, z) + x = cross_yz / np.linalg.norm(cross_yz) + + # Form transformation matrix + T = np.array([x, y, z]) + + return T + + +def compute_transformation_matrix_rpy_body(euler_angles_in_rad): + """Creates the transformation matrix to transform a vector in the Roll-Pitch-Yaw (RPY) reference frame to the body. + fixed reference frame of the spacecraft. + + To go from RPY to body fixed, this transformation matrix is used. + To go from body fixed to RPY, the inverse is used. + + Args: + euler_angles_in_rad (np.ndarray): [roll, pitch, yaw] in radians. + + Returns: + T (np.ndarray): transformation matrix. + """ + roll, pitch, yaw = euler_angles_in_rad + + # individual axis rotations: + A = np.array([[1, 0, 0], [0, np.cos(roll), np.sin(roll)], [0, -np.sin(roll), np.cos(roll)]]) + + B = np.array( + [ + [np.cos(pitch), 0, -np.sin(pitch)], + [0, 1, 0], + [np.sin(pitch), 0, np.cos(pitch)], + ] + ) + + C = np.array([[np.cos(yaw), np.sin(yaw), 0], [-np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) + + # Transformation matrix: + T = A @ B @ C + + return T + + +def eci_to_rpy(u, r, v, translation=False): + """Converts a vector in the Earth-Centered Inertial Frame (ECI) to the Roll-Pitch-Yaw (RPY) reference frame of the + spacecraft, using transformation matrix from transformation_matrix_eci_rpy function. + + Args: + u (np.ndarray): vector in ECI + r (np.ndarray): position vector of RPY reference frame wrt ECI frame + v (np.ndarray): velocity of the spacecraft in earth reference frame, centered on spacecraft + translation (bool): does the vector need to be translated? (default=False) + + Returns: + vector u w.r.t. RPY frame + """ + + T = compute_transformation_matrix_eci_rpy(r, v) + + if translation: + shift = r + else: + shift = 0 + + # transform u vector with matrix multiplication + return T @ u - shift + + +def rpy_to_eci(u, r, v, translation=False): + """Converts a vector in the Roll-Pitch-Yaw (RPY) of the spacecraft to the Earth-Centered Inertial Frame (ECI) + reference frame, using the inverse transformation matrix from transformation_matrix_eci_rpy function. + + Args: + u (np.ndarray): vector in RPY + r (np.ndarray): position vector of RPY reference frame wrt ECI frame + v (np.ndarray): velocity of the spacecraft in earth reference frame, centered on spacecraft + translation (bool): does the vector need to be translated? (default=False) + + Returns: + vector u w.r.t. ECI frame + """ + + T = np.linalg.inv(compute_transformation_matrix_eci_rpy(r, v)) + if translation: + shift = r + else: + shift = 0 + # transform u vector with matrix multiplication + return T @ u + shift + + +def rpy_to_body(u, euler_angles_in_rad): + """Converts a vector in the Roll-Pitch-Yaw (RPY) reference frame to the body fixed reference frame of the + spacecraft, using transformation matrix from transformation_matrix_rpy_body function. + + Args: + u (np.ndarray): vector in RPY + euler_angles_in_rad (np.ndarray): [roll, pitch, yaw] in radians + + Returns: + vector u w.r.t. the body fixed frame + """ + # for undisturbed calculations: zero euler angles result in no transformation + # numpy default absolute tolerance: 1e-0.8 + if all(np.isclose(euler_angles_in_rad, np.zeros(3))): + return u + else: + T = compute_transformation_matrix_rpy_body(euler_angles_in_rad) + return T @ u + + +def body_to_rpy(u, euler_angles_in_rad): + """Converts a vector in the body fixed reference frame to the Roll-Pitch-Yaw (RPY) reference frame of the + spacecraft, using the inverse transformation matrix from transformation_matrix_rpy_body function. + + Args: + u (np.ndarray): vector in the body fixed frame + euler_angles_in_rad (np.ndarray): [roll, pitch, yaw] in radians + + Returns: + vector u w.r.t. the RPY frame + """ + # for undisturbed calculations: zero euler angles result in no transformation + # numpy default absolute tolerance: 1e-0.8 + if all(np.isclose(euler_angles_in_rad, np.zeros(3))): + return u + else: + T = np.linalg.inv(compute_transformation_matrix_rpy_body(euler_angles_in_rad)) + return T @ u + + +def rodrigues_rotation(p, angles): + """Rotates vector p around the rotation vector v in the same reference frame. Using Rodrigues' rotation formula. + + Args: + p (np.ndarray): vector to be rotated [x, y, z] + angles (np.ndarray): vector with decomposed rotation angles [theta_x, theta_y, theta_z] + + Returns: + new vector p rotated with given angles + """ + # scalar rotation: + theta = np.linalg.norm(angles) + + # no rotation: + if theta == 0.0: + return p + # non-zero rotation: + else: + # unit rotation vector + k = angles / theta + + # Rodrigues' formula: + p_rotated = (p * np.cos(theta) + (np.cross(k, p)) * np.sin(theta)) + k * ( + np.linalg.multi_dot([k, p]) + ) * (1 - np.cos(theta)) + return p_rotated + + +def get_rpy_angles(x, y, z, vectors_in_rpy_frame=True): + """Returns Roll, Pitch, Yaw angles of rotated frame wrt fixed frame. + example: input body frame primary axes expressed wrt rpy frame, returns roll, pitch, yaw of body + + This function assumes the following transformation matrix: where c = cos, s = sin, and the angles being: + roll (r), pitch (p) and yaw (y) + + | R00 R01 R02 | | cp cy cp sy -sp | ^ (-1) + R = | R10 R11 R12 | = | sr sp cy - cr sy sr sp sy + cr cy sr cp | + | R20 R21 R22 | | cr sp cy + sr sy cr sp sy - sr cy sr cp | + + This is the transformation matrix from RPY to body frame, when the body frame is expressed in RPY, to find the + roll, pitch, yaw angles, this matrix needs to be inverted. + + Args: + x (np.ndarray): new orientation of [1,0,0] x-axis expressed in fixed reference frame + y (np.ndarray): new orientation of [0,1,0] y-axis expressed in fixed reference frame + z (np.ndarray): new orientation of [0,0,1] z-axis expressed in fixed reference frame + vectors_in_rpy_frame (bool): are the input vectors expressed in the rpy frame? (default: True) + + Returns: + roll, pitch, yaw angles + """ + # transformation matrix: + R = np.c_[x, y, z] + + if vectors_in_rpy_frame: + # different transformation matrix + R = np.linalg.inv(R) + + # when pitch = +- 90 degrees(R_03 = +-1), yaw and roll have the same effect. Choose roll to be zero + # (avoid dividing by zero) + if R[0][2] == -1.0: + pitch = np.pi / 2 + roll = 0.0 + yaw = -np.arctan2(R[1][0], R[2][0]) + # or yaw = -np.arctan2( - R[2][1], R[1][1]) + elif R[0][2] == 1.0: + pitch = -np.pi / 2 + roll = 0.0 + yaw = np.arctan2(-R[1][0], R[2][0]) + # or yaw = np.arctan2( - R[2][1], - R[1][1]) + else: + # problem: two rpy sets possible as pitch = atan2( ..., +- sqrt(..)). Positive x component of atan2 is chosen + pitch = np.arctan2(-R[0][2], np.sqrt((R[1][2]) ** 2 + (R[2][2]) ** 2)) + roll = np.arctan2(R[1][2] / np.cos(pitch), R[2][2] / np.cos(pitch)) + yaw = np.arctan2(R[0][1] / np.cos(pitch), R[0][0] / np.cos(pitch)) + + return np.array([roll, pitch, yaw]) + + +def rotate_body_vectors(x, y, z, p, angle): + """Used for rotating multiple vectors about one common rotation vector. In this case rotating the primary x-, y-, z- + axes and the actor's pointing vector. + + Args: + x (np.ndarray): x-axis + y (np.ndarray): y-axis + z (np.ndarray): z-axis + p (np.ndarray): pointing vector + angle (np.ndarray): rotation vector with magnitude being the rotation angle + + Returns: + rotated x-, y-, z- axes and pointing vector + """ + x = rodrigues_rotation(x, angle) + y = rodrigues_rotation(y, angle) + z = rodrigues_rotation(z, angle) + p = rodrigues_rotation(p, angle) + + return x, y, z, p + + +def frame_rotation(position, next_position, velocity): + """Calculate the rotation vector of the RPY frame rotation within the inertial frame. + This rotation component makes the actor body attitude stay constant w.r.t. inertial frame. + + Args: + position (np.ndarray): actor position vector. + next_position (np.ndarray): actor position vector in the next timestep. + velocity (np.ndarray): actor velocity vector. + + Returns: rotation vector of RPY frame w.r.t. ECI frame expressed in the ECI frame. + """ + # orbital plane normal unit vector: (p x v)/||p x v|| + orbital_plane_normal = np.cross(position, velocity) / np.linalg.norm( + np.cross(position, velocity) + ) + + # rotation angle: arccos((p . p_previous) / (||p|| ||p_previous||)) + rpy_frame_rotation_angle_in_eci = np.arccos( + np.linalg.multi_dot([position, next_position]) + / (np.linalg.norm(position) * np.linalg.norm(next_position)) + ) + + # assign this scalar rotation angle to the vector perpendicular to rotation plane + rpy_frame_rotation_vector_in_eci = orbital_plane_normal * rpy_frame_rotation_angle_in_eci + + # this rotation needs to be compensated in the rotation of the body frame, so its attitude stays fixed + return -eci_to_rpy(rpy_frame_rotation_vector_in_eci, position, velocity) diff --git a/paseos/visualization/space_animation.py b/paseos/visualization/space_animation.py index 013bcad..ea6e3a2 100644 --- a/paseos/visualization/space_animation.py +++ b/paseos/visualization/space_animation.py @@ -135,7 +135,13 @@ def _plot_comm_lines(self): self.comm_lines.append( self.ax_3d.plot3D( - x1x2, y1y2, z1z2, "--", color="green", linewidth=0.5, zorder=10 + x1x2, + y1y2, + z1z2, + "--", + color="green", + linewidth=0.5, + zorder=10, ) )