Skip to content

BUG: apply gimbaled thrust components to translational dynamics #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ting-hong-shieh wants to merge 1 commit into
base: enh/actuator-merge-rocketpy
Choose a base branch
from
+153 −71
Open

BUG: apply gimbaled thrust components to translational dynamics #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
142 changes: 71 additions & 71 deletions rocketpy/simulation/flight.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1820,6 +1820,31 @@ def udot_rail2(self, t, u, post_processing=False): # pragma: no cover
# 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)

@staticmethod
def _calculate_thrust_vector(
effective_thrust, gimbal_angle_x=0.0, gimbal_angle_y=0.0
):
"""Calculate the motor thrust vector in the rocket body frame.

Positive ``gimbal_angle_x`` rotates the nominal +e3 thrust toward
-e2, while positive ``gimbal_angle_y`` rotates it toward +e1. Angles
are expressed in degrees and thrust components in newtons.

The axial component follows the two-axis decomposition already used
by the TVC moment model, so adding the lateral components preserves
the effective thrust magnitude.
"""
angle_x = np.deg2rad(gimbal_angle_x)
angle_y = np.deg2rad(gimbal_angle_y)
thrust1 = effective_thrust * np.sin(angle_y)
thrust2 = -effective_thrust * np.sin(angle_x)
axial_thrust_squared = max(
effective_thrust**2 - thrust1**2 - thrust2**2,
0.0,
)
thrust3 = np.sqrt(axial_thrust_squared)
return Vector([thrust1, thrust2, thrust3])

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
when rocket is flying in 6 DOF motion during ascent out of rail
Expand Down Expand Up @@ -1879,40 +1904,26 @@ def u_dot(self, t, u, post_processing=False): # pylint: disable=too-many-locals
)

# Thrust Vector Control (TVC)
if hasattr(self.rocket, "thrust_vector_control"):
# TVC Fz thrust: F = T * sqrt(1 - sin(gimbal_angle_x)**2 - sin(gimbal_angle_y)**2)
thrust3 = effective_thrust * np.sqrt(
1
- np.sin(
self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180)
)
** 2
- np.sin(
self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180)
)
** 2
)
tvc_lever = self.rocket.nozzle_to_cdm
# TVC Mx My moments: M = T * sin(x) * r
M1 += (
np.sin(
self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180)
)
* effective_thrust
* tvc_lever
)
M2 += (
np.sin(
self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180)
)
* effective_thrust
* tvc_lever
)
else:
thrust3 = effective_thrust
# Off center moment
M1 += self.rocket.thrust_eccentricity_y * thrust3
M2 -= self.rocket.thrust_eccentricity_x * thrust3
tvc = getattr(self.rocket, "thrust_vector_control", None)
thrust_vector = self._calculate_thrust_vector(
effective_thrust,
getattr(tvc, "gimbal_angle_x", 0.0),
getattr(tvc, "gimbal_angle_y", 0.0),
)
thrust1, thrust2, thrust3 = thrust_vector

# Moment from applying the complete thrust vector at the nozzle.
thrust_position = Vector(
[
self.rocket.thrust_eccentricity_x,
self.rocket.thrust_eccentricity_y,
self.rocket.nozzle_to_cdm,
]
)
thrust_moment = thrust_position ^ thrust_vector
M1 += thrust_moment.x
M2 += thrust_moment.y
M3 += thrust_moment.z

else:
# Motor stopped
Expand All @@ -1926,7 +1937,7 @@ def u_dot(self, t, u, post_processing=False): # pylint: disable=too-many-locals
# Mass
mass_flow_rate_at_t, propellant_mass_at_t = 0, 0
# thrust
thrust3 = 0
thrust1, thrust2, thrust3 = 0, 0, 0
net_thrust = 0

# Retrieve important quantities
Expand Down Expand Up @@ -2149,12 +2160,14 @@ def u_dot(self, t, u, post_processing=False): # pylint: disable=too-many-locals
L = [
(
R1
+ thrust1
- b * propellant_mass_at_t * (omega2**2 + omega3**2)
- 2 * c * mass_flow_rate_at_t * omega2
)
/ total_mass_at_t,
(
R2
+ thrust2
+ b * propellant_mass_at_t * (alpha3 + omega1 * omega2)
+ 2 * c * mass_flow_rate_at_t * omega1
)
Expand Down Expand Up @@ -2606,43 +2619,30 @@ def u_dot_generalized(self, t, u, post_processing=False): # pylint: disable=too
)

# Thrust Vector Control (TVC)
if hasattr(self.rocket, "thrust_vector_control"):
tvc_lever = self.rocket.nozzle_to_cdm
# TVC Mx My moments: M = T * sin(x) * r
M1 += (
np.sin(self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180))
* effective_thrust
* tvc_lever
)
M2 += (
np.sin(self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180))
* effective_thrust
* tvc_lever
)
# TVC Fz thrust: F = T * sqrt(1 - sin^2(x) - sin^2(y))
thrust3 = effective_thrust * np.sqrt(
1
- np.sin(
self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180)
)
** 2
- np.sin(
self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180)
)
** 2
)
else:
thrust3 = effective_thrust

# Off center moment
M1 += (
self.rocket.cp_eccentricity_y * R3
+ self.rocket.thrust_eccentricity_y * thrust3
tvc = getattr(self.rocket, "thrust_vector_control", None)
thrust_vector = self._calculate_thrust_vector(
effective_thrust,
getattr(tvc, "gimbal_angle_x", 0.0),
getattr(tvc, "gimbal_angle_y", 0.0),
)
M2 -= (
self.rocket.cp_eccentricity_x * R3
+ self.rocket.thrust_eccentricity_x * thrust3
thrust3 = thrust_vector.z

# Moment from applying the complete thrust vector at the nozzle.
thrust_position = Vector(
[
self.rocket.thrust_eccentricity_x,
self.rocket.thrust_eccentricity_y,
self.rocket.nozzle_to_cdm,
]
)
thrust_moment = thrust_position ^ thrust_vector
M1 += thrust_moment.x
M2 += thrust_moment.y
M3 += thrust_moment.z

# Off center moment
M1 += self.rocket.cp_eccentricity_y * R3
M2 -= self.rocket.cp_eccentricity_x * R3
M3 += self.rocket.cp_eccentricity_x * R2 - self.rocket.cp_eccentricity_y * R1

# Roll control moment
Expand All @@ -2656,7 +2656,7 @@ def u_dot_generalized(self, t, u, post_processing=False): # pylint: disable=too
T00 = total_mass * r_CM
T03 = 2 * total_mass_dot * (r_NOZ - r_CM) - 2 * total_mass * r_CM_dot
T04 = (
Vector([0, 0, thrust3])
thrust_vector
- total_mass * r_CM_ddot
- 2 * total_mass_dot * r_CM_dot
+ total_mass_ddot * (r_NOZ - r_CM)
Expand Down
82 changes: 82 additions & 0 deletions tests/unit/simulation/test_flight.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
from scipy import optimize

from rocketpy import Components, Flight, Function, Rocket
from rocketpy.mathutils.vector_matrix import Vector

plt.rcParams.update({"figure.max_open_warning": 0})

Expand Down Expand Up @@ -83,6 +84,87 @@ def compute_static_margin_error_given_distance(position, static_margin, rocket):
# Tests


def test_calculate_thrust_vector_zero_gimbal():
"""A centered nozzle must keep all effective thrust on the body e3 axis."""
# Arrange
effective_thrust = 100.0

# Act
thrust_vector = Flight._calculate_thrust_vector(effective_thrust)

# Assert
assert tuple(thrust_vector) == pytest.approx((0.0, 0.0, effective_thrust))


@pytest.mark.parametrize(
"gimbal_angle_x, gimbal_angle_y, expected_direction",
[
(15.0, 0.0, (0.0, -1.0, 1.0)),
(0.0, 15.0, (1.0, 0.0, 1.0)),
(-15.0, 0.0, (0.0, 1.0, 1.0)),
(0.0, -15.0, (-1.0, 0.0, 1.0)),
],
)
def test_calculate_thrust_vector_single_axis_gimbal(
gimbal_angle_x, gimbal_angle_y, expected_direction
):
"""A single-axis gimbal must create the expected signed lateral force."""
# Arrange
effective_thrust = 100.0
lateral = effective_thrust * np.sin(np.deg2rad(15.0))
axial = effective_thrust * np.cos(np.deg2rad(15.0))
scale = (lateral, lateral, axial)
expected = tuple(
direction * component
for direction, component in zip(expected_direction, scale)
)

# Act
thrust_vector = Flight._calculate_thrust_vector(
effective_thrust, gimbal_angle_x, gimbal_angle_y
)

# Assert
assert tuple(thrust_vector) == pytest.approx(expected)
assert abs(thrust_vector) == pytest.approx(effective_thrust)


def test_calculate_thrust_vector_two_axis_gimbal_preserves_magnitude():
"""Two-axis gimbaling must redirect, not create or destroy, thrust."""
# Arrange
effective_thrust = 100.0

# Act
thrust_vector = Flight._calculate_thrust_vector(
effective_thrust, gimbal_angle_x=6.0, gimbal_angle_y=8.0
)

# Assert
assert thrust_vector.x > 0.0
assert thrust_vector.y < 0.0
assert abs(thrust_vector) == pytest.approx(effective_thrust)


def test_gimbaled_thrust_moment_matches_force_at_nozzle():
"""The TVC moment must be the nozzle lever arm crossed with thrust."""
# Arrange
effective_thrust = 100.0
nozzle_to_cdm = 2.0
thrust_vector = Flight._calculate_thrust_vector(
effective_thrust, gimbal_angle_x=15.0
)
thrust_position = Vector([0.0, 0.0, nozzle_to_cdm])

# Act
thrust_moment = thrust_position ^ thrust_vector

# Assert
expected_moment_x = (
nozzle_to_cdm * effective_thrust * np.sin(np.deg2rad(15.0))
)
assert tuple(thrust_moment) == pytest.approx((expected_moment_x, 0.0, 0.0))


def test_get_solution_at_time(flight_calisto):
"""Test the get_solution_at_time method of the Flight class. This test
simply calls the method at the initial and final time and checks if the
Expand Down