diff --git a/CHANGELOG.md b/CHANGELOG.md index 38c9028b4..06fe848b5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,7 @@ Attention: The newest changes should be on top --> ### Added +- ENH: 3-DOF single rail button flight phase (tip-off analysis) [#920](https://github.com/RocketPy-Team/RocketPy/pull/920) - ENH: Add simplified opening shock force estimation [#1092](https://github.com/RocketPy-Team/RocketPy/pull/1092) - ENH: Add Qodo PR-Agent workflow using Google Gemini [#1089](https://github.com/RocketPy-Team/RocketPy/pull/1089) - ENH: Support for Meteomatics API in the `Environment` class [#1079](https://github.com/RocketPy-Team/RocketPy/pull/1079) @@ -46,6 +47,7 @@ Attention: The newest changes should be on top --> ### Fixed +- BUG: A `Flight` continued from another `Flight` object now records `t_initial`, so a rocket carrying sensors or controllers no longer raises `AttributeError` on that path. [#920](https://github.com/RocketPy-Team/RocketPy/pull/920) - BUG: Give each `CustomSampler` input its own deterministic stream, and seed samplers sharing one generator once as a group. Existing fixed-seed `CustomSampler` baselines change, and samplers built on the legacy `RandomState` must move to `default_rng` because seeds now carry the full 128 bits. [#1102](https://github.com/RocketPy-Team/RocketPy/pull/1102) - BUG: Accept the callable parachute triggers `StochasticParachute` documents, and reject the ones it cannot mean. An invalid string, an empty list or a boolean now fails during validation instead of reaching `Parachute` or becoming a one-metre height trigger. [#1103](https://github.com/RocketPy-Team/RocketPy/pull/1103) - BUG: rocket with a late-starting thrust curve never leaves the rail [#1085](https://github.com/RocketPy-Team/RocketPy/pull/1085) diff --git a/docs/technical/index.rst b/docs/technical/index.rst index 73583eba9..ee1f353e6 100644 --- a/docs/technical/index.rst +++ b/docs/technical/index.rst @@ -12,6 +12,7 @@ in their code. Equations of Motion v0 Equations of Motion v1 + Tip-off Elliptical Fins Individual Fin Roll Moment diff --git a/docs/technical/references.rst b/docs/technical/references.rst index 0eb4132f5..02b90b8ff 100644 --- a/docs/technical/references.rst +++ b/docs/technical/references.rst @@ -6,3 +6,7 @@ References .. [Niskanen] Niskanen, S. (2013). *Development of an Open Source model rocket simulation software*. .. [Model] Barrowman, James S.. (1970). *Model Rocketry*. + +.. [Chou] Chou, P.-C., et al. *Tip-off effect analysis of a vehicle moving along an inclined guideway by considering dynamic interactions*. + +.. [Hosken] Hosken, R. W., et al. *Analysis of missile launchers, part Q: tip-off effects in helical rail launchers*. diff --git a/docs/technical/tip_off.rst b/docs/technical/tip_off.rst new file mode 100644 index 000000000..ade9ddfb1 --- /dev/null +++ b/docs/technical/tip_off.rst @@ -0,0 +1,244 @@ +.. _tipoff: + +=========================================== +Tip-off: the 3-DOF Single Rail Button Phase +=========================================== + +Introduction +------------ + +Between the moment the *upper* rail button leaves the launch rail and the moment +the *lower* button follows it, the rocket is still guided --- but only at one +point. It slides along the rail while free to pitch and yaw about that remaining +button. This short interval is what the literature calls **tip-off**, and it sets +the attitude and angular rate with which the rocket begins free flight. + +This document derives the equations of motion used by +:meth:`rocketpy.Flight.udot_rail2`, the flight phase that models this interval. +It is enabled with ``Flight(..., use_udot_rail2=True)``; when disabled (the +default) the simulation transitions straight from the 1-DOF rail phase to the +generalized 6-DOF equations, exactly as it did before this phase existed. + +The three flight phases around rail departure are, in order of the distance +``d`` travelled from the launch point: + +.. math:: + + \begin{aligned} + \texttt{udot_rail1} \quad & \text{for } d < \ell_1 + && \text{(both buttons engaged, 1 DOF)} \\ + \texttt{udot_rail2} \quad & \text{for } \ell_1 \le d < \ell_2 + && \text{(upper button gone, lower engaged, 3 DOF)} \\ + \texttt{u_dot_generalized} \quad & \text{for } d \ge \ell_2 + && \text{(free flight, 6 DOF)} + \end{aligned} + +where :math:`\ell_1` and :math:`\ell_2` are the ``effective_1rl`` and +``effective_2rl`` attributes of :class:`rocketpy.Flight` --- the distances at +which the upper and the lower button reach the end of the rail. Their difference +is the button-to-button distance, so the phase has zero length for a rocket with +a single rail button and is skipped in that case. + +Frames and conventions +---------------------- + +The solver integrates the same 13-element state vector used by the other +right-hand sides, + +.. math:: + + \mathbf{u} = [\,x,\ y,\ z,\ v_x,\ v_y,\ v_z,\ e_0,\ e_1,\ e_2,\ e_3,\ + \omega_1,\ \omega_2,\ \omega_3\,] + +with :math:`\mathbf{r} = [x, y, z]` the inertial position of the **center of dry +mass** (CDM, the tracked point), :math:`\mathbf{v}` its inertial velocity, +:math:`\mathbf{e}` the attitude quaternion and :math:`\boldsymbol{\omega}` the +angular velocity in the **body** frame. The matrix +:math:`\mathbf{K} = \texttt{Matrix.transformation}(\mathbf{e})` rotates body +components into inertial ones, and :math:`\hat{\mathbf{z}}_b = [0, 0, 1]` is the +body roll (symmetry) axis. + +The relevant mass geometry at time :math:`t`, in the body frame, is the total +mass :math:`m`, the CDM-to-center-of-mass offset :math:`\mathbf{r}_{CM}`, and the +inertia tensor about the CDM, :math:`\mathbf{I}`. Shifting the latter to the +instantaneous center of mass gives + +.. math:: + + \mathbf{I}_{CM} = \mathbf{I} - m\left(|\mathbf{r}_{CM}|^2 \mathbb{1} + - \mathbf{r}_{CM}\mathbf{r}_{CM}^{\mathsf T}\right). + +**Rail geometry.** The rail is a line fixed in the inertial frame, set by the +launch inclination and heading. Its unit vector is the ``attitude_unit`` +attribute of :class:`rocketpy.Flight`; the constrained body point is the lower +rail button, at the fixed body position :math:`\mathbf{r}_{B}` relative to the +CDM. + +The constraint +-------------- + +A free rigid body has six degrees of freedom. The single engaged button removes +three of them: + +#. **The button stays on the rail line.** Its position may only vary along the + rail, so the component of its acceleration perpendicular to the rail + vanishes. That is **two** scalar constraints. +#. **Roll is suppressed** by the button in its rail slot: + :math:`\dot{\boldsymbol{\omega}} \cdot \hat{\mathbf{z}}_b = 0`. That is + **one** more. + +Three constraints leave :math:`6 - 3 = 3` degrees of freedom: translation along +the rail, pitch and yaw --- the 3 DOF this phase is named for. + +The forces that enforce them are the unknowns of the problem: + +* a **normal reaction** at the button, perpendicular to the rail because a + frictionless slot can neither pull nor push along it. Writing an orthonormal + body triad :math:`\{\hat{\mathbf{e}}_1, \hat{\mathbf{e}}_2, \hat{\mathbf{n}}_b\}` + around the body-frame rail direction + :math:`\hat{\mathbf{n}}_b = \mathbf{K}^{\mathsf T}\hat{\mathbf{n}}`, it has two + components: :math:`\mathbf{N}_b = \lambda_1 \hat{\mathbf{e}}_1 + \lambda_2 + \hat{\mathbf{e}}_2`; +* a **roll reaction moment** :math:`\mu \hat{\mathbf{z}}_b` about the body axis. + +Three unknowns, three constraints: a :math:`3\times3` linear system, solved once +per evaluation of the right-hand side. + +The initial conditions are consistent with the constraint. The phase is entered +from ``udot_rail1``, where the rocket has no angular velocity and its velocity is +along the rail, so the button's perpendicular *velocity* is already zero. +Enforcing zero perpendicular *acceleration* therefore keeps it on the rail. + +Augmented dynamics +------------------ + +The generalized equations of motion assemble a total force +:math:`\mathbf{T}_{20}` and a total moment about the CDM :math:`\mathbf{T}_{21}`, +both in the body frame, and solve + +.. math:: + + \dot{\boldsymbol{\omega}}_{\text{free}} = \mathbf{I}_{CM}^{-1} + \left(\mathbf{T}_{21} + \mathbf{T}_{20} \times \mathbf{r}_{CM}\right), + \qquad + \mathbf{a}_{\text{free}} = \frac{\mathbf{T}_{20}}{m} + - \mathbf{r}_{CM} \times \dot{\boldsymbol{\omega}}_{\text{free}}. + +Both totals are *sums of external contributions*, which is what makes the +constraint clean to add: the reaction wrench simply enters the sums, + +.. math:: + + \mathbf{T}_{20}' = \mathbf{T}_{20} + \mathbf{N}_b, + \qquad + \mathbf{T}_{21}' = \mathbf{T}_{21} + \mathbf{r}_{B} \times \mathbf{N}_b + + \mu \hat{\mathbf{z}}_b, + +after which the same two lines apply. Since the solve is linear in the totals, +the result splits into the free solution plus a response to the unknowns. Using +:math:`\mathbf{r}_B \times \mathbf{N} + \mathbf{N} \times \mathbf{r}_{CM} += (\mathbf{r}_B - \mathbf{r}_{CM}) \times \mathbf{N}` and writing +:math:`\mathbf{d} = \mathbf{r}_{B} - \mathbf{r}_{CM}` for the center-of-mass-to-button +vector, + +.. math:: + + \Delta\dot{\boldsymbol{\omega}} = \mathbf{I}_{CM}^{-1} + \left(\mathbf{d} \times \mathbf{N}_b + \mu \hat{\mathbf{z}}_b\right), + \qquad + \Delta\mathbf{a} = \frac{\mathbf{N}_b}{m} + - \mathbf{r}_{CM} \times \Delta\dot{\boldsymbol{\omega}}. + +The button is body-fixed, so its acceleration in body components is + +.. math:: + + \mathbf{A} = \mathbf{A}_{\text{free}} + \Delta\mathbf{a} + + \Delta\dot{\boldsymbol{\omega}} \times \mathbf{r}_{B}, + \qquad + \mathbf{A}_{\text{free}} = \mathbf{K}^{\mathsf T}\mathbf{a}_{\text{free}} + + \dot{\boldsymbol{\omega}}_{\text{free}} \times \mathbf{r}_{B} + + \boldsymbol{\omega} \times + \left(\boldsymbol{\omega} \times \mathbf{r}_{B}\right). + +The linear solve +---------------- + +Collect the unknowns in :math:`\boldsymbol{\chi} = [\lambda_1, \lambda_2, \mu]`. +The three constraints read + +.. math:: + + \mathbf{A} \cdot \hat{\mathbf{e}}_1 = 0, + \qquad + \mathbf{A} \cdot \hat{\mathbf{e}}_2 = 0, + \qquad + \dot{\boldsymbol{\omega}} \cdot \hat{\mathbf{z}}_b = 0, + +and because :math:`\mathbf{A}` and :math:`\dot{\boldsymbol{\omega}}` are linear +in :math:`\boldsymbol{\chi}`, they form the system +:math:`\mathbf{J}\boldsymbol{\chi} = -\mathbf{g}_{\text{free}}` with + +.. math:: + + \mathbf{g}_{\text{free}} = + \begin{bmatrix} + \mathbf{A}_{\text{free}} \cdot \hat{\mathbf{e}}_1 \\ + \mathbf{A}_{\text{free}} \cdot \hat{\mathbf{e}}_2 \\ + \dot{\boldsymbol{\omega}}_{\text{free}} \cdot \hat{\mathbf{z}}_b + \end{bmatrix}. + +Each column of :math:`\mathbf{J}` is obtained by evaluating the response above at +one of the three unit inputs :math:`(\mathbf{N}_b, \mu) = +(\hat{\mathbf{e}}_1, 0)`, :math:`(\hat{\mathbf{e}}_2, 0)`, +:math:`(\mathbf{0}, 1)`. Solving for :math:`\boldsymbol{\chi}` and substituting +back gives the constrained accelerations, which override the free ones in the +returned derivative: + +.. math:: + + \dot{\boldsymbol{\omega}} = \dot{\boldsymbol{\omega}}_{\text{free}} + + \Delta\dot{\boldsymbol{\omega}}, + \qquad + \mathbf{a}_{CDM} = \mathbf{a}_{\text{free}} + + \mathbf{K}\,\Delta\mathbf{a}. + +The position and quaternion derivatives are the ordinary kinematic ones. In +particular :math:`\dot{\mathbf{r}} = \mathbf{v}`: the velocity is **not** +projected onto the rail. The constraint acts at the acceleration level on the +*button*, and the CDM legitimately acquires a small perpendicular velocity as the +rocket pitches about that button. + +Modelling assumptions and edge cases +------------------------------------ + +* **Roll axis.** The constraint suppresses roll about the *body* axis. The rocket + travels only the button-to-button distance during this phase, so the tip-off + angle is small and the body axis stays close to the rail direction; the + difference between body roll and rail-axis roll is of that order. This is + consistent with ``udot_rail1``, which freezes rotation entirely. +* **Radial button offset.** The button is modelled on the rocket axis. The roll + constraint is enforced explicitly by :math:`\mu`, so what is lost is only the + (small) roll coupling through the button's radial standoff. +* **Zero-length phase.** A rocket with a single rail button has + :math:`\ell_1 = \ell_2`, and the phase is skipped. +* **Singular system.** Should the geometry make :math:`\mathbf{J}` singular, the + step falls back to the unconstrained dynamics and warns. + +Expected behaviour +------------------ + +The phase reproduces the two effects tip-off is modelled for. With no wind, the +center of mass sits ahead of the button that the rocket now pivots about, so +gravity pitches the nose down by a fraction of a degree. With a crosswind, the +aerodynamic moment turns the rocket into the wind before it is fully free --- +the weathercock effect --- and the rocket therefore leaves the rail with a small +angular rate rather than none. + +References +---------- + +The tip-off phase and its effect on the initial conditions of free flight are +treated in the launcher dynamics literature: [Chou]_ studies a vehicle moving +along an inclined guideway with dynamic interactions, and [Hosken]_ analyses +tip-off effects in rail launchers. diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index 55ca3486f..b51c5a618 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -504,6 +504,7 @@ def __init__( # pylint: disable=too-many-arguments,too-many-statements verbose=False, name="Flight", equations_of_motion="standard", + use_udot_rail2=False, ode_solver="LSODA", simulation_mode="6 DOF", ): @@ -581,6 +582,15 @@ def __init__( # pylint: disable=too-many-arguments,too-many-statements more restricted set of equations of motion that only works for solid propulsion rockets. Such equations were used in RocketPy v0 and are kept here for backwards compatibility. + use_udot_rail2 : bool, optional + If True, enable the intermediate 3-DOF "tip-off" rail phase + ``udot_rail2``: after the upper rail button leaves the rail (at + ``effective_1rl``) the rocket pivots about the still-engaged lower + button until it too leaves the rail (at ``effective_2rl``), before + the generalized 6-DOF free flight. If False, the flight transitions + directly from the 1-DOF ``udot_rail1`` phase to the generalized + 6-DOF dynamics at the upper button exit, as in previous versions. + Default is False. ode_solver : str, ``scipy.integrate.OdeSolver``, optional Integration method to use to solve the equations of motion ODE. Available options are: 'RK23', 'RK45', 'DOP853', 'Radau', 'BDF', @@ -626,6 +636,8 @@ def __init__( # pylint: disable=too-many-arguments,too-many-statements self.equations_of_motion = equations_of_motion self.simulation_mode = simulation_mode self.ode_solver = ode_solver + # Enable or disable the intermediate 3-DOF rail phase + self.use_udot_rail2 = use_udot_rail2 # Controller initialization self.__init_controllers() @@ -1043,14 +1055,31 @@ def __check_simulation_events(self, phase, phase_index, node_index): bool True if an event occurred and the simulation should break. """ - # Check for first out of rail event - if len(self.out_of_rail_state) == 1 and ( + # Squared distance travelled from the launch point, used by both rail + # button exit checks below. + squared_distance_travelled = ( self.y_sol[0] ** 2 + self.y_sol[1] ** 2 + (self.y_sol[2] - self.env.elevation) ** 2 - >= self.effective_1rl**2 + ) + # Check for first out of rail event (upper rail button leaving the + # rail, at effective_1rl). This starts the 3-DOF tip-off phase when + # enabled, otherwise transitions straight to generalized 6-DOF flight. + if ( + len(self.out_of_rail_state) == 1 + and squared_distance_travelled >= self.effective_1rl**2 ): return self.__handle_out_of_rail_event(phase, phase_index, node_index) + # Check for the lower rail button leaving the rail (at effective_2rl), + # which ends the tip-off phase and begins generalized 6-DOF flight. + # Only relevant while the intermediate udot_rail2 phase is active. + elif ( + self.use_udot_rail2 + and len(self.between_rails_state) == 1 + and len(self.out_of_rail_state) != 1 + and squared_distance_travelled >= self.effective_2rl**2 + ): + return self.__handle_between_rails_event(phase, phase_index, node_index) # Check for apogee event # TODO: negative vz doesn't really mean apogee. Improve this. @@ -1063,34 +1092,37 @@ def __check_simulation_events(self, phase, phase_index, node_index): return False - def __handle_out_of_rail_event(self, phase, phase_index, node_index): - """Handle the out of rail event. + def __root_find_rail_exit_time(self, phase, effective_rl): + """Root-find the exact time at which the squared distance travelled + (from the launch point, ignoring ground elevation) equals + ``effective_rl ** 2``, using cubic Hermite interpolation between the two + most recent solution points. Parameters ---------- phase : FlightPhase - The current flight phase. - phase_index : int - The index of the current phase. - node_index : int - The index of the current node. + The current flight phase (provides the solver step size). + effective_rl : float + Effective rail length whose crossing is being solved for + (``effective_1rl`` for the upper button, ``effective_2rl`` for the + lower button). Returns ------- - bool - True to indicate the simulation should break. + float + Absolute simulation time of the crossing. """ # Check exactly when it went out using root finding # Disconsider elevation self.solution[-2][3] -= self.env.elevation self.solution[-1][3] -= self.env.elevation # Get points - y0 = sum(self.solution[-2][i] ** 2 for i in [1, 2, 3]) - self.effective_1rl**2 + y0 = sum(self.solution[-2][i] ** 2 for i in [1, 2, 3]) - effective_rl**2 yp0 = 2 * sum( self.solution[-2][i] * self.solution[-2][i + 3] for i in [1, 2, 3] ) t1 = self.solution[-1][0] - self.solution[-2][0] - y1 = sum(self.solution[-1][i] ** 2 for i in [1, 2, 3]) - self.effective_1rl**2 + y1 = sum(self.solution[-1][i] ** 2 for i in [1, 2, 3]) - effective_rl**2 yp1 = 2 * sum( self.solution[-1][i] * self.solution[-1][i + 3] for i in [1, 2, 3] ) @@ -1119,15 +1151,87 @@ def __handle_out_of_rail_event(self, phase, phase_index, node_index): raise ValueError("Multiple roots found when solving for rail exit time.") if len(valid_t_root) == 0: # pragma: no cover raise ValueError("No valid roots found when solving for rail exit time.") + return valid_t_root[0] + self.solution[-2][0] + + def __handle_out_of_rail_event(self, phase, phase_index, node_index): + """Handle the out of rail event (upper rail button leaving the rail, at + ``effective_1rl``). + + Records the out-of-rail state and inserts the next flight phase: the + 3-DOF tip-off phase ``udot_rail2`` when it is enabled and there is a + nonzero single-button window (``effective_2rl > effective_1rl``), + otherwise the generalized 6-DOF dynamics. + + Parameters + ---------- + phase : FlightPhase + The current flight phase. + phase_index : int + The index of the current phase. + node_index : int + The index of the current node. + + Returns + ------- + bool + True to indicate the simulation should break. + """ # Determine final state when upper button is going out of rail - self.t = valid_t_root[0] + self.solution[-2][0] + self.t = self.__root_find_rail_exit_time(phase, self.effective_1rl) interpolator = phase.solver.dense_output() self.y_sol = interpolator(self.t) self.solution[-1] = [self.t, *self.y_sol] self.out_of_rail_time = self.t self.out_of_rail_time_index = len(self.solution) - 1 self.out_of_rail_state = self.y_sol - # Create new flight phase + # Create new flight phase: the intermediate 3-DOF tip-off phase if + # enabled and the two buttons are distinct, otherwise straight to the + # generalized 6-DOF dynamics (previous behavior). + if self.use_udot_rail2 and self.effective_2rl > self.effective_1rl: + next_derivative = self.udot_rail2 + else: + next_derivative = self.u_dot_generalized + self.flight_phases.add_phase( + self.t, + next_derivative, + index=phase_index + 1, + ) + # Prepare to leave loops and start new flight phase + phase.time_nodes.flush_after(node_index) + phase.time_nodes.add_node(self.t, [], [], []) + phase.solver.status = "finished" + return True + + def __handle_between_rails_event(self, phase, phase_index, node_index): + """Handle the end of the 3-DOF tip-off phase (lower rail button leaving + the rail, at ``effective_2rl``). + + Records the between-rails state at the exact ``effective_2rl`` crossing + and inserts the generalized 6-DOF free-flight phase. + + Parameters + ---------- + phase : FlightPhase + The current flight phase. + phase_index : int + The index of the current phase. + node_index : int + The index of the current node. + + Returns + ------- + bool + True to indicate the simulation should break. + """ + # Determine final state when lower button is going out of rail + self.t = self.__root_find_rail_exit_time(phase, self.effective_2rl) + interpolator = phase.solver.dense_output() + self.y_sol = interpolator(self.t) + self.solution[-1] = [self.t, *self.y_sol] + self.between_rails_time = self.t + self.between_rails_time_index = len(self.solution) - 1 + self.between_rails_state = self.y_sol + # Create the generalized 6-DOF free-flight phase self.flight_phases.add_phase( self.t, self.u_dot_generalized, @@ -1547,6 +1651,9 @@ def __init_solution_monitors(self): self.out_of_rail_time = 0 self.out_of_rail_time_index = 0 self.out_of_rail_state = np.array([0]) + self.between_rails_state = np.array([0]) + self.between_rails_time = 0 + self.between_rails_time_index = 0 self.apogee_state = np.array([0]) self.apogee = 0 self.apogee_time = 0 @@ -1559,6 +1666,16 @@ def __init_solution_monitors(self): def __init_flight_state(self): """Initialize flight state variables.""" + # The rail is a fixed inertial line set by the launch inclination and + # heading, so its unit vector is known regardless of how the flight + # state is initialized. udot_rail2 constrains the lower button to it. + self.attitude_unit = Vector( + [ + np.cos(np.radians(self.inclination)) * np.sin(np.radians(self.heading)), + np.cos(np.radians(self.inclination)) * np.cos(np.radians(self.heading)), + np.sin(np.radians(self.inclination)), + ] + ) if self.initial_solution is None: # Initialize time and state variables self.t_initial = 0 @@ -1588,6 +1705,7 @@ def __init_flight_state(self): e0_init, e1_init, e2_init, e3_init = euler313_to_quaternions( self.phi_init, self.theta_init, self.psi_init ) + # Store initial conditions self.initial_solution = [ self.t_initial, @@ -1607,23 +1725,24 @@ def __init_flight_state(self): ] # Set initial derivative for rail phase self.initial_derivative = self.udot_rail1 - elif isinstance(self.initial_solution, Flight): - # Initialize time and state variables based on last solution of - # previous flight - self.initial_solution = self.initial_solution.solution[-1] - # Set unused monitors - self.out_of_rail_state = self.initial_solution[1:] - self.out_of_rail_time = self.initial_solution[0] - self.out_of_rail_time_index = 0 - # Set initial derivative for 6-DOF flight phase - self.initial_derivative = self.u_dot_generalized else: - # Initial solution given, ignore rail phase + if isinstance(self.initial_solution, Flight): + # Initialize time and state variables based on last solution of + # previous flight + self.initial_solution = self.initial_solution.solution[-1] + # Initial solution given, ignore rail phases # TODO: Check if rocket is actually out of rail. Otherwise, start at rail + # Both rail phases are skipped, so their monitors record the given + # starting state: out of rail (upper button) and, when the + # intermediate tip-off phase is enabled, between rails (lower button). self.out_of_rail_state = self.initial_solution[1:] self.out_of_rail_time = self.initial_solution[0] self.out_of_rail_time_index = 0 + self.between_rails_state = self.initial_solution[1:] + self.between_rails_time = self.initial_solution[0] + self.between_rails_time_index = 0 self.t_initial = self.initial_solution[0] + # Set initial derivative for 6-DOF flight phase self.initial_derivative = self.u_dot_generalized if self._controllers or self.sensors: # Handle post process during simulation, get initial accel/forces @@ -1890,9 +2009,27 @@ def udot_rail1(self, t, u, post_processing=False): return [vx, vy, vz, ax, ay, az, 0, 0, 0, 0, 0, 0, 0] - def udot_rail2(self, t, u, post_processing=False): # pragma: no cover - """[Still not implemented] Calculates derivative of u state vector with - respect to time when rocket is flying in 3 DOF motion in the rail. + def udot_rail2(self, t, u, post_processing=False): + """Calculates the derivative of the u state vector with respect to time + for the intermediate 3-DOF "tip-off" rail phase: the upper rail button + has left the rail but the lower button is still engaged, so the rocket + slides along the (fixed) rail line while free to pitch and yaw about the + lower button, with roll suppressed. + + The dynamics reuse the free variable-mass generalized equations of + motion (:meth:`u_dot_generalized`) and enforce the single-button + constraint by adding an unknown reaction wrench solved from a small + linear system: a normal force ``N`` at the lower button (perpendicular + to the rail, 2 DOF) plus a roll reaction moment ``mu`` (1 DOF). The + three unknowns are found from three constraints -- the button's + acceleration perpendicular to the rail is zero (2) and the roll angular + acceleration is zero (1). The full derivation is in the technical + documentation, :ref:`Tip-off `. All reaction quantities are + expressed in the "true" + body frame (the one used by ``surfaces_cp_to_cdm``, body-z toward the + nose), so ``r_CM`` and the button position are taken with that sign + convention -- independent of the internal (point-to-CDM) convention used + by the generalized equations. Parameters ---------- @@ -1911,8 +2048,102 @@ def udot_rail2(self, t, u, post_processing=False): # pragma: no cover State vector defined by u_dot = [vx, vy, vz, ax, ay, az, e0dot, e1dot, e2dot, e3dot, alpha1, alpha2, alpha3]. """ - # Hey! We will finish this function later, now we just can use u_dot - return self.u_dot_generalized(t, u, post_processing=post_processing) + # Free (unconstrained) generalized solution. This also handles the + # aerodynamic/post-processing bookkeeping. We keep its position and + # quaternion derivatives and only override the 6-DOF accelerations + # (indices 3:6 inertial linear, 10:13 body angular) with the + # constrained values computed below. + u_dot = list(self.u_dot_generalized(t, u, post_processing=post_processing)) + a_cdm_free = Vector(u_dot[3:6]) # inertial CDM acceleration (free) + w_dot_free = Vector(u_dot[10:13]) # body angular acceleration (free) + + # State quantities + e0, e1, e2, e3 = u[6:10] + w = Vector(u[10:13]) + K = Matrix.transformation([e0, e1, e2, e3]) + Kt = K.transpose + total_mass = self.rocket.total_mass.get_value_opt(t) + + # Geometry in the true body frame (body-z toward the nose): + # position of a point p relative to the CDM = (p - cdm) * csys. + # The generalized EOM store r_CM / r_NOZ as (point -> CDM) vectors, i.e. + # the negative of the true-frame position; hence the sign flips below. + csys = self.rocket._csys + cdm = self.rocket.center_of_dry_mass_position + r_CM = Vector([0, 0, -self.rocket.com_to_cdm_function.get_value_opt(t)]) + lower_button_z = self.rocket.rail_buttons[0].position.z + r_button = Vector([0, 0, (lower_button_z - cdm) * csys]) + + # Inertia about the instantaneous center of mass (sign-independent). + inertia_tensor = self.rocket.get_inertia_tensor_at_time(t) + H = (r_CM.cross_matrix @ -r_CM.cross_matrix) * total_mass + inv_inertia_cm = (inertia_tensor - H).inverse + + # Orthonormal body triad with the rail direction. The rail is the fixed + # inertial unit vector ``attitude_unit``; express it in the body frame. + n = Kt @ self.attitude_unit + n = n / abs(n) + # Pick the coordinate axis least aligned with n to build a stable basis. + if abs(n.z) <= abs(n.x) and abs(n.z) <= abs(n.y): + helper = Vector([0.0, 0.0, 1.0]) + elif abs(n.y) <= abs(n.x): + helper = Vector([0.0, 1.0, 0.0]) + else: + helper = Vector([1.0, 0.0, 0.0]) + e1_b = n ^ helper + e1_b = e1_b / abs(e1_b) + e2_b = n ^ e1_b # already unit (n, e1_b orthonormal) + z_b = Vector([0.0, 0.0, 1.0]) # body roll axis + + # Linear response of (angular accel, CDM accel, button accel) in the + # body frame to a reaction force ``Fr`` (body) at the button and a roll + # reaction moment ``tau`` about the body axis. + def _response(f_r, tau): + d_wdot = inv_inertia_cm @ ( + ((r_button - r_CM) ^ f_r) + Vector([0.0, 0.0, tau]) + ) + d_a_cdm_body = f_r * (1.0 / total_mass) - (d_wdot ^ r_CM) + d_a_button = d_a_cdm_body + (d_wdot ^ r_button) + return d_wdot, d_a_cdm_body, d_a_button + + # Free button acceleration in the body frame. + a_button_free = ( + (Kt @ a_cdm_free) + (w_dot_free ^ r_button) + (w ^ (w ^ r_button)) + ) + + # Assemble the 3x3 system J @ [lambda1, lambda2, mu] = -g_free. + jacobian = np.empty((3, 3)) + for col, (f_r, tau) in enumerate( + ((e1_b, 0.0), (e2_b, 0.0), (Vector([0.0, 0.0, 0.0]), 1.0)) + ): + d_wdot, _, d_a_button = _response(f_r, tau) + jacobian[0, col] = d_a_button @ e1_b + jacobian[1, col] = d_a_button @ e2_b + jacobian[2, col] = d_wdot @ z_b + g_free = np.array( + [a_button_free @ e1_b, a_button_free @ e2_b, w_dot_free @ z_b] + ) + + try: + lambda1, lambda2, mu = np.linalg.solve(jacobian, -g_free) + except np.linalg.LinAlgError: # pragma: no cover + warnings.warn( + "Singular constraint system in udot_rail2; falling back to " + "unconstrained dynamics for this step.", + RuntimeWarning, + ) + return u_dot + + f_r = e1_b * lambda1 + e2_b * lambda2 + d_wdot, d_a_cdm_body, _ = _response(f_r, mu) + w_dot = w_dot_free + d_wdot + # Enforce exactly zero roll acceleration (kills residual round-off). + w_dot = Vector([w_dot.x, w_dot.y, 0.0]) + a_cdm = a_cdm_free + (K @ d_a_cdm_body) + + u_dot[3:6] = [a_cdm.x, a_cdm.y, a_cdm.z] + u_dot[10:13] = [w_dot.x, w_dot.y, w_dot.z] + return u_dot def u_dot(self, t, u, post_processing=False): # pylint: disable=too-many-locals,too-many-statements """Calculates derivative of u state vector with respect to time @@ -4224,6 +4455,7 @@ def to_dict(self, **kwargs): "time": self.time, "out_of_rail_velocity": self.out_of_rail_velocity, "out_of_rail_state": self.out_of_rail_state, + "between_rails_state": self.between_rails_state, "apogee_x": self.apogee_x, "apogee_y": self.apogee_y, "apogee_state": self.apogee_state, diff --git a/tests/unit/simulation/test_flight.py b/tests/unit/simulation/test_flight.py index 9a3c54477..d2afde6f1 100644 --- a/tests/unit/simulation/test_flight.py +++ b/tests/unit/simulation/test_flight.py @@ -139,6 +139,42 @@ def test_get_controller_observed_variables(flight_calisto_air_brakes): assert len(obs_vars) == 0 +def test_initial_solution_from_flight_sets_initial_time( + calisto_with_sensors, example_plain_env +): + """A Flight continued from another Flight object must record ``t_initial``. + It is needed to post-process the initial state, so a rocket carrying sensors + or controllers used to raise ``AttributeError`` on this path. + + Arrange: fly a rocket that carries sensors. + Act: start a second flight from the first Flight object. + Assert: ``t_initial`` is the time the previous flight ended at. + """ + # Arrange + first = Flight( + rocket=calisto_with_sensors, + environment=example_plain_env, + rail_length=5.2, + inclination=85, + heading=0, + terminate_on_apogee=True, + ) + + # Act + second = Flight( + rocket=calisto_with_sensors, + environment=example_plain_env, + rail_length=5.2, + inclination=85, + heading=0, + initial_solution=first, + max_time=first.t_final + 1, + ) + + # Assert + assert second.t_initial == first.solution[-1][0] + + def test_initial_stability_margin(flight_calisto_custom_wind): """Test the initial_stability_margin method of the Flight class. diff --git a/tests/unit/simulation/test_udot_rail2_feature.py b/tests/unit/simulation/test_udot_rail2_feature.py new file mode 100644 index 000000000..b6769d434 --- /dev/null +++ b/tests/unit/simulation/test_udot_rail2_feature.py @@ -0,0 +1,239 @@ +"""Unit tests for the ``udot_rail2`` 3-DOF "tip-off" rail phase (issue #28). + +These tests follow the project's testing conventions: each test uses the +Arrange / Act / Assert pattern and documents the expected behaviour in its +docstring. They avoid plotting and optional-dependency features so they run +reliably in CI. + +Coverage: +- Phase ordering: ``udot_rail1`` -> ``udot_rail2`` -> ``u_dot_generalized``. +- Opt-in default: ``use_udot_rail2`` defaults to False and, when disabled, no + ``udot_rail2`` phase is inserted (previous behaviour preserved). +- Constraint satisfaction: the lower rail button stays on the rail line and the + roll acceleration/rate remains zero throughout the phase. +- Physical direction: with no wind the nose pitches over (gravity tip-off). +""" + +import math + +from rocketpy.mathutils import Matrix, Vector +from rocketpy.simulation.flight import Flight + + +def _make_flight(rocket, environment, use_udot_rail2): + return Flight( + rocket=rocket, + environment=environment, + rail_length=5.2, + inclination=85, + heading=0, + terminate_on_apogee=True, + use_udot_rail2=use_udot_rail2, + ) + + +def _phase_names(flight): + return [ + phase.derivative.__name__ if phase.derivative is not None else None + for phase in flight.flight_phases.list + ] + + +def _lower_button_body_position(rocket): + """Lower rail button position relative to the CDM in the true body frame.""" + z = (rocket.rail_buttons[0].position.z - rocket.center_of_dry_mass_position) * ( + rocket._csys + ) + return Vector([0.0, 0.0, z]) + + +def _tip_off_rows(flight): + """Solution rows [t, *state] within the tip-off window (inclusive).""" + t0, t1 = flight.out_of_rail_time, flight.between_rails_time + return [row for row in flight.solution if t0 - 1e-12 <= row[0] <= t1 + 1e-12] + + +def _attitude_inclination_deg(state): + """Inclination (deg from horizontal) of the body axis in the inertial frame.""" + body_axis = Matrix.transformation(state[6:10]) @ Vector([0.0, 0.0, 1.0]) + return math.degrees(math.atan2(body_axis.z, math.hypot(body_axis.x, body_axis.y))) + + +def test_udot_rail2_default_is_opt_in(calisto_robust, example_spaceport_env): + """``use_udot_rail2`` defaults to False and, when not requested, the flight + keeps the previous rail1 -> generalized transition with no ``udot_rail2``. + + Arrange: build a Flight without passing ``use_udot_rail2``. + Act: read the flag and the phase derivative names. + Assert: the flag is False and no ``udot_rail2`` phase was inserted. + """ + # Arrange / Act + flight = Flight( + rocket=calisto_robust, + environment=example_spaceport_env, + rail_length=5.2, + inclination=85, + heading=0, + terminate_on_apogee=True, + ) + + # Assert + assert flight.use_udot_rail2 is False, ( + "tip-off phase must be opt-in (default False)" + ) + assert "udot_rail2" not in _phase_names(flight) + + +def test_udot_rail2_inserts_phase_in_order(calisto_robust, example_spaceport_env): + """With ``use_udot_rail2=True`` the intermediate 3-DOF ``udot_rail2`` phase + is inserted between the 1-DOF ``udot_rail1`` and the 6-DOF + ``u_dot_generalized`` phases. + + Arrange: build a Flight with ``use_udot_rail2=True``. + Act: inspect the ordered phase derivative names. + Assert: rail1 < rail2 < generalized in the phase list. + """ + # Arrange / Act + flight = _make_flight(calisto_robust, example_spaceport_env, True) + names = _phase_names(flight) + + # Assert + assert "udot_rail2" in names, "udot_rail2 phase not present" + assert names.index("udot_rail1") < names.index("udot_rail2"), ( + "udot_rail2 should follow the 1-DOF rail phase" + ) + assert names.index("udot_rail2") < names.index("u_dot_generalized"), ( + "udot_rail2 should precede the generalized 6-DOF phase" + ) + + +def test_udot_rail2_window_is_between_effective_rail_lengths( + calisto_robust, example_spaceport_env +): + """The tip-off phase spans the interval between the upper button exit + (``effective_1rl``, recorded as ``out_of_rail``) and the lower button exit + (``effective_2rl``, recorded as ``between_rails``). + + Arrange: build a Flight with the tip-off phase enabled. + Act: read the event times. + Assert: ``0 < out_of_rail_time < between_rails_time`` and the window is short. + """ + # Arrange / Act + flight = _make_flight(calisto_robust, example_spaceport_env, True) + + # Assert + assert flight.effective_2rl > flight.effective_1rl + assert 0 < flight.out_of_rail_time < flight.between_rails_time + assert (flight.between_rails_time - flight.out_of_rail_time) < 1.0 + + +def test_udot_rail2_button_stays_on_rail(calisto_robust, example_spaceport_env): + """The single-button constraint must keep the lower rail button on the rail + line: its distance from the rail axis stays ~0 throughout the tip-off phase. + + Arrange: run a Flight with the tip-off phase enabled. + Act: for every solution point in the tip-off window, compute the lower + button position and its perpendicular distance to the (fixed) rail line. + Assert: the maximum perpendicular offset is negligible. + """ + # Arrange + flight = _make_flight(calisto_robust, example_spaceport_env, True) + r_b = _lower_button_body_position(flight.rocket) + rail_dir = flight.attitude_unit + rail_origin = Vector(flight.solution[0][1:4]) + + # Act + max_perp = 0.0 + for row in _tip_off_rows(flight): + state = row[1:] + cdm = Vector(state[0:3]) + button = cdm + Matrix.transformation(state[6:10]) @ r_b + offset = button - rail_origin + perpendicular = offset - rail_dir * (offset @ rail_dir) + max_perp = max(max_perp, abs(perpendicular)) + + # Assert + assert len(_tip_off_rows(flight)) >= 2, "tip-off window not exercised" + assert max_perp < 1e-6, f"button drifted off the rail (max offset {max_perp} m)" + + +def test_udot_rail2_no_roll(calisto_robust, example_spaceport_env): + """The tip-off phase must not induce roll: both the roll angular + acceleration returned by ``udot_rail2`` and the integrated roll rate stay + zero. + + Arrange: run a Flight with the tip-off phase enabled. + Act: evaluate ``udot_rail2`` at the phase-end state and scan the roll rate + over the tip-off window. + Assert: the roll angular acceleration and every sampled roll rate are ~0. + """ + # Arrange + flight = _make_flight(calisto_robust, example_spaceport_env, True) + + # Act + u_dot = flight.udot_rail2(flight.between_rails_time, flight.between_rails_state) + roll_acceleration = u_dot[12] + max_roll_rate = max(abs(row[1:][12]) for row in _tip_off_rows(flight)) + + # Assert + assert abs(roll_acceleration) < 1e-12, ( + f"expected zero roll acceleration, got {roll_acceleration}" + ) + assert max_roll_rate < 1e-9, f"expected zero roll rate, got {max_roll_rate}" + + +def test_udot_rail2_rail_axis_defined_for_given_initial_solution( + calisto_robust, example_spaceport_env +): + """The rail axis ``udot_rail2`` constrains the button to is set by the launch + inclination and heading, so it must exist even when the rail phase is skipped + because an ``initial_solution`` was supplied. + + Arrange: run a launch and take a mid-flight state from its solution. + Act: build a Flight that starts from that state instead of the rail. + Assert: ``attitude_unit`` is the same rail unit vector as the launch's, so + ``udot_rail2`` cannot raise ``AttributeError``. + """ + # Arrange + launch = _make_flight(calisto_robust, example_spaceport_env, True) + mid_flight_state = list(launch.solution[len(launch.solution) // 2]) + + # Act + continuation = Flight( + rocket=calisto_robust, + environment=example_spaceport_env, + rail_length=5.2, + inclination=85, + heading=0, + terminate_on_apogee=True, + initial_solution=mid_flight_state, + use_udot_rail2=True, + ) + + # Assert + assert abs(abs(continuation.attitude_unit) - 1) < 1e-12 + for axis in range(3): + assert continuation.attitude_unit[axis] == launch.attitude_unit[axis] + + +def test_udot_rail2_gravity_tip_off_direction(calisto_robust, example_spaceport_env): + """With no wind, gravity acting on the center of mass (ahead of the lower + button pivot) tips the nose over: the attitude inclination decreases across + the tip-off phase by a small amount. + + Arrange: run a wind-free Flight with the tip-off phase enabled. + Act: measure the attitude inclination at the start and end of the window. + Assert: the inclination decreases, by a small (sub-degree) magnitude. + """ + # Arrange + flight = _make_flight(calisto_robust, example_spaceport_env, True) + rows = _tip_off_rows(flight) + + # Act + incl_start = _attitude_inclination_deg(rows[0][1:]) + incl_end = _attitude_inclination_deg(rows[-1][1:]) + delta = incl_end - incl_start + + # Assert + assert delta < 0, f"nose should pitch down (gravity tip-off), got {delta:+.4f} deg" + assert abs(delta) < 1.0, f"tip-off rotation implausibly large: {delta:+.4f} deg"