diff --git a/CHANGELOG.md b/CHANGELOG.md index 13c598421..ac21f0d71 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -52,6 +52,7 @@ Attention: The newest changes should be on top --> ### Changed +- BUG: Negate `com_to_cdm_function` and `nozzle_to_cdm` where `u_dot_generalized` reads them. Both are defined pointing at the center of dry mass and the derivation measures from it, so the moment arm they formed had the wrong sign. Every result out of the generalized equations of motion changes, so a study built on the old numbers will not reproduce. On the measured 2024 Defiance flight the apogee error against 9308.32 m AGL goes from 0.6928 % to 0.6483 %. [#1186](https://github.com/RocketPy-Team/RocketPy/issues/1186) - ENH: Number the simulations of a serial Monte Carlo run from zero, as the parallel path already did — the two used to name the same simulation `1, 2, 3` and `0, 1, 2`. The `index` field written into the inputs and outputs rows shifts by one for serial runs, so code reading indices off those files has to expect a run of `n` simulations to be numbered `0` to `n - 1`. An append onto a study written before this release is refused rather than continued, since its rows do not record which root drew them. [#1187](https://github.com/RocketPy-Team/RocketPy/pull/1187) - MNT: Store the reference-area correction factor on each rocket aero surface component, so it is carried with the surface instead of being recomputed at every lift evaluation. `Rocket.aerodynamic_surfaces`, `rail_buttons` and `sensors` now yield `(component, position, ref_factor)`, so code that unpacks a pair from them (`for surface, position in rocket.aerodynamic_surfaces`) has to take the third field or absorb it. Simulation results are unchanged, and `.rpy` files written before this still load. [#1129](https://github.com/RocketPy-Team/RocketPy/pull/1129) [#561](https://github.com/RocketPy-Team/RocketPy/issues/561) - ENH: Compute the rocket static margin lazily [#1135](https://github.com/RocketPy-Team/RocketPy/pull/1135) [#780](https://github.com/RocketPy-Team/RocketPy/issues/780) diff --git a/docs/technical/equations_of_motion_v1.rst b/docs/technical/equations_of_motion_v1.rst index b9d811d54..d7f1a4600 100644 --- a/docs/technical/equations_of_motion_v1.rst +++ b/docs/technical/equations_of_motion_v1.rst @@ -11,6 +11,13 @@ algebraic steps used to get to the final form of the equations of motion used in the code. For a more detailed explanation of the equations of motion, please refer to :ref:`Equations of Motion v0 `. +Throughout this page the reference point is the center of dry mass (CDM), +:math:`\mathbf{r}_{\mathrm{CM}}` is the vector from the CDM to the +instantaneous center of mass and :math:`\mathbf{r}_{\mathrm{noz}}` is the +vector from the CDM to the nozzle exit. Note that the ``Rocket`` attributes +``com_to_cdm_function`` and ``nozzle_to_cdm`` are the opposite vectors, so +the implementation negates them before use. + Development ----------- @@ -34,7 +41,7 @@ Development \begin{aligned} m \mathbf{r}_{\mathrm{CM}} \times \dot{\mathbf{v}}+m \mathbf{r}_{\mathrm{CM}} \times\left(\dot{\boldsymbol{\omega}} \times \mathbf{r}_{\mathrm{CM}}\right)+m \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times\left(\boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}\right)+m \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime \prime}+2 m \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime} \\ - \quad=\mathbf{r}_{\mathrm{CM}} \times \mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+m \mathbf{r}_{\mathrm{CM}}^{\prime} \times\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right) \\ + \quad=\mathbf{r}_{\mathrm{CM}} \times \mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\ddot{m} \mathbf{r}_{\mathrm{CM}} \times\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right) \\ \quad+\mathbf{r}_{\mathrm{CM}} \times \mathbf{A}+\mathbf{r}_{\mathrm{CM}} \times \sum_i \mathbf{N}_i-m \mathbf{r}_{\mathrm{CM}} \times g \hat{\mathbf{a}}_3 \end{aligned} @@ -81,7 +88,7 @@ Development .. math:: \begin{gathered} - \dot{\mathbf{v}}=\frac{\left(\mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\ddot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right) \mathbf{A}+\sum_i \mathbf{N}_i\right)}{m}-g \hat{a}_3-\dot{\boldsymbol{\omega}} \times \mathbf{r}_{\mathrm{CM}} \\ + \dot{\mathbf{v}}=\frac{\left(\mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\ddot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\mathbf{A}+\sum_i \mathbf{N}_i\right)}{m}-g \hat{a}_3-\dot{\boldsymbol{\omega}} \times \mathbf{r}_{\mathrm{CM}} \\ -\boldsymbol{\omega} \times\left(\boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}\right)-\mathbf{r}_{\mathrm{CM}}^{\prime \prime}-2 \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime} \end{gathered} @@ -137,8 +144,8 @@ Development 1. :math:`m`: mass 2. :math:`\mathrm{r}_{CM}`: position vector of the center of mass -3. :math:`\mathbf{T}_{03}`: :math:`2\dot{m} \left( r_{noz} - r_{CM} \right) - 2 \cdot m \cdot r_{CM}` -4. :math:`\mathbf{T}_{04}`: :math:`T - m \cdot r_{CM}' - 2 \cdot 𝑚̇ \cdot r_{CM} + 𝑚̈ \cdot (r_{noz} - r_{CM})` +3. :math:`\mathbf{T}_{03}`: :math:`2\dot{m} \left( r_{noz} - r_{CM} \right) - 2 \cdot m \cdot r_{CM}'` +4. :math:`\mathbf{T}_{04}`: :math:`T - m \cdot r_{CM}'' - 2 \cdot 𝑚̇ \cdot r_{CM}' + 𝑚̈ \cdot (r_{noz} - r_{CM})` 5. :math:`\mathbf{T}_{05}`: :math:`\dot{m} \cdot S_{noz} - I'` 6. :math:`g`: gravity acceleration 7. :math:`\mathbf{I}`: inertia tensor @@ -148,11 +155,11 @@ Pre-computed terms 1. :math:`\mathbf{T}_{00}`: :math:`m \cdot \mathrm{r}_{\mathrm{CM}}` 2. :math:`\mathbf{T}_{01}`: :math:`[m \cdot \mathrm{r}_{\mathrm{CM}}] \times` 3. :math:`\mathbf{T}_{02}`: :math:`[m \cdot \mathrm{r}_{\mathrm{CM}}] \times \mathbf{T}'` -4. :math:`\mathbf{T}_{03}`: :math:`2\cdot \dot{m} (\mathrm{r}_{noz} - \mathrm{r}_{\mathrm{CM}}) - 2 \cdot m \mathrm{r}_{\mathrm{CM}}` -5. :math:`\mathbf{T}_{04}`: :math:`\mathbf{T} - m \cdot \mathrm{r}_{\mathrm{CM}}'' - 2 \cdot \dot{m} \cdot \mathrm{r}_{\mathrm{CM}} + \ddot{m} (\mathrm{r}_{noz} - \mathrm{r}_{\mathrm{CM}})` +4. :math:`\mathbf{T}_{03}`: :math:`2\cdot \dot{m} (\mathrm{r}_{noz} - \mathrm{r}_{\mathrm{CM}}) - 2 \cdot m \mathrm{r}_{\mathrm{CM}}'` +5. :math:`\mathbf{T}_{04}`: :math:`\mathbf{T} - m \cdot \mathrm{r}_{\mathrm{CM}}'' - 2 \cdot \dot{m} \cdot \mathrm{r}_{\mathrm{CM}}' + \ddot{m} (\mathrm{r}_{noz} - \mathrm{r}_{\mathrm{CM}})` 6. :math:`\mathbf{T}_{05}`: :math:`\dot{m} \cdot S_{noz} - \mathbf{I}'` 7. :math:`\mathbf{T}_{20}`: :math:`-\omega \times (\omega \times \mathbf{T}_{00}) + \omega \times (\mathbf{T}_{03}) + \mathbf{T}_{04} - m \cdot g \hat{a}_3 + \mathbf{A} + \sum \mathbf{N}_{i}` -8. :math:`\mathbf{T}_{21}`: :math:`-\omega \times (\mathbf{I} \cdot \omega) + (T_{05}) \cdot \omega + \mathrm{r}_{\mathrm{CM}} \times m \cdot g \hat{a}_3 + \sum r_{i} \times \mathbf{N}_{i}` +8. :math:`\mathbf{T}_{21}`: :math:`-\omega \times (\mathbf{I} \cdot \omega) + (T_{05}) \cdot \omega - \mathrm{r}_{\mathrm{CM}} \times m \cdot g \hat{a}_3 + \sum r_{i} \times \mathbf{N}_{i}` **Final system of equations** @@ -204,8 +211,8 @@ Pre-computed terms \mathbf{H}=-m\left[\begin{array}{ccc} -r_{\mathrm{CM}_3}^2-r_{\mathrm{CM}_2}^2 & r_{\mathrm{CM}_2} r_{\mathrm{CM}_1} & r_{\mathrm{CM}_3} r_{\mathrm{CM}_1} \\ - r_{\mathrm{CM}_2} r_{\mathrm{CM}_1} & -r_{\mathrm{CM}_3}^2-r_{\mathrm{CM}_1^2} & r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} \\ - r_{\mathrm{CM}_3} r_{\mathrm{CM}_1} & r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} & -r_{\mathrm{CM}_2}-r_{\mathrm{CM}_1}^2 + r_{\mathrm{CM}_2} r_{\mathrm{CM}_1} & -r_{\mathrm{CM}_3}^2-r_{\mathrm{CM}_1}^2 & r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} \\ + r_{\mathrm{CM}_3} r_{\mathrm{CM}_1} & r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} & -r_{\mathrm{CM}_2}^2-r_{\mathrm{CM}_1}^2 \end{array}\right] .. math:: diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index 43790110c..a92401586 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -2769,13 +2769,13 @@ def u_dot_generalized(self, t, u, post_processing=False): # pylint: disable=too total_mass_dot = self.rocket.total_mass_flow_rate.get_value_opt(t) total_mass_ddot = self.rocket.total_mass_flow_rate.differentiate_complex_step(t) ## CM position vector and time derivatives relative to CDM in body frame - r_CM_z = self.rocket.com_to_cdm_function - r_CM_t = r_CM_z.get_value_opt(t) - r_CM = Vector([0, 0, r_CM_t]) - r_CM_dot = Vector([0, 0, r_CM_z.differentiate_complex_step(t)]) - r_CM_ddot = Vector([0, 0, r_CM_z.differentiate(t, order=2)]) - ## Nozzle position vector - r_NOZ = Vector([0, 0, self.rocket.nozzle_to_cdm]) + ## com_to_cdm_function runs CM to CDM, so it is negated here + com_to_cdm = self.rocket.com_to_cdm_function + r_CM = Vector([0, 0, -com_to_cdm.get_value_opt(t)]) + r_CM_dot = Vector([0, 0, -com_to_cdm.differentiate_complex_step(t)]) + r_CM_ddot = Vector([0, 0, -com_to_cdm.differentiate(t, order=2)]) + ## Nozzle position vector, likewise negated + r_NOZ = Vector([0, 0, -self.rocket.nozzle_to_cdm]) ## Nozzle gyration tensor S_nozzle = self.rocket.nozzle_gyration_tensor ## Inertia tensor @@ -2961,7 +2961,7 @@ def u_dot_generalized(self, t, u, post_processing=False): # pylint: disable=too # Velocity vector derivative + Coriolis acceleration w_earth = Vector(self.env.earth_rotation_vector) - v_dot = K @ (T20 / total_mass - (r_CM ^ w_dot)) - 2 * (w_earth ^ v) + v_dot = K @ (T20 / total_mass + (r_CM ^ w_dot)) - 2 * (w_earth ^ v) # Position vector derivative r_dot = [vx, vy, vz] diff --git a/tests/acceptance/test_defiance_rocket.py b/tests/acceptance/test_defiance_rocket.py index 47d96359b..1afe5143d 100644 --- a/tests/acceptance/test_defiance_rocket.py +++ b/tests/acceptance/test_defiance_rocket.py @@ -11,8 +11,8 @@ MAX_RELATIVE_APOGEE_ERROR = 0.01 REFERENCE_MAX_SPEED = 444.24 REFERENCE_MAX_ACCELERATION = 10400.76 -REFERENCE_IMPACT_X = 1609.40 -REFERENCE_IMPACT_Y = 87.03 +REFERENCE_IMPACT_X = 1550.23 +REFERENCE_IMPACT_Y = 104.80 REFERENCE_METRIC_RELATIVE_TOLERANCE = 0.01 REFERENCE_IMPACT_ABSOLUTE_TOLERANCE = 3.0 diff --git a/tests/integration/simulation/test_flight.py b/tests/integration/simulation/test_flight.py index a3d4bb9c2..1e05601dd 100644 --- a/tests/integration/simulation/test_flight.py +++ b/tests/integration/simulation/test_flight.py @@ -466,7 +466,7 @@ def test_freestream_speed_at_apogee(example_plain_env, calisto): npt.assert_allclose( test_flight.stream_velocity_x(test_flight.apogee_time), - 0.4641507314747016, + 0.4639602017451516, atol=hard_atol, rtol=soft_rtol, ) @@ -485,13 +485,13 @@ def test_freestream_speed_at_apogee(example_plain_env, calisto): ) npt.assert_allclose( test_flight.free_stream_speed(test_flight.apogee_time), - 0.46415073147558955, + 0.46396020175352876, atol=hard_atol, rtol=soft_rtol, ) npt.assert_allclose( test_flight.apogee_freestream_speed, - 0.46415073147558955, + 0.46396020175352876, atol=hard_atol, rtol=soft_rtol, ) diff --git a/tests/unit/simulation/test_flight.py b/tests/unit/simulation/test_flight.py index 64082f7a4..a99a05bd0 100644 --- a/tests/unit/simulation/test_flight.py +++ b/tests/unit/simulation/test_flight.py @@ -352,7 +352,7 @@ def test_export_sensor_data(flight_calisto_with_sensors): [ ("t_initial", (0.25886, -0.649623, 0)), ("out_of_rail_time", (0.792028, -1.987634, 0)), - ("apogee_time", (-0.519917, -0.734918, -1.005368e-18)), + ("apogee_time", (-0.652606, -0.734029, -2.115675e-14)), ("t_final", (0, 0, 0)), ], ) @@ -391,7 +391,7 @@ def test_aerodynamic_moments(flight_calisto_custom_wind, flight_time, expected_v [ ("t_initial", (1.654150, 0.659142, -0.067103)), ("out_of_rail_time", (5.052628, 2.013361, -1.75370)), - ("apogee_time", (2.322999, -1.643037, -0.950316)), + ("apogee_time", (2.266124, -2.014696, -0.818709)), ("t_final", (-0.019802, 0.012030, 159.051604)), ], ) @@ -432,7 +432,7 @@ def test_aerodynamic_forces(flight_calisto_custom_wind, flight_time, expected_va ("out_of_rail_time", (0, 2.248540, 25.700928)), ( "apogee_time", - (-14.593411, 15.743567, -0.000409), + (-11.634095, 16.695945, -0.000173), ), ("t_final", (5, 2, -5.660155)), ], @@ -471,7 +471,7 @@ def test_velocities(flight_calisto_custom_wind, flight_time, expected_values): [ ("t_initial", (0, 0, 0)), ("out_of_rail_time", (0, 7.8067, 89.2315)), - ("apogee_time", (0.07649, -0.053530, -9.620037)), + ("apogee_time", (0.073233, -0.062635, -9.614299)), ("t_final", (0, 0, 0.0019548)), ], ) diff --git a/tests/unit/simulation/test_generalized_eom.py b/tests/unit/simulation/test_generalized_eom.py new file mode 100644 index 000000000..4981c5fe9 --- /dev/null +++ b/tests/unit/simulation/test_generalized_eom.py @@ -0,0 +1,264 @@ +"""Equation-level checks for Flight.u_dot_generalized. + +These drive the real right-hand side with a stubbed rigid body so that every +term except the one under test is exactly zero, which makes the expected +answer a closed-form Newton-Euler result rather than a recorded trajectory. +""" + +import numpy as np +import pytest + +from rocketpy.mathutils.vector_matrix import Matrix, Vector +from rocketpy.rocket.rocket import Rocket +from rocketpy.simulation.flight import Flight + +MASS = 40.0 +COM_TO_CDM = 0.30 +NOZZLE_TO_CDM = 1.25 +INERTIA = [[60.0, 0.0, 0.0], [0.0, 60.0, 0.0], [0.0, 0.0, 2.0]] + + +class _Scalar: + """Constant Function stand-in with a settable first derivative.""" + + def __init__(self, value, derivative=0.0): + self.value = float(value) + self.derivative = float(derivative) + + def get_value_opt(self, _t): + return self.value + + def differentiate_complex_step(self, _t): + return self.derivative + + def differentiate(self, _t, order=1): + return self.derivative if order == 1 else 0.0 + + +class _Surface: + """Aerodynamic surface returning a prescribed force and moment.""" + + reference_length = 1.0 + + def __init__(self, forces_and_moments): + self.forces_and_moments = forces_and_moments + + def compute_forces_and_moments(self, *_args, **_kwargs): + return self.forces_and_moments + + +class _Motor: + burn_start_time = 0.0 + burn_out_time = 0.0 + nozzle_radius = 0.05 + thrust = _Scalar(0.0) + + def pressure_thrust(self, _pressure): + return 0.0 + + +class _Rocket: + area = 1.0 + radius = 0.5 + cp_eccentricity_x = cp_eccentricity_y = 0.0 + thrust_eccentricity_x = thrust_eccentricity_y = 0.0 + air_brakes = [] + + def __init__( + self, + mass, + com_to_cdm, + inertia, + cp, + forces_and_moments, + mass_flow_rate=0.0, + mass_flow_rate_dot=0.0, + nozzle_to_cdm=NOZZLE_TO_CDM, + ): + self.motor = _Motor() + self.total_mass = _Scalar(mass) + self.total_mass_flow_rate = _Scalar(mass_flow_rate, mass_flow_rate_dot) + self.com_to_cdm_function = _Scalar(com_to_cdm) + self.nozzle_to_cdm = nozzle_to_cdm + self.nozzle_gyration_tensor = Matrix([[0, 0, 0], [0, 0, 0], [0, 0, 0]]) + self._inertia = Matrix(inertia) + surface = _Surface(forces_and_moments) + # (component, position, ref_factor) since #1129. The factor is 1.0, + # which is Components.add's own default, and u_dot_generalized does + # not read it. + self.aerodynamic_surfaces = [(surface, None, 1.0)] + self.surfaces_cp_to_cdm = {surface: Vector(cp)} + + def get_inertia_tensor_at_time(self, _t): + return self._inertia + + def get_inertia_tensor_derivative_at_time(self, _t): + return Matrix([[0, 0, 0], [0, 0, 0], [0, 0, 0]]) + + def power_off_drag_7d(self, *_args): + return 0.0 + + def power_on_drag_7d(self, *_args): + return 0.0 + + +class _Environment: + earth_rotation_vector = [0.0, 0.0, 0.0] + density = _Scalar(0.0) + wind_velocity_x = _Scalar(0.0) + wind_velocity_y = _Scalar(0.0) + speed_of_sound = _Scalar(340.0) + dynamic_viscosity = _Scalar(1.8e-5) + pressure = _Scalar(0.0) + gravity = _Scalar(0.0) + + +def _derivatives(**kwargs): + """Return (linear acceleration, angular acceleration) in the body frame.""" + flight = Flight.__new__(Flight) + flight.rocket = _Rocket(**kwargs) + flight.env = _Environment() + state = [0.0, 0.0, 1000.0, 0.0, 0.0, 10.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + u_dot = flight.u_dot_generalized(0.0, state) + return np.array(u_dot[3:6]), np.array(u_dot[10:13]) + + +def _center_of_mass_inertia(mass, com_to_cdm, inertia): + """Inertia about the true center of mass, by the parallel axis theorem.""" + lever = np.array([0.0, 0.0, -com_to_cdm]) + return np.array(inertia) - mass * ( + lever @ lever * np.eye(3) - np.outer(lever, lever) + ) + + +def _skew(vector): + x, y, z = vector + return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]]) + + +@pytest.mark.parametrize("cp", [[0.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 0.0, 0.4]]) +def test_angular_acceleration_uses_the_lever_arm_from_the_center_of_mass(cp): + """A force applied at cp must turn the rocket about its center of mass.""" + force = np.array([100.0, 0.0, 0.0]) + moment = np.cross(np.array(cp), force) + _, angular = _derivatives( + mass=MASS, + com_to_cdm=COM_TO_CDM, + inertia=INERTIA, + cp=cp, + forces_and_moments=(*force, *moment), + ) + inertia_cm = _center_of_mass_inertia(MASS, COM_TO_CDM, INERTIA) + lever = np.array(cp) - np.array([0.0, 0.0, -COM_TO_CDM]) + expected = np.linalg.solve(inertia_cm, np.cross(lever, force)) + np.testing.assert_allclose(angular, expected, rtol=1e-12, atol=1e-12) + + +def test_axial_force_through_the_axis_produces_no_rotation(): + """Control: an on-axis force can never generate an angular acceleration.""" + _, angular = _derivatives( + mass=MASS, + com_to_cdm=COM_TO_CDM, + inertia=INERTIA, + cp=[0.0, 0.0, -1.0], + forces_and_moments=(0.0, 0.0, 500.0, 0.0, 0.0, 0.0), + ) + np.testing.assert_allclose(angular, np.zeros(3), atol=1e-12) + + +def test_reference_origin_invariance(): + """The same rocket described about its CDM or about its CM must agree.""" + cp = np.array([0.0, 0.0, -1.0]) + force = np.array([100.0, 0.0, 0.0]) + cdm_to_cm = np.array([0.0, 0.0, -COM_TO_CDM]) + inertia_cm = _center_of_mass_inertia(MASS, COM_TO_CDM, INERTIA) + + about_cdm = { + "mass": MASS, + "com_to_cdm": COM_TO_CDM, + "inertia": INERTIA, + "cp": list(cp), + "forces_and_moments": (*force, *np.cross(cp, force)), + } + cp_from_cm = cp - cdm_to_cm + about_cm = { + "mass": MASS, + "com_to_cdm": 0.0, + "inertia": inertia_cm.tolist(), + "cp": list(cp_from_cm), + "forces_and_moments": (*force, *np.cross(cp_from_cm, force)), + "nozzle_to_cdm": NOZZLE_TO_CDM - COM_TO_CDM, + } + linear_cdm, angular_cdm = _derivatives(**about_cdm) + linear_cm, angular_cm = _derivatives(**about_cm) + + np.testing.assert_allclose(angular_cdm, angular_cm, rtol=1e-12, atol=1e-12) + np.testing.assert_allclose( + linear_cdm + np.cross(angular_cdm, cdm_to_cm), + linear_cm, + rtol=1e-12, + atol=1e-12, + ) + + +def test_satisfies_the_documented_block_system(): + """The returned pair must solve M*v + [m r]^T*w = T20 and I*w + [m r]*v = T21.""" + cp = [0.0, 0.0, -1.0] + force = np.array([100.0, -40.0, 250.0]) + moment = np.cross(np.array(cp), force) + linear, angular = _derivatives( + mass=MASS, + com_to_cdm=COM_TO_CDM, + inertia=INERTIA, + cp=cp, + forces_and_moments=(*force, *moment), + ) + coupling = MASS * _skew(np.array([0.0, 0.0, -COM_TO_CDM])) + np.testing.assert_allclose( + MASS * linear + coupling.T @ angular, force, rtol=1e-11, atol=1e-11 + ) + np.testing.assert_allclose( + np.array(INERTIA) @ angular + coupling @ linear, moment, rtol=1e-11, atol=1e-11 + ) + + +def test_nozzle_offset_enters_as_the_center_of_mass_to_nozzle_vector(): + """The mddot*(r_NOZ - r_CM) term must push toward the nozzle, that is aft.""" + mass_flow_rate_dot = 3.0 + linear, angular = _derivatives( + mass=MASS, + com_to_cdm=COM_TO_CDM, + inertia=INERTIA, + cp=[0.0, 0.0, 0.0], + forces_and_moments=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + mass_flow_rate=0.0, + mass_flow_rate_dot=mass_flow_rate_dot, + ) + expected_z = -mass_flow_rate_dot * (NOZZLE_TO_CDM - COM_TO_CDM) / MASS + np.testing.assert_allclose(angular, np.zeros(3), atol=1e-12) + np.testing.assert_allclose(linear, [0.0, 0.0, expected_z], rtol=1e-12, atol=1e-12) + + +class _NozzleStub: + def __init__(self, nozzle_radius, nozzle_to_cdm): + self.nozzle_to_cdm = nozzle_to_cdm + self.motor = type("motor", (), {"nozzle_radius": nozzle_radius}) + + +@pytest.mark.parametrize("nozzle_to_cdm", [0.0, 0.75, 1.35]) +def test_nozzle_gyration_tensor_matches_the_disk_second_moment(nozzle_to_cdm): + """S is the exit disk second moment per unit area about the CDM.""" + radius = 0.033 + tensor = Rocket.evaluate_nozzle_gyration_tensor(_NozzleStub(radius, nozzle_to_cdm)) + lateral = radius**2 / 4 + nozzle_to_cdm**2 + expected = np.diag([lateral, lateral, radius**2 / 2]) + np.testing.assert_allclose(np.array(tensor), expected, rtol=1e-12, atol=1e-15) + + +def test_nozzle_gyration_tensor_reduces_to_a_point_mass_flux(): + """A vanishing exit radius leaves only the offset, diag(d^2, d^2, 0).""" + offset = 1.35 + tensor = Rocket.evaluate_nozzle_gyration_tensor(_NozzleStub(0.0, offset)) + np.testing.assert_allclose( + np.array(tensor), np.diag([offset**2, offset**2, 0.0]), atol=1e-15 + )