From 7d17a0021d0afd3f5019895b7e97c9ec2d2d7d04 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 3 Dec 2025 21:35:16 +0000 Subject: [PATCH 1/3] Initial plan From e63a73f31d8cc27f015fca114a385271d02d9e3c Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 3 Dec 2025 21:39:33 +0000 Subject: [PATCH 2/3] Refactor: eliminate quaternion derivative code duplication in u_dot_generalized_3dof Co-authored-by: aZira371 <99824864+aZira371@users.noreply.github.com> --- rocketpy/simulation/flight.py | 75 +++++++++++++---------------------- 1 file changed, 27 insertions(+), 48 deletions(-) diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index 81d2c48d6..f1000669d 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -1929,55 +1929,33 @@ def u_dot_generalized_3dof(self, t, u, post_processing=False): rotation_axis = body_z_inertial ^ desired_direction rotation_axis_mag = abs(rotation_axis) + # Determine omega_body based on alignment state + omega_body = None + if rotation_axis_mag > 1e-8: - # Normalize rotation axis + # Normal case: compute angular velocity from misalignment rotation_axis = rotation_axis / rotation_axis_mag # The magnitude of the cross product of two unit vectors equals # the sine of the angle between them - sin_angle = rotation_axis_mag - - # Clamp sin_angle to valid range - sin_angle = min(1.0, max(-1.0, sin_angle)) + sin_angle = min(1.0, max(-1.0, rotation_axis_mag)) # Angular velocity magnitude proportional to misalignment angle omega_mag = self.weathercock_coeff * sin_angle - # Angular velocity in inertial frame - omega_inertial = rotation_axis * omega_mag - - # Transform angular velocity to body frame - omega_body = Kt @ omega_inertial - - # Quaternion derivative from angular velocity in body frame - omega1_wc, omega2_wc, omega3_wc = ( - omega_body.x, - omega_body.y, - omega_body.z, - ) - e0_dot = 0.5 * (-omega1_wc * e1 - omega2_wc * e2 - omega3_wc * e3) - e1_dot = 0.5 * (omega1_wc * e0 + omega3_wc * e2 - omega2_wc * e3) - e2_dot = 0.5 * (omega2_wc * e0 - omega3_wc * e1 + omega1_wc * e3) - e3_dot = 0.5 * (omega3_wc * e0 + omega2_wc * e1 - omega1_wc * e2) - e_dot = [e0_dot, e1_dot, e2_dot, e3_dot] - w_dot = [0, 0, 0] # No angular acceleration in 3DOF model + # Angular velocity in inertial frame, then transform to body frame + omega_body = Kt @ (rotation_axis * omega_mag) else: # Check if aligned or anti-aligned using dot product - dot = body_z_inertial @ desired_direction # Vector dot product - if dot > 0.999: # Aligned - e_dot = [0, 0, 0, 0] - w_dot = [0, 0, 0] - elif dot < -0.999: # Anti-aligned + dot = body_z_inertial @ desired_direction + if dot < -0.999: # Anti-aligned # Choose an arbitrary perpendicular axis - # Try [1,0,0] unless body_z_inertial is parallel to it x_axis = Vector([1.0, 0.0, 0.0]) perp_axis = body_z_inertial ^ x_axis if abs(perp_axis) < 1e-6: - # If parallel, use y axis y_axis = Vector([0.0, 1.0, 0.0]) perp_axis = body_z_inertial ^ y_axis if abs(perp_axis) < 1e-6: - # If still parallel, raise an error or choose a default axis raise ValueError( "Cannot determine a valid rotation axis: " "body_z_inertial is parallel to both x and y axes." @@ -1985,23 +1963,24 @@ def u_dot_generalized_3dof(self, t, u, post_processing=False): rotation_axis = perp_axis.unit_vector # 180 degree rotation: sin(angle) = 1 omega_mag = self.weathercock_coeff * 1.0 - omega_inertial = rotation_axis * omega_mag - omega_body = Kt @ omega_inertial - omega1_wc, omega2_wc, omega3_wc = ( - omega_body.x, - omega_body.y, - omega_body.z, - ) - e0_dot = 0.5 * (-omega1_wc * e1 - omega2_wc * e2 - omega3_wc * e3) - e1_dot = 0.5 * (omega1_wc * e0 + omega3_wc * e2 - omega2_wc * e3) - e2_dot = 0.5 * (omega2_wc * e0 - omega3_wc * e1 + omega1_wc * e3) - e3_dot = 0.5 * (omega3_wc * e0 + omega2_wc * e1 - omega1_wc * e2) - e_dot = [e0_dot, e1_dot, e2_dot, e3_dot] - w_dot = [0, 0, 0] - else: - # Vectors are nearly aligned, treat as aligned - e_dot = [0, 0, 0, 0] - w_dot = [0, 0, 0] + omega_body = Kt @ (rotation_axis * omega_mag) + # else: aligned or nearly aligned, omega_body remains None + + # Compute quaternion derivatives from omega_body + if omega_body is not None: + omega1_wc, omega2_wc, omega3_wc = ( + omega_body.x, + omega_body.y, + omega_body.z, + ) + e0_dot = 0.5 * (-omega1_wc * e1 - omega2_wc * e2 - omega3_wc * e3) + e1_dot = 0.5 * (omega1_wc * e0 + omega3_wc * e2 - omega2_wc * e3) + e2_dot = 0.5 * (omega2_wc * e0 - omega3_wc * e1 + omega1_wc * e3) + e3_dot = 0.5 * (omega3_wc * e0 + omega2_wc * e1 - omega1_wc * e2) + e_dot = [e0_dot, e1_dot, e2_dot, e3_dot] + else: + e_dot = [0, 0, 0, 0] + w_dot = [0, 0, 0] # No angular acceleration in 3DOF model else: # No weathercocking or negligible freestream speed e_dot = [0, 0, 0, 0] From c92d5f09ad9528225984ca7abfcba6eddc4bec87 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 3 Dec 2025 21:41:32 +0000 Subject: [PATCH 3/3] Improve comment clarity in weathercocking aligned case Co-authored-by: aZira371 <99824864+aZira371@users.noreply.github.com> --- rocketpy/simulation/flight.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index f1000669d..0cf1e23ab 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -1964,7 +1964,7 @@ def u_dot_generalized_3dof(self, t, u, post_processing=False): # 180 degree rotation: sin(angle) = 1 omega_mag = self.weathercock_coeff * 1.0 omega_body = Kt @ (rotation_axis * omega_mag) - # else: aligned or nearly aligned, omega_body remains None + # else: aligned (dot > 0.999) - no rotation needed, omega_body stays None # Compute quaternion derivatives from omega_body if omega_body is not None: