@@ -1055,14 +1055,19 @@ def __check_simulation_events(self, phase, phase_index, node_index):
10551055 bool
10561056 True if an event occurred and the simulation should break.
10571057 """
1058- # Check for first out of rail event (upper rail button leaving the
1059- # rail, at effective_1rl). This starts the 3-DOF tip-off phase when
1060- # enabled, otherwise transitions straight to generalized 6-DOF flight.
1061- if len (self .out_of_rail_state ) == 1 and (
1058+ # Squared distance travelled from the launch point, used by both rail
1059+ # button exit checks below.
1060+ squared_distance_travelled = (
10621061 self .y_sol [0 ] ** 2
10631062 + self .y_sol [1 ] ** 2
10641063 + (self .y_sol [2 ] - self .env .elevation ) ** 2
1065- >= self .effective_1rl ** 2
1064+ )
1065+ # Check for first out of rail event (upper rail button leaving the
1066+ # rail, at effective_1rl). This starts the 3-DOF tip-off phase when
1067+ # enabled, otherwise transitions straight to generalized 6-DOF flight.
1068+ if (
1069+ len (self .out_of_rail_state ) == 1
1070+ and squared_distance_travelled >= self .effective_1rl ** 2
10661071 ):
10671072 return self .__handle_out_of_rail_event (phase , phase_index , node_index )
10681073 # Check for the lower rail button leaving the rail (at effective_2rl),
@@ -1072,12 +1077,7 @@ def __check_simulation_events(self, phase, phase_index, node_index):
10721077 self .use_udot_rail2
10731078 and len (self .between_rails_state ) == 1
10741079 and len (self .out_of_rail_state ) != 1
1075- and (
1076- self .y_sol [0 ] ** 2
1077- + self .y_sol [1 ] ** 2
1078- + (self .y_sol [2 ] - self .env .elevation ) ** 2
1079- >= self .effective_2rl ** 2
1080- )
1080+ and squared_distance_travelled >= self .effective_2rl ** 2
10811081 ):
10821082 return self .__handle_between_rails_event (phase , phase_index , node_index )
10831083
@@ -1666,6 +1666,16 @@ def __init_solution_monitors(self):
16661666
16671667 def __init_flight_state (self ):
16681668 """Initialize flight state variables."""
1669+ # The rail is a fixed inertial line set by the launch inclination and
1670+ # heading, so its unit vector is known regardless of how the flight
1671+ # state is initialized. udot_rail2 constrains the lower button to it.
1672+ self .attitude_unit = Vector (
1673+ [
1674+ np .cos (np .radians (self .inclination )) * np .sin (np .radians (self .heading )),
1675+ np .cos (np .radians (self .inclination )) * np .cos (np .radians (self .heading )),
1676+ np .sin (np .radians (self .inclination )),
1677+ ]
1678+ )
16691679 if self .initial_solution is None :
16701680 # Initialize time and state variables
16711681 self .t_initial = 0
@@ -1696,17 +1706,6 @@ def __init_flight_state(self):
16961706 self .phi_init , self .theta_init , self .psi_init
16971707 )
16981708
1699- K_init = Matrix .transformation ([e0_init , e1_init , e2_init , e3_init ])
1700-
1701- # Body axis pointing along rocket symmetry (body z-axis)
1702- body_axis = Vector ([0 , 0 , 1 ])
1703-
1704- # Attitude vector in inertial frame
1705- attitude_vec = K_init @ body_axis
1706-
1707- # Unit vector (normalize)
1708- self .attitude_unit = attitude_vec / abs (attitude_vec )
1709-
17101709 # Store initial conditions
17111710 self .initial_solution = [
17121711 self .t_initial ,
@@ -2065,8 +2064,8 @@ def udot_rail2(self, t, u, post_processing=False):
20652064 w_dot_free = Vector (u_dot [10 :13 ]) # body angular acceleration (free)
20662065
20672066 # State quantities
2068- _ , _ , _ , vx , vy , vz , e0 , e1 , e2 , e3 , omega1 , omega2 , omega3 = u
2069- w = Vector ([ omega1 , omega2 , omega3 ])
2067+ e0 , e1 , e2 , e3 = u [ 6 : 10 ]
2068+ w = Vector (u [ 10 : 13 ])
20702069 K = Matrix .transformation ([e0 , e1 , e2 , e3 ])
20712070 Kt = K .transpose
20722071 total_mass = self .rocket .total_mass .get_value_opt (t )
@@ -2079,12 +2078,12 @@ def udot_rail2(self, t, u, post_processing=False):
20792078 cdm = self .rocket .center_of_dry_mass_position
20802079 r_CM = Vector ([0 , 0 , - self .rocket .com_to_cdm_function .get_value_opt (t )])
20812080 lower_button_z = self .rocket .rail_buttons [0 ].position .z
2082- r_B = Vector ([0 , 0 , (lower_button_z - cdm ) * csys ])
2081+ r_button = Vector ([0 , 0 , (lower_button_z - cdm ) * csys ])
20832082
20842083 # Inertia about the instantaneous center of mass (sign-independent).
20852084 inertia_tensor = self .rocket .get_inertia_tensor_at_time (t )
20862085 H = (r_CM .cross_matrix @ - r_CM .cross_matrix ) * total_mass
2087- I_CM_inv = (inertia_tensor - H ).inverse
2086+ inv_inertia_cm = (inertia_tensor - H ).inverse
20882087
20892088 # Orthonormal body triad with the rail direction. The rail is the fixed
20902089 # inertial unit vector ``attitude_unit``; express it in the body frame.
@@ -2106,13 +2105,17 @@ def udot_rail2(self, t, u, post_processing=False):
21062105 # body frame to a reaction force ``Fr`` (body) at the button and a roll
21072106 # reaction moment ``tau`` about the body axis.
21082107 def _response (f_r , tau ):
2109- d_wdot = I_CM_inv @ (((r_B - r_CM ) ^ f_r ) + Vector ([0.0 , 0.0 , tau ]))
2108+ d_wdot = inv_inertia_cm @ (
2109+ ((r_button - r_CM ) ^ f_r ) + Vector ([0.0 , 0.0 , tau ])
2110+ )
21102111 d_a_cdm_body = f_r * (1.0 / total_mass ) - (d_wdot ^ r_CM )
2111- d_a_button = d_a_cdm_body + (d_wdot ^ r_B )
2112+ d_a_button = d_a_cdm_body + (d_wdot ^ r_button )
21122113 return d_wdot , d_a_cdm_body , d_a_button
21132114
21142115 # Free button acceleration in the body frame.
2115- a_button_free = (Kt @ a_cdm_free ) + (w_dot_free ^ r_B ) + (w ^ (w ^ r_B ))
2116+ a_button_free = (
2117+ (Kt @ a_cdm_free ) + (w_dot_free ^ r_button ) + (w ^ (w ^ r_button ))
2118+ )
21162119
21172120 # Assemble the 3x3 system J @ [lambda1, lambda2, mu] = -g_free.
21182121 jacobian = np .empty ((3 , 3 ))
0 commit comments