Repository: aidotse/PASEOS (language: python)

Issue:
Aerodynamic disturbances in Attitude Model
# Feature
Adding the aerodynamic disturbance torque to the attitude model. Part 3.1 of #190 

## Desired Behavior / Functionality
The modelled spacecraft should have a torque acting on it due to the aerodynamic forces (lift and drag) resulting from its interaction with the higher atmosphere. The model to describe such forces is the one used in "Roto-Translational Spacecraft Formation Control Using Aerodynamic Forces", Ran S. *et al.* , JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS, 2017. 
The function output shall be the aerodynamic torque vector calculated from the aerodynamic forces on each face of the satellite. 

## What Needs to Be Done
A location is already determined in the attitude model where this implementation should go. Hence, the aerodynamic disturbance model needs to be implemented as a function, and then the correct actor/simulation parameters should be implemented.

## How Can It Be Tested
By creating a pytest test. It is possible to implement tests that verify that the aerodynamic torque values correspond to torques experienced by previously flown satellites in the same orbit. With the attitude model, it is possible to verify (in simple cases) that the aerodynamic-induced model attitude changes align with the expected change in attitude. 



Discussion / hints:

Fix patch:
diff --git a/paseos/attitude/disturbance_calculations.py b/paseos/attitude/disturbance_calculations.py
index 76d964b..9a8093c 100644
--- a/paseos/attitude/disturbance_calculations.py
+++ b/paseos/attitude/disturbance_calculations.py
@@ -1,84 +1,193 @@
-"""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
-from ..utils.reference_frame_transfer import rpy_to_body, eci_to_rpy
-
-
-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(central_body, u_r, u_n, J, r):
-    """
-    Equation for gravity gradient torque with up to J2 effect from:
-    https://doi.org/10.1016/j.asr.2018.06.025, chapter 3.3
-    This function currently only works for Earth centered orbits
-
-    Args:
-        u_r (np.array): Unit vector pointing from Satellite center of gravity to Earth's center of gravity
-        u_n (np.array): Unit vector along the Earth's rotation axis, in the spacecraft body frame
-        J (np.array): The satellites moment of inertia, in the form of [[Ixx Ixy Ixz]
-                                                                        [Iyx Iyy Iyx]
-                                                                        [Izx Izy Izz]]
-        r (float): The distance from the center of the Earth to the satellite
-
-    Returns:
-        np.array: total gravitational torques in Nm expressed in the spacecraft body frame
-    """
-    # Constants
-    mu = central_body.mu_self  # Earth's gravitational parameter, [m^3/s^2]
-    J2 = 1.0826267e-3  # Earth's J2 coefficient, from https://ocw.tudelft.nl/wp-content/uploads/AE2104-Orbital-Mechanics-Slides_8.pdf
-    Re = central_body.radius  # Earth's radius, [m]
-
-
-    tg_term_1 = (3 * mu / (r ** 3))*np.cross(u_r, np.dot(J,u_r))
-    tg_term_2 = 30 * np.dot(u_r, u_n)*(np.cross(u_n, np.dot(J,u_r)) + np.cross(u_r, np.dot(J,u_n)))
-    tg_term_3 = np.cross((15 - 105 * np.dot(u_r, u_n) ** 2) * u_r, np.dot(J,u_r)) + np.cross(6 * u_n, np.dot(J ,u_n))
-    tg = tg_term_1 + mu * J2 * Re ** 2 / (2 * r ** 5) * (tg_term_2 + tg_term_3)
-    return np.array(tg)
-
-
-def calculate_magnetic_torque(m_earth, m_sat, position, velocity, attitude):
-    """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
+from ..utils.reference_frame_transfer import transformation_matrix_eci_rpy, transformation_matrix_rpy_body, rpy_to_body, eci_to_rpy
+import numpy as np
+import trimesh
+
+def calculate_aero_torque(r,v,mesh, transformation_matrix_rpy_body):
+    """ 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:
+        r (np.array): distance from the satellite and the Earth's center of mass [km]
+        v (np.array): velocity of the satellite in ECI reference frame [m/s]
+        mesh (trimesh): mesh of the satellite from the geometric model
+        transformation_matrix_rpy_body (np.array): transformation matrix from Roll-Pitch_Yaw frame to the Spacecraft
+            Body frame. From reference_frame_transfer.py .
+
+     Returns:
+         T (np.array): torque vector in the spacecraft body frame
+    """
+
+    #  Constants for aerodynamic coefficients calculation
+    temperature_spacecraft = 300  # [K]
+    temperature_gas = 1000  # [K]
+    R = 8.314462  # Universal Gas Constant, [J/(K mol)]
+    altitude = np.linalg.norm(r)-6371  # [km]
+    density = 10**(-(altitude+1285)/151)  # equation describing the best linear fit for the data, [kg/m^3]
+    molecular_speed_ratio_t = np.linalg.norm(v)/np.sqrt(2*R*temperature_gas)  # Used in the Cd and Cl calculation
+    molecular_speed_ratio_r = np.linalg.norm(v)/np.sqrt(2*R*temperature_spacecraft)  # Used in the Cd and Cl calculation
+    accommodation_parameter = 0.85
+
+    #  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]
+    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(v, r, v)
+    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(12):
+        if np.dot(face_normals_rpy[i], v_rpy) > 0:
+            normals_faces_with_airflow[j] = face_normals_rpy[i]
+            area_faces_airflow[j] = area_all_faces_mesh[i]  # get the area of the plate [i] which is receiving airflow
+            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
+            centroids_faces_airflow[j] = np.mean(face_vertices_rpy, axis=0)  # get the centroids of the face[i] with airflow
+            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 = 1/molecular_speed_ratio_t**2 * (molecular_speed_ratio_t / np.sqrt(np.pi) * \
+            (4 * np.sin(alpha_scalar)**2 + 2 * accommodation_parameter * np.cos(2*alpha_scalar)) * \
+            np.exp(-(molecular_speed_ratio_t*np.sin(alpha_scalar))**2) + np.sin(alpha_scalar) * \
+            (1 + 2 * molecular_speed_ratio_t ** 2 + (1 - accommodation_parameter) * \
+            (1 - 2 * molecular_speed_ratio_t ** 2 * np.cos(2 * alpha_scalar))) * \
+            np.math.erf(molecular_speed_ratio_t * np.sin(alpha_scalar)) + accommodation_parameter * np.sqrt(np.pi)* \
+            molecular_speed_ratio_t ** 2 / molecular_speed_ratio_r * np.sin(alpha_scalar) ** 2)
+
+        C_l = np.cos(alpha[k]) / molecular_speed_ratio_t**2 * (2 / np.sqrt(np.pi) * (2-2*accommodation_parameter) * \
+            molecular_speed_ratio_t*np.sin(alpha_scalar)*np.exp(-(molecular_speed_ratio_t*np.sin(alpha_scalar))**2)+\
+            (2*(2-2*accommodation_parameter)*(molecular_speed_ratio_t*np.sin(alpha_scalar))**2 + \
+            (2-accommodation_parameter)) * np.math.erf(molecular_speed_ratio_t*np.sin(alpha_scalar)) + \
+            accommodation_parameter*np.sqrt(np.pi) * molecular_speed_ratio_t**2 / molecular_speed_ratio_r * \
+            np.sin(alpha_scalar))
+
+        # 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(v)**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(v)**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
+    T[np.isnan(T)] = 0  # substitutes 0 to any NaN value
+
+    return np.array(T)
+
+def calculate_grav_torque(central_body, u_r, u_n, J, r):
+    """
+    Equation for gravity gradient torque with up to J2 effect from:
+    https://doi.org/10.1016/j.asr.2018.06.025, chapter 3.3
+    This function currently only works for Earth centered orbits
+
+    Args:
+        u_r (np.array): Unit vector pointing from Satellite center of gravity to Earth's center of gravity
+        u_n (np.array): Unit vector along the Earth's rotation axis, in the spacecraft body frame
+        J (np.array): The satellites moment of inertia, in the form of [[Ixx Ixy Ixz]
+                                                                        [Iyx Iyy Iyx]
+                                                                        [Izx Izy Izz]]
+        r (float): The distance from the center of the Earth to the satellite
+
+    Returns:
+        np.array: total gravitational torques in Nm expressed in the spacecraft body frame
+    """
+    # Constants
+    mu = central_body.mu_self  # Earth's gravitational parameter, [m^3/s^2]
+    J2 = 1.0826267e-3  # Earth's J2 coefficient, from https://ocw.tudelft.nl/wp-content/uploads/AE2104-Orbital-Mechanics-Slides_8.pdf
+    Re = central_body.radius  # Earth's radius, [m]
+
+
+    tg_term_1 = (3 * mu / (r ** 3))*np.cross(u_r, np.dot(J,u_r))
+    tg_term_2 = 30 * np.dot(u_r, u_n)*(np.cross(u_n, np.dot(J,u_r)) + np.cross(u_r, np.dot(J,u_n)))
+    tg_term_3 = np.cross((15 - 105 * np.dot(u_r, u_n) ** 2) * u_r, np.dot(J,u_r)) + np.cross(6 * u_n, np.dot(J ,u_n))
+    tg = tg_term_1 + mu * J2 * Re ** 2 / (2 * r ** 5) * (tg_term_2 + tg_term_3)
+    return np.array(tg)
+
+
+def calculate_magnetic_torque(m_earth, m_sat, position, velocity, attitude):
+    """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
+


Test patch:
diff --git a/paseos/attitude/aero_disturbance_test.py b/paseos/attitude/aero_disturbance_test.py
new file mode 100644
index 0000000..5eae11d
--- /dev/null
+++ b/paseos/attitude/aero_disturbance_test.py
@@ -0,0 +1,112 @@
+from paseos.attitude.disturbance_calculations import calculate_aero_torque
+from paseos.utils.reference_frame_transfer import transformation_matrix_rpy_body
+import trimesh
+import numpy as np
+
+
+#  create mesh
+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],
+]
+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(vertices, faces)
+
+
+def test_1():
+    """ First test uses existing spacecraft data to check that with the same initial conditions the torque exerted on the
+        satellite is comparable to the real GOCE data. Goce has a cross-section of 1.1 m^2 and during it's drag free
+        phase experienced between 1 and 20 mN of drag force. It is reasonable to expect a torque in the same order of
+        magnitude for a satellite of 1x1x1 m^2 at the same altitude as GOCE.
+    """
+    # GOCE orbital altitude and speed
+    r = np.array([6626, 0, 0])  # [km]
+    v = np.array([0, 7756, 0])  # [m/s]
+
+    transformation_matrix = transformation_matrix_rpy_body(np.array([np.pi/4, np.pi/6, 0]))
+
+    T = calculate_aero_torque(r, v, mesh, transformation_matrix)
+    abs_T = np.linalg.norm(T)
+
+    assert 5*10**-4 < abs_T < 2*10**-2
+
+def test_2a():
+    """ Second tests aim to model simple cases and verify that the torque exerted matches the expected torque calculated
+        by the function, with particular emphasis on the torque vector direction.
+
+        Base case: RPY frame is aligned with Body Frame. There should be no torque.
+    """
+    # Circular orbit at GOCE altitude
+    r = np.array([6626, 0, 0])  # [km]
+    v = np.array([0, 7756, 0])  # [km]
+
+    transformation_matrix = transformation_matrix_rpy_body(np.array([0, 0, 0]))  # can change the euler angles if willing
+    T = calculate_aero_torque(r, v, mesh, transformation_matrix)
+
+    assert np.linalg.norm(T) < 10**(-8)
+
+def test_2b():
+    """ Second tests aim to model simple cases and verify that the torque exerted matches the expected torque calculated
+        by the function, with particular emphasis on the torque vector direction.
+
+        Second case, satellite with an attitude that yields a torque vector parallel to [0, -1, 0]
+    """
+    # Circular orbit at GOCE altitude
+    r = np.array([6626, 0, 0])  # [km]
+    v = np.array([0, 7756, 0])  # [km]
+
+    transformation_matrix = transformation_matrix_rpy_body(np.array([0, np.pi/6, 0]))
+    T = calculate_aero_torque(r, v, mesh, transformation_matrix)
+    similarity = np.dot(T,[0, 1, 0])
+
+    assert similarity == np.linalg.norm(T)
+
+def test_2c():
+    """ Second tests aim to model simple cases and verify that the torque exerted matches the expected torque calculated
+        by the function, with particular emphasis on the torque vector direction.
+
+        Third case, satellite with an attitude that yields zero torque
+    """
+    # Circular orbit at GOCE altitude
+    r = np.array([6626, 0, 0])  # [km]
+    v = np.array([0, 7756, 0])  # [km]
+
+    transformation_matrix = transformation_matrix_rpy_body(np.array([np.pi/6, 0, 0]))
+    T = calculate_aero_torque(r, v, mesh, transformation_matrix)
+
+    assert all(T) == 0
+
+def test_2d():
+    """ Second tests aim to model simple cases and verify that the torque exerted matches the expected torque calculated
+        by the function, with particular emphasis on the torque vector direction.
+
+        Third case, positive rotation of 30° about the z-axis, expected torque in the positive z-axis direction.
+    """
+    # Circular orbit at GOCE altitude
+    r = np.array([6626, 0, 0])  # [km]
+    v = np.array([0, 7756, 0])  # [km]
+
+    transformation_matrix = transformation_matrix_rpy_body(np.array([0, 0, np.pi/6]))
+    T = calculate_aero_torque(r, v, mesh, transformation_matrix)
+    similarity = np.dot(T, [0, 0, 1])
+
+    assert similarity == np.linalg.norm(T)