diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..13566b8
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/.idea/PASEOS.iml b/.idea/PASEOS.iml
new file mode 100644
index 0000000..8a05c6e
--- /dev/null
+++ b/.idea/PASEOS.iml
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml
new file mode 100644
index 0000000..7341d3e
--- /dev/null
+++ b/.idea/inspectionProfiles/Project_Default.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000..105ce2d
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..d5642e8
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000..5d5a3da
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..35eb1dd
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/paseos/actors/actor_builder.py b/paseos/actors/actor_builder.py
index 17d022a..350ca88 100644
--- a/paseos/actors/actor_builder.py
+++ b/paseos/actors/actor_builder.py
@@ -13,6 +13,7 @@
from ..thermal.thermal_model import ThermalModel
from ..power.power_device_type import PowerDeviceType
from ..radiation.radiation_model import RadiationModel
+from ..attitude.attitude_model import AttitudeModel
from paseos.geometric_model.geometric_model import GeometricModel
@@ -519,6 +520,40 @@ def set_thermal_model(
power_consumption_to_heat_ratio=power_consumption_to_heat_ratio,
)
+ @staticmethod
+ def set_disturbances(
+ actor: SpacecraftActor,
+ aerodynamic: bool = False,
+ gravitational: bool = False,
+ magnetic: bool = False,
+ ):
+ disturbance_list = []
+ if aerodynamic:
+ disturbance_list.append("aerodynamic")
+ if gravitational:
+ disturbance_list.append("gravitational")
+ if magnetic:
+ disturbance_list.append("magnetic")
+ actor._disturbances = disturbance_list
+
+ @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._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,
+
+
+ )
+
@staticmethod
def add_comm_device(actor: BaseActor, device_name: str, bandwidth_in_kbps: float):
"""Creates a communication device.
diff --git a/paseos/actors/base_actor.py b/paseos/actors/base_actor.py
index bae022c..f016dcd 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
+ # Attitude disturbances experienced by the actor
+ _disturbances = None
+
# The following variables are used to track last evaluated state vectors to avoid recomputation.
_previous_position = None
_time_of_previous_position = None
@@ -157,6 +160,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.
@@ -328,6 +340,14 @@ def get_position_velocity(self, epoch: pk.epoch):
self._time_of_previous_position = epoch.mjd2000
return pos, vel
+ def get_disturbances(self):
+ """Get the user specified spacecraft attitude disturbances
+
+ Returns:
+ list[string]: name of disturbances
+ """
+ return self._disturbances
+
def is_in_line_of_sight(
self,
other_actor: "BaseActor",
diff --git a/paseos/actors/spacecraft_actor.py b/paseos/actors/spacecraft_actor.py
index 30f7527..a2c6040 100644
--- a/paseos/actors/spacecraft_actor.py
+++ b/paseos/actors/spacecraft_actor.py
@@ -1,3 +1,4 @@
+import numpy as np
from loguru import logger
import pykep as pk
@@ -21,6 +22,7 @@ class SpacecraftActor(BaseActor):
_thermal_model = None
_radiation_model = None
+ _attitude_model = None
# If radiation randomly restarted the device
_was_interrupted = False
@@ -164,3 +166,34 @@ 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}")
+
+ 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) == np.ndarray:
+ return np.ndarray.tolist(self._attitude_model._actor_attitude_in_rad)
+ else:
+ return self._attitude_model._actor_attitude_in_rad
+
+ 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) == 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)
+
+
+ def pointing_vector(self):
+ """Returns the spacecraft pointing vector
+
+ Returns:
+ numpy ndarray (x, y, z)
+ """
+ return self._attitude_model._actor_pointing_vector_eci
\ No newline at end of file
diff --git a/paseos/attitude/attitude_model.py b/paseos/attitude/attitude_model.py
new file mode 100644
index 0000000..0b94d9a
--- /dev/null
+++ b/paseos/attitude/attitude_model.py
@@ -0,0 +1,229 @@
+import numpy as np
+
+from paseos.attitude.disturbance_calculations import (
+ calculate_aero_torque,
+ calculate_magnetic_torque,
+ calculate_grav_torque,
+)
+from paseos.utils.reference_frame_transfer import (
+ eci_to_rpy,
+ rpy_to_eci,
+ body_to_rpy,
+ rodrigues_rotation,
+ get_rpy_angles,
+ rotate_body_vectors,
+)
+
+
+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.
+ """
+
+ _actor = None
+
+ _actor_attitude_in_rad = None
+ _actor_angular_velocity = None
+ _actor_angular_acceleration = None
+
+ _actor_pointing_vector_body = None
+ _actor_pointing_vector_eci = None
+
+ # _actor_prev_pos = None
+ def __init__(
+ self,
+ local_actor,
+ # initial conditions: (defaults to 0)
+ 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 z-axis)
+ actor_pointing_vector_body: list[float] = [0.0, 0.0, 1.0]
+ ## add args with default value = ...
+ # actor_dipole
+ # actor_drag_coefficient (more for geometric model)
+ # body_J2
+ ):
+ self._actor = local_actor
+ 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]),
+ )
+
+ self._first_run = True
+
+ def nadir_vector(self):
+ # unused but might be useful during disturbance calculations or pointing vector relative position
+ """compute unit vector pointing towards earth, inertial body 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))
+ return -u / np.linalg.norm(u)
+
+ def calculate_disturbance_torque(self):
+ """Compute total torque due to user specified disturbances
+
+ Returns:
+ np.array([Tx, Ty, Tz]): total combined torques in Nm expressed in the spacecraft body frame
+ """
+
+ T = np.array([0.0, 0.0, 0.0])
+ if "aerodynamic" in self._actor.get_disturbances():
+ T += calculate_aero_torque()
+ if "gravitational" in self._actor.get_disturbances():
+ T += calculate_grav_torque()
+ if "magnetic" in self._actor.get_disturbances():
+ T += calculate_magnetic_torque()
+ return T
+
+ def calculate_angular_acceleration(self):
+ """Calculate the spacecraft angular acceleration (external disturbance torques and gyroscopic accelerations)"""
+ # TODO implement geometric model
+ # I = self._actor_moment_of_inertia
+ # TODO in the future control torques could be added
+ I = np.array([[50, 0, 0], [0, 50, 0], [0, 0, 50]]) # test placeholder
+
+ # Euler's equation for rigid body rotation: a = I^(-1) (T - w x (Iw))
+ # a = angular acceleration, I = inertia matrix, T = torque vector, w = angular velocity
+ self._actor_angular_acceleration = np.linalg.inv(I) @ (
+ self.calculate_disturbance_torque()
+ - np.cross(self._actor_angular_velocity, I @ self._actor_angular_velocity)
+ )
+
+ def body_rotation(self, dt):
+ """Calculates the rotation vector around which the spacecraft body rotates
+ based on angular acceleration and velocity
+
+ Args:
+ dt (float): time to advance
+
+ Returns: rotation vector of spacecraft body expressed in the body frame
+ """
+ # todo: check if need to skip first step
+ # theta_2:
+
+ # to not have the spacecraft rotate in the first timestep:
+ # if self._first_run:
+ # body_rotation = self._actor_angular_velocity * dt
+ # self._actor_theta_2 = body_to_rpy(body_rotation, self._actor_attitude_in_rad)
+ # self._first_run = False
+ self.calculate_angular_acceleration()
+ self._actor_angular_velocity += self._actor_angular_acceleration * dt
+ body_rotation = self._actor_angular_velocity * dt
+ return body_to_rpy(body_rotation, self._actor_attitude_in_rad)
+
+ def frame_rotation(self, position, previous_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
+ previous_position (np.ndarray): actor position vector in previous 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, previous_position])
+ / (np.linalg.norm(position) * np.linalg.norm(previous_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 it's attitude stays fixed
+ return -eci_to_rpy(rpy_frame_rotation_vector_in_eci, position, velocity)
+
+ 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):
+ """Updates the actor attitude based on initial conditions and disturbance torques over time
+
+ Args:
+ dt (float): How far to advance the attitude computation.
+ """
+ # position
+ position = np.array(self._actor.get_position(self._actor.local_time))
+
+ # previous position
+ previous_position = self._actor._previous_position
+
+ # velocity
+ velocity = np.array(
+ self._actor.get_position_velocity(self._actor.local_time)[1]
+ )
+
+ # body axes expressed in rpy frame: (x, y, z, custom pointing vector)
+ xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy = self.body_axes_in_rpy()
+
+ # todo: check if possible to do both angles at once
+
+ # attitude change due to two rotations
+ # rpy frame rotation, in inertial frame:
+ theta_1 = self.frame_rotation(position, previous_position, velocity)
+ # body rotation due to its physical rotation
+ theta_2 = self.body_rotation(dt)
+
+ # 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
+ )
+
+ """
+ xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy = rotate_body_vectors(
+ xb_rpy, yb_rpy, zb_rpy, pointing_vector_rpy, theta_1 + theta_2
+ )
+ """
+
+ # update new attitude:
+ self._actor_attitude_in_rad = get_rpy_angles(xb_rpy, yb_rpy, zb_rpy)
+
+ # update pointing vector
+ self._actor_pointing_vector_eci = rpy_to_eci(
+ pointing_vector_rpy, position, velocity
+ )
diff --git a/paseos/attitude/disturbance_calculations.py b/paseos/attitude/disturbance_calculations.py
new file mode 100644
index 0000000..43f6fe2
--- /dev/null
+++ b/paseos/attitude/disturbance_calculations.py
@@ -0,0 +1,26 @@
+"""this file contains functions that return attitude disturbance torque vectors expressed in the actor body frame"""
+
+# OUTPUT NUMPY ARRAYS
+# OUTPUT IN BODY FRAME
+import numpy as np
+
+
+def calculate_aero_torque():
+ # calculations for torques
+ # T must be in actor body fixed frame (to be discussed)
+ T = [0, 0, 0]
+ return np.array(T)
+
+
+def calculate_grav_torque():
+ # calculations for torques
+ # T must be in actor body fixed frame (to be discussed)
+ T = [0, 0, 0]
+ return np.array(T)
+
+
+def calculate_magnetic_torque():
+ # calculations for torques
+ # T must be in actor body fixed frame (to be discussed)
+ T = [0, 0, 0]
+ return np.array(T)
diff --git a/paseos/paseos.py b/paseos/paseos.py
index 94661f0..a6c6a33 100644
--- a/paseos/paseos.py
+++ b/paseos/paseos.py
@@ -167,6 +167,10 @@ def advance_time(
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)
+
# 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/utils/reference_frame_transfer.py b/paseos/utils/reference_frame_transfer.py
new file mode 100644
index 0000000..4522672
--- /dev/null
+++ b/paseos/utils/reference_frame_transfer.py
@@ -0,0 +1,288 @@
+"""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 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 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 = 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(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 = 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(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
diff --git a/test_attitude_code/test_attitude_plotting.py b/test_attitude_code/test_attitude_plotting.py
new file mode 100644
index 0000000..36ab6a5
--- /dev/null
+++ b/test_attitude_code/test_attitude_plotting.py
@@ -0,0 +1,107 @@
+# We use pykep for orbit determination
+import pykep as pk
+import matplotlib
+import matplotlib.pyplot as plt
+import numpy as np
+
+import paseos
+from paseos.actors.spacecraft_actor import SpacecraftActor
+from paseos.actors.actor_builder import ActorBuilder
+
+matplotlib.use("Qt5Agg")
+
+# Define central body
+earth = pk.planet.jpl_lp("earth")
+
+sat1 = ActorBuilder.get_actor_scaffold("sat1", SpacecraftActor, pk.epoch(0))
+
+
+ActorBuilder.set_orbit(
+ sat1,
+ position=[10000000, 0, 0],
+ velocity=[0, 8000, 0],
+ epoch=pk.epoch(0),
+ central_body=earth,
+)
+"""
+ActorBuilder.set_orbit(
+ sat1,
+ position=[10000000, 3000000, 0],
+ velocity=[0, 8000, 2000],
+ epoch=pk.epoch(0),
+ central_body=earth,
+)
+"""
+
+ActorBuilder.set_thermal_model(
+ sat1,
+ actor_mass=100,
+ actor_initial_temperature_in_K=270,
+ actor_sun_absorptance=0.5,
+ actor_infrared_absorptance=0.5,
+ actor_sun_facing_area=1,
+ actor_central_body_facing_area=4,
+ actor_emissive_area=18,
+ actor_thermal_capacity=0.89,
+)
+# following used to check if model is correct. Pure rotations around pointig vector should not move the pointing vector.
+pure_axis_rotation = [[0.00158, 0.0, 0.0], [0.0, 0.00158, 0.0], [0.0, 0.0, 0.00158]]
+axes_list = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
+axis = 1 # x:0, y:1, z:2
+# when i = 21 in loop, 0.00158 rad/sec will rotate 180 deg about 1 axis
+ActorBuilder.set_attitude_model(
+ sat1,
+ #actor_initial_angular_velocity=pure_axis_rotation[1],
+ #actor_pointing_vector_body=axes_list[0],
+ actor_initial_angular_velocity=[0.0, 0.0, 0.0],
+ actor_pointing_vector_body=[0.0, 0.0, 1.0],
+ actor_initial_attitude_in_rad=[0.0, 0.0, 0.0]
+)
+ActorBuilder.set_disturbances(sat1,True, True)
+
+sim = paseos.init_sim(sat1)
+plt.close()
+# Plot current status of PASEOS and get a plotter
+#%%
+# Run some operations and inbetween update PASEOS
+pos = []
+x = []
+y = []
+z = []
+pointing_vector = []
+ratio = 100/440
+
+
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+for i in range(41):
+ pos = (sat1.get_position(sat1.local_time))
+ x.append(sat1.get_position(sat1.local_time)[0])
+ y.append(sat1.get_position(sat1.local_time)[1])
+ z.append(sat1.get_position(sat1.local_time)[2])
+
+ euler = sat1.attitude_in_rad()
+
+ vector = sat1.pointing_vector()
+ vector[np.isclose(vector, np.zeros(3))] = 0
+ #print(vector, "test test test test test")
+ #print(i, sat1.attitude_in_deg())
+ print(i, vector, sat1.attitude_in_deg())
+ vector = vector * 2e6
+
+ ax.quiver(pos[0], pos[1], pos[2], vector[0], vector[1], vector[2])
+ sim.advance_time(100, 0)
+
+axmin = min([min(x), min(y), min(z)])*1.1
+axmax = max([max(x), max(y), max(z)])*1.1
+
+ax.axes.set_xlim3d(left=axmin, right=axmax)
+ax.axes.set_ylim3d(bottom=axmin, top=axmax)
+ax.axes.set_zlim3d(bottom=axmin, top=axmax)
+
+ax.set_xlabel("x")
+ax.set_ylabel("y")
+ax.set_zlabel("z")
+
+ax.plot(x,y,z)
+ax.scatter(0,0,0)