diff --git a/.pylintrc b/.pylintrc index 15c897e99..752149a6c 100644 --- a/.pylintrc +++ b/.pylintrc @@ -437,6 +437,7 @@ disable=raw-checker-failed, no-else-return, inconsistent-return-statements, unspecified-encoding, + no-member, # because we use funcify_method decorator # Enable the message, report, category or checker with the given id(s). You can # either give multiple identifier separated by comma (,) or put this option diff --git a/CHANGELOG.md b/CHANGELOG.md index 6bcdd8904..c6e3c1fda 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -41,6 +41,7 @@ straightforward as possible. - ENH: Function Reverse Arithmetic Priority [#488](https://github.com/RocketPy-Team/RocketPy/pull/488) - DOC: Update Header Related Docs - ENH Precalculate Barometric Height [#511](https://github.com/RocketPy-Team/RocketPy/pull/511) +- MNT: Encapsulate quaternion conversions [#537](https://github.com/RocketPy-Team/RocketPy/pull/537) - MNT: improve the low pass filter and document an example [#538](https://github.com/RocketPy-Team/RocketPy/pull/538) diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index 3ffb0eb68..39b0c5c7e 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -11,7 +11,12 @@ from ..mathutils.vector_matrix import Matrix, Vector from ..plots.flight_plots import _FlightPlots from ..prints.flight_prints import _FlightPrints -from ..tools import find_closest +from ..tools import ( + find_closest, + quaternions_to_spin, + quaternions_to_precession, + quaternions_to_nutation, +) class Flight: @@ -2289,33 +2294,24 @@ def lateral_attitude_angle(self): @funcify_method("Time (s)", "Precession Angle - ψ (°)", "spline", "constant") def psi(self): """Precession angle as a Function of time.""" - psi = (180 / np.pi) * ( - np.arctan2(self.e3[:, 1], self.e0[:, 1]) - + np.arctan2(-self.e2[:, 1], -self.e1[:, 1]) - ) # Precession angle - psi = np.column_stack([self.time, psi]) # Precession angle - return psi + psi = quaternions_to_precession( + self.e0.y_array, self.e1.y_array, self.e2.y_array, self.e3.y_array + ) + return np.column_stack([self.time, psi]) @funcify_method("Time (s)", "Spin Angle - φ (°)", "spline", "constant") def phi(self): """Spin angle as a Function of time.""" - phi = (180 / np.pi) * ( - np.arctan2(self.e3[:, 1], self.e0[:, 1]) - - np.arctan2(-self.e2[:, 1], -self.e1[:, 1]) - ) # Spin angle - phi = np.column_stack([self.time, phi]) # Spin angle - return phi + phi = quaternions_to_spin( + self.e0.y_array, self.e1.y_array, self.e2.y_array, self.e3.y_array + ) + return np.column_stack([self.time, phi]) @funcify_method("Time (s)", "Nutation Angle - θ (°)", "spline", "constant") def theta(self): """Nutation angle as a Function of time.""" - theta = ( - (180 / np.pi) - * 2 - * np.arcsin(-((self.e1[:, 1] ** 2 + self.e2[:, 1] ** 2) ** 0.5)) - ) # Nutation angle - theta = np.column_stack([self.time, theta]) # Nutation angle - return theta + theta = quaternions_to_nutation(self.e1.y_array, self.e2.y_array) + return np.column_stack([self.time, theta]) # Fluid Mechanics variables # Freestream Velocity diff --git a/rocketpy/tools.py b/rocketpy/tools.py index 6e765ca7a..bcff91fb8 100644 --- a/rocketpy/tools.py +++ b/rocketpy/tools.py @@ -3,6 +3,7 @@ import re from bisect import bisect_left +import numpy as np import pytz from cftime import num2pydate from packaging import version as packaging_version @@ -381,6 +382,69 @@ def check_requirement_version(module_name, version): return True +# Flight + + +def quaternions_to_precession(e0, e1, e2, e3): + """Calculates the Precession angle + + Parameters + ---------- + e0 : float + Euler parameter 0, must be between -1 and 1 + e1 : float + Euler parameter 1, must be between -1 and 1 + e2 : float + Euler parameter 2, must be between -1 and 1 + e3 : float + Euler parameter 3, must be between -1 and 1 + Returns + ------- + float + Euler Precession angle in degrees + """ + return (180 / np.pi) * (np.arctan2(e3, e0) + np.arctan2(-e2, -e1)) + + +def quaternions_to_spin(e0, e1, e2, e3): + """Calculates the Spin angle from quaternions. + + Parameters + ---------- + e0 : float + Euler parameter 0, must be between -1 and 1 + e1 : float + Euler parameter 1, must be between -1 and 1 + e2 : float + Euler parameter 2, must be between -1 and 1 + e3 : float + Euler parameter 3, must be between -1 and 1 + + Returns + ------- + float + Euler Spin angle in degrees + """ + return (180 / np.pi) * (np.arctan2(e3, e0) - np.arctan2(-e2, -e1)) + + +def quaternions_to_nutation(e1, e2): + """Calculates the Nutation angle from quaternions. + + Parameters + ---------- + e1 : float + Euler parameter 1, must be between -1 and 1 + e2 : float + Euler parameter 2, must be between -1 and 1 + Returns + ------- + float + Euler Nutation angle in degrees + """ + return (180 / np.pi) * 2 * np.arcsin(-((e1**2 + e2**2) ** 0.5)) + + if __name__ == "__main__": import doctest