Add Thrust Vector Control

rocketpy/environment/environment.py

@@ -2434,22 +2434,26 @@ def add_wind_gust(self, wind_gust_x, wind_gust_y):
 
         # Reset wind heading and velocity magnitude
         self.wind_heading = Function(
-            lambda h: (180 / np.pi)
-            * np.arctan2(
-                self.wind_velocity_x.get_value_opt(h),
-                self.wind_velocity_y.get_value_opt(h),
-            )
-            % 360,
+            lambda h: (
+                (180 / np.pi)
+                * np.arctan2(
+                    self.wind_velocity_x.get_value_opt(h),
+                    self.wind_velocity_y.get_value_opt(h),
+                )
+                % 360
+            ),
             "Height (m)",
             "Wind Heading (degrees)",
             extrapolation="constant",
         )
         self.wind_speed = Function(
             lambda h: (
-                self.wind_velocity_x.get_value_opt(h) ** 2
-                + self.wind_velocity_y.get_value_opt(h) ** 2
-            )
-            ** 0.5,
+                (
+                    self.wind_velocity_x.get_value_opt(h) ** 2
+                    + self.wind_velocity_y.get_value_opt(h) ** 2
+                )
+                ** 0.5
+            ),
             "Height (m)",
             "Wind Speed (m/s)",
             extrapolation="constant",

rocketpy/prints/tvc_prints.py

@@ -0,0 +1,29 @@
+class _TVCPrints:
+    """Class that contains all TVC prints."""
+
+    def __init__(self, tvc):
+        """Initializes _TVCPrints class
+
+        Parameters
+        ----------
+        tvc: rocketpy.TVC
+            Instance of the TVC class.
+
+        Returns
+        -------
+        None
+        """
+        self.tvc = tvc
+
+    def geometry(self):
+        """Prints geometric information of the TVC system."""
+        print("Geometric information of the TVC System:")
+        print("----------------------------------")
+        print(f"Maximum Gimbal Angle: {self.tvc.gimbal_range:.2f}°")
+        print(f"Current Gimbal Angle X: {self.tvc.gimbal_angle_x:.2f}°")
+        print(f"Current Gimbal Angle Y: {self.tvc.gimbal_angle_y:.2f}°")
+        print(f"Angle Clamping: {'Enabled' if self.tvc.clamp else 'Disabled'}")
+
+    def all(self):
+        """Prints all information of the TVC system."""
+        self.geometry()

rocketpy/rocket/aero_surface/nose_cone.py

@@ -237,10 +237,16 @@ def theta(x):
                 return np.arccos(1 - 2 * max(min(x / self.length, 1), 0))
 
             self.y_nosecone = Function(
-                lambda x: self.base_radius
-                * (theta(x) - np.sin(2 * theta(x)) / 2 + (np.sin(theta(x)) ** 3) / 3)
-                ** (0.5)
-                / (np.pi**0.5)
+                lambda x: (
+                    self.base_radius
+                    * (
+                        theta(x)
+                        - np.sin(2 * theta(x)) / 2
+                        + (np.sin(theta(x)) ** 3) / 3
+                    )
+                    ** (0.5)
+                    / (np.pi**0.5)
+                )
             )
 
         elif value in ["tangent", "tangentogive", "ogive"]:
@@ -253,15 +259,19 @@ def theta(x):
             area = np.pi * self.base_radius**2
             self.k = 1 - volume / (area * self.length)
             self.y_nosecone = Function(
-                lambda x: np.sqrt(rho**2 - (min(x - self.length, 0)) ** 2)
-                + (self.base_radius - rho)
+                lambda x: (
+                    np.sqrt(rho**2 - (min(x - self.length, 0)) ** 2)
+                    + (self.base_radius - rho)
+                )
             )
 
         elif value == "elliptical":
             self.k = 1 / 3
             self.y_nosecone = Function(
-                lambda x: self.base_radius
-                * np.sqrt(1 - ((x - self.length) / self.length) ** 2)
+                lambda x: (
+                    self.base_radius
+                    * np.sqrt(1 - ((x - self.length) / self.length) ** 2)
+                )
             )
 
         elif value == "vonkarman":
@@ -271,15 +281,19 @@ def theta(x):
                 return np.arccos(1 - 2 * max(min(x / self.length, 1), 0))
 
             self.y_nosecone = Function(
-                lambda x: self.base_radius
-                * (theta(x) - np.sin(2 * theta(x)) / 2) ** (0.5)
-                / (np.pi**0.5)
+                lambda x: (
+                    self.base_radius
+                    * (theta(x) - np.sin(2 * theta(x)) / 2) ** (0.5)
+                    / (np.pi**0.5)
+                )
             )
         elif value == "parabolic":
             self.k = 0.5
             self.y_nosecone = Function(
-                lambda x: self.base_radius
-                * ((2 * x / self.length - (x / self.length) ** 2) / (2 - 1))
+                lambda x: (
+                    self.base_radius
+                    * ((2 * x / self.length - (x / self.length) ** 2) / (2 - 1))
+                )
             )
         elif value == "powerseries":
             self.k = (2 * self.power) / ((2 * self.power) + 1)

rocketpy/rocket/aero_surface/tail.py

@@ -164,10 +164,12 @@ def evaluate_lift_coefficient(self):
         """
         # Calculate clalpha
         self.clalpha = Function(
-            lambda mach: 2
-            * (
-                (self.bottom_radius / self.rocket_radius) ** 2
-                - (self.top_radius / self.rocket_radius) ** 2
+            lambda mach: (
+                2
+                * (
+                    (self.bottom_radius / self.rocket_radius) ** 2
+                    - (self.top_radius / self.rocket_radius) ** 2
+                )
             ),
             "Mach",
             f"Lift coefficient derivative for {self.name}",

rocketpy/rocket/parachute.py

@@ -211,9 +211,10 @@ def __init__(
         )
 
         alpha, beta = self.noise_corr
-        self.noise_function = lambda: alpha * self.noise_signal[-1][
-            1
-        ] + beta * np.random.normal(noise[0], noise[1])
+        self.noise_function = lambda: (
+            alpha * self.noise_signal[-1][1]
+            + beta * np.random.normal(noise[0], noise[1])
+        )
 
         self.prints = _ParachutePrints(self)
 

rocketpy/rocket/rocket.py

@@ -23,6 +23,7 @@
 from rocketpy.rocket.aero_surface.generic_surface import GenericSurface
 from rocketpy.rocket.components import Components
 from rocketpy.rocket.parachute import Parachute
+from rocketpy.rocket.tvc import TVC
 from rocketpy.tools import (
     deprecated,
     find_obj_from_hash,
@@ -625,12 +626,14 @@ def evaluate_stability_margin(self):
         self.stability_margin.set_source(
             lambda mach, time: (
                 (
-                    self.center_of_mass.get_value_opt(time)
-                    - self.cp_position.get_value_opt(mach)
+                    (
+                        self.center_of_mass.get_value_opt(time)
+                        - self.cp_position.get_value_opt(mach)
+                    )
+                    / (2 * self.radius)
                 )
-                / (2 * self.radius)
+                * self._csys
             )
-            * self._csys
         )
         return self.stability_margin
 
@@ -647,10 +650,12 @@ def evaluate_static_margin(self):
         # Calculate static margin
         self.static_margin.set_source(
             lambda time: (
-                self.center_of_mass.get_value_opt(time)
-                - self.cp_position.get_value_opt(0)
+                (
+                    self.center_of_mass.get_value_opt(time)
+                    - self.cp_position.get_value_opt(0)
+                )
+                / (2 * self.radius)
             )
-            / (2 * self.radius)
         )
         # Change sign if coordinate system is upside down
         self.static_margin *= self._csys
@@ -1718,6 +1723,124 @@ def add_air_brakes(
         else:
             return air_brakes
 
+    def add_tvc(
+        self,
+        gimbal_range,
+        controller_function,
+        sampling_rate,
+        clamp=True,
+        initial_observed_variables=None,
+        return_controller=False,
+        name="TVC",
+        controller_name="TVC Controller",
+    ):
+        """Creates a new thrust vector control (TVC) system, storing its
+        parameters such as gimbal angle maximum controller function, and
+        sampling rate.
+
+        Parameters
+        ----------
+        gimbal_range : int, float
+            Maximum gimbal range in degrees. Both x and y gimbal
+            angles are clamped to this range if clamp is True. Must be
+            positive.
+        controller_function : function, callable
+            An user-defined function responsible for controlling the TVC system.
+            This function is expected to take the following arguments, in order:
+
+            1. `time` (float): The current simulation time in seconds.
+            2. `sampling_rate` (float): The rate at which the controller
+               function is called, measured in Hertz (Hz).
+            3. `state` (list): The state vector of the simulation, structured as
+               `[x, y, z, vx, vy, vz, e0, e1, e2, e3, wx, wy, wz]`.
+            4. `state_history` (list): A record of the rocket's state at each
+               step throughout the simulation. The state_history is organized as a
+               list of lists, with each sublist containing a state vector. The last
+               item in the list always corresponds to the previous state vector,
+               providing a chronological sequence of the rocket's evolving states.
+            5. `observed_variables` (list): A list containing the variables that
+               the controller function manages. The initial values in the first
+               step of the simulation are provided by the
+               `initial_observed_variables` argument.
+            6. `interactive_objects` (list): A list containing the TVC object
+               that the controller function can interact with.
+            7. `sensors` (list): A list of sensors that are attached to the
+                rocket. The most recent measurements of the sensors are provided
+                with the ``sensor.measurement`` attribute. The sensors are
+                listed in the same order as they are added to the rocket
+               ``interactive_objects``
+
+            This function will be called during the simulation at the specified
+            sampling rate. The function should evaluate and change the observed
+            objects as needed. The function should return None.
+
+            .. note::
+
+                The function will be called according to the sampling rate specified.
+
+        sampling_rate : float
+            The sampling rate of the controller function in Hertz (Hz). This
+            means that the controller function will be called every
+            `1/sampling_rate` seconds.
+        clamp : bool, optional
+            If True, the simulation will clamp gimbal angles to the range
+            [-gimbal_range, gimbal_range]. If False, a warning is
+            issued when gimbal angles exceed the range. Default is True.
+        initial_observed_variables : list, optional
+            A list of the initial values of the variables that the controller
+            function manages. This list is used to initialize the
+            `observed_variables` argument of the controller function. The
+            default value is None, which initializes the list as an empty list.
+        return_controller : bool, optional
+            If True, the function will return the controller object created.
+            Default is False.
+        name : string, optional
+            TVC system name. Has no impact in simulation, as it is only used to
+            display data in a more organized matter. Default is "TVC".
+        controller_name : string, optional
+            Controller name. Has no impact in simulation, as it is only used to
+            display data in a more organized matter. Default is "TVC Controller".
+
+        Returns
+        -------
+        tvc : TVC
+            TVC object created.
+        controller : Controller, optional
+            Controller object created (only if return_controller is True).
+        """
+        if hasattr(self, "tvc"):
+            # pylint: disable=access-member-before-definition
+            print(
+                "Only one TVC per rocket is currently supported. "
+                + "Overwriting previous TVC and controllers."
+            )
+            self._controllers = [
+                controller
+                for controller in self._controllers
+                if not isinstance(controller.interactive_objects, TVC)
+            ]
+
+        tvc = TVC(
+            gimbal_range=gimbal_range,
+            clamp=clamp,
+            gimbal_angle_x=0,
+            gimbal_angle_y=0,
+            name=name,
+        )
+        _controller = _Controller(
+            interactive_objects=tvc,
+            controller_function=controller_function,
+            sampling_rate=sampling_rate,
+            initial_observed_variables=initial_observed_variables,
+            name=controller_name,
+        )
+        self.tvc = tvc
+        self._add_controllers(_controller)
+        if return_controller:
+            return tvc, _controller
+        else:
+            return tvc
+
     def set_rail_buttons(
         self,
         upper_button_position,