diff --git a/docs/examples/halcyon_flight_sim_active_control.ipynb b/docs/examples/halcyon_flight_sim_active_control.ipynb new file mode 100644 index 000000000..3e2878665 --- /dev/null +++ b/docs/examples/halcyon_flight_sim_active_control.ipynb @@ -0,0 +1,543 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Halcyon - Aerospace Team Graz - 2023\n", + "\n", + "Launched at EuRoC'23, allowing the team to become the Overall Champions.\n", + "Permission to use flight data given by Dorothea Krasser, 2024.\n", + "\n", + "These results were extracted out of the flight card:\n", + "\n", + "1. Team number: `1`\n", + "2. Launch date: `October 13th, 2023. around 14hrs local time`\n", + "3. Last simulated apogee before flight: `3163 m` (this value differs from the simulation shown below because of the updates to rocketpy software)\n", + "4. Official recorded apogee: `3450 m`\n", + "\n", + "The relative error of altitude apogee is only `9.1%`\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "%load_ext autoreload\n", + "%autoreload 2" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import datetime\n", + "\n", + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "\n", + "from rocketpy import (\n", + " Accelerometer,\n", + " Environment,\n", + " Flight,\n", + " Gyroscope,\n", + " Rocket,\n", + ")\n", + "from rocketpy.motors import CylindricalTank, Fluid, HybridMotor\n", + "from rocketpy.motors.tank import MassFlowRateBasedTank\n", + "\n", + "plt.style.use(\"seaborn-v0_8-colorblind\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "tomorrow = datetime.date(2023, 10, 13) + datetime.timedelta(days=1)\n", + "\n", + "Env = Environment(\n", + " latitude=39.388692,\n", + " longitude=-8.287814,\n", + " elevation=130,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The team preferred to set a custom atmospheric model. However, for better resolution, this examples will only run the windy atmosphere that was registered in windy. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "Env.set_date((tomorrow.year, tomorrow.month, tomorrow.day, 12))\n", + "\n", + "Env.set_atmospheric_model(\n", + " type=\"custom_atmosphere\",\n", + " pressure=None,\n", + " temperature=300,\n", + " wind_u=[(0, 8), (1000, 10)],\n", + " wind_v=[(0, 0), (500, 0), (1600, 0)],\n", + ")\n", + "\n", + "Env.max_expected_height = 5000" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "Env.info()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Environment" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "env = Environment(\n", + " gravity=9.80665,\n", + " date=(2023, 10, 13, 14),\n", + " latitude=39.388692,\n", + " longitude=-8.287814,\n", + " elevation=130,\n", + " datum=\"WGS84\",\n", + " timezone=\"Portugal\",\n", + ")\n", + "\n", + "# env.set_atmospheric_model(type=\"Windy\", file=\"ECMWF\")\n", + "# env.max_expected_height = 4000\n", + "# env.info()\n", + "\n", + "env.set_atmospheric_model(\n", + " type=\"Reanalysis\",\n", + " file=\"../../data/weather/euroc_2023_all_windows.nc\",\n", + " dictionary=\"ECMWF\",\n", + ")\n", + "env.max_expected_height = 4000\n", + "env.info()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Motor" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "oxidizer_liq = Fluid(name=\"N2O_l\", density=960)\n", + "oxidizer_gas = Fluid(name=\"N2O_g\", density=1.9277)\n", + "\n", + "tank_shape = CylindricalTank(70 / 1000, 320 / 1000)\n", + "\n", + "oxidizer_tank = MassFlowRateBasedTank(\n", + " name=\"oxidizer_tank\",\n", + " geometry=tank_shape,\n", + " flux_time=(5),\n", + " initial_liquid_mass=4.3,\n", + " initial_gas_mass=0,\n", + " liquid_mass_flow_rate_in=0,\n", + " liquid_mass_flow_rate_out=4.2 / 5,\n", + " gas_mass_flow_rate_in=0,\n", + " gas_mass_flow_rate_out=0,\n", + " liquid=oxidizer_liq,\n", + " gas=oxidizer_gas,\n", + ")\n", + "\n", + "hybrid_motor = HybridMotor(\n", + " thrust_source=\"../../data/rockets/astg/engine_Halcyon_4thHotfire.eng\",\n", + " dry_mass=10670 / 1000,\n", + " dry_inertia=(1.668, 1.668, 0.026),\n", + " center_of_dry_mass_position=780 / 1000,\n", + " burn_time=5,\n", + " reshape_thrust_curve=False,\n", + " grain_number=1,\n", + " grain_separation=0,\n", + " grain_outer_radius=43 / 1000,\n", + " grain_initial_inner_radius=22.5 / 1000,\n", + " grain_initial_height=310 / 1000,\n", + " grain_density=920,\n", + " nozzle_radius=0.0141,\n", + " throat_radius=0.00677,\n", + " interpolation_method=\"linear\",\n", + " grains_center_of_mass_position=385 / 1000,\n", + " coordinate_system_orientation=\"nozzle_to_combustion_chamber\",\n", + ")\n", + "\n", + "hybrid_motor.add_tank(tank=oxidizer_tank, position=934.75 / 1000)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "hybrid_motor.info()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Rocket and Aerodynamic surfaces" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "HALCYON = Rocket(\n", + " radius=152.4 / 2000,\n", + " mass=14613 / 1000,\n", + " inertia=(24.56, 24.56, 70.074),\n", + " center_of_mass_without_motor=2344 / 1000,\n", + " power_off_drag=\"../../data/rockets/astg/DragCoeffOR_off.csv\",\n", + " power_on_drag=\"../../data/rockets/astg/DragCoeffOR_on.csv\",\n", + " coordinate_system_orientation=\"tail_to_nose\",\n", + ")\n", + "\n", + "HALCYON.set_rail_buttons(2.808, 1.549)\n", + "\n", + "HALCYON.add_motor(hybrid_motor, position=20 / 1000)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "NoseCone = HALCYON.add_nose(length=0.46, kind=\"vonKarman\", position=3556 / 1000)\n", + "\n", + "FinSet = HALCYON.add_trapezoidal_fins(\n", + " n=4,\n", + " span=0.125,\n", + " root_chord=0.247,\n", + " tip_chord=0.045,\n", + " position=0.263,\n", + ")\n", + "\n", + "Tail = HALCYON.add_tail(\n", + " top_radius=152.4 / 2000, bottom_radius=0.0496, length=0.254, position=0.254\n", + ")\n", + "HALCYON.info()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Remove parachute\n", + "# Main = HALCYON.add_parachute(\n", + "# name=\"Main\",\n", + "# cd_s=9.621,\n", + "# trigger=\"apogee\",\n", + "# sampling_rate=100,\n", + "# lag=5,\n", + "# )\n", + "gyro_clean = Gyroscope(sampling_rate=100)\n", + "accelerometer_clean = Accelerometer(sampling_rate=100)\n", + "HALCYON.add_sensor(gyro_clean, position=1.5)\n", + "HALCYON.add_sensor(accelerometer_clean, position=1.5)\n", + "HALCYON.plots.draw()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def tvc_controller_function(\n", + " time,\n", + " sampling_rate,\n", + " state,\n", + " state_history,\n", + " observed_variables,\n", + " thrust_vector_control,\n", + " sensors,\n", + "):\n", + " # state = [x, y, z, vx, vy, vz, e0, e1, e2, e3, wx, wy, wz]\n", + "\n", + " # print(time)\n", + "\n", + " thrust_vector_control.gimbal_angle_x = 0\n", + " thrust_vector_control.gimbal_angle_y = 0\n", + " # Return variables of interest to be saved in the observed_variables list\n", + " return (\n", + " time,\n", + " thrust_vector_control.gimbal_angle_x,\n", + " thrust_vector_control.gimbal_angle_y,\n", + " )\n", + "\n", + "\n", + "thrust_vector_control, tvc_controller = HALCYON.add_thrust_vector_control(\n", + " controller_function=tvc_controller_function,\n", + " sampling_rate=100,\n", + " max_gimbal_angle=10,\n", + " gimbal_rate_limit=100,\n", + " return_controller=True,\n", + ")\n", + "thrust_vector_control.x.info()\n", + "tvc_controller.info()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def roll_controller_function(\n", + " time, sampling_rate, state, state_history, observed_variables, roll_control, sensors\n", + "):\n", + " # state = [x, y, z, vx, vy, vz, e0, e1, e2, e3, wx, wy, wz]\n", + "\n", + " # Separate sensor data by type\n", + " gyro_data = None # rad/s\n", + " accel_data = None # m/s^2\n", + "\n", + " for sensor in sensors:\n", + " if isinstance(sensor, Gyroscope):\n", + " sensor_time, gx, gy, gz = zip(*sensor.measured_data)\n", + " gyro_data = {\n", + " \"time\": np.array(sensor_time),\n", + " \"x\": np.array(gx),\n", + " \"y\": np.array(gy),\n", + " \"z\": np.array(gz),\n", + " }\n", + " elif isinstance(sensor, Accelerometer):\n", + " sensor_time, ax, ay, az = zip(*sensor.measured_data)\n", + " accel_data = {\n", + " \"time\": np.array(sensor_time),\n", + " \"x\": np.array(ax),\n", + " \"y\": np.array(ay),\n", + " \"z\": np.array(az),\n", + " }\n", + "\n", + " KP = 50\n", + " KI = 1\n", + " KD = 0.0\n", + "\n", + " # Roll rate target: 0.5 Hz sinusoidal, ±10 deg/s\n", + " target = np.deg2rad(10 * np.sin(2 * np.pi * 0.5 * time)) # rad/s\n", + "\n", + " if gyro_data is None:\n", + " roll_control.roll_torque = 0\n", + " return (\n", + " time,\n", + " roll_control.roll_torque,\n", + " )\n", + "\n", + " roll_rate_errors = target - gyro_data[\"z\"]\n", + " roll_rate_error_integral = np.sum(roll_rate_errors) / sampling_rate\n", + " roll_rate_error_derivative = (\n", + " (roll_rate_errors[-1] - roll_rate_errors[-2]) * sampling_rate\n", + " if len(roll_rate_errors) > 1\n", + " else 0\n", + " )\n", + "\n", + " roll_control.roll_torque = (\n", + " KP * roll_rate_errors[-1]\n", + " + KI * roll_rate_error_integral\n", + " + KD * roll_rate_error_derivative\n", + " )\n", + "\n", + " # Return variables of interest to be saved in the observed_variables list\n", + " return (\n", + " time,\n", + " roll_control.roll_torque,\n", + " )\n", + "\n", + "\n", + "roll_control, roll_controller = HALCYON.add_roll_control(\n", + " controller_function=roll_controller_function,\n", + " sampling_rate=100,\n", + " max_roll_torque=10,\n", + " torque_rate_limit=1000,\n", + " return_controller=True,\n", + ")\n", + "roll_control.info()\n", + "roll_controller.info()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def throttle_controller_function(\n", + " time,\n", + " sampling_rate,\n", + " state,\n", + " state_history,\n", + " observed_variables,\n", + " throttle_control,\n", + " sensors,\n", + "):\n", + " # state = [x, y, z, vx, vy, vz, e0, e1, e2, e3, wx, wy, wz]\n", + "\n", + " # Throttle command: 0.5 Hz sinusoid, centered at 0.8, amplitude 0.2\n", + " throttle_command = 0.8 + 0.2 * np.sin(2 * np.pi * 0.5 * time)\n", + " throttle_control.throttle = throttle_command\n", + " throttle_actual = throttle_control.throttle\n", + "\n", + " # Return variables of interest to be saved in the observed_variables list\n", + " return (\n", + " time,\n", + " throttle_command,\n", + " throttle_actual,\n", + " )\n", + "\n", + "\n", + "throttle_obj, throttle_controller = HALCYON.add_throttle_control(\n", + " controller_function=throttle_controller_function,\n", + " sampling_rate=100,\n", + " throttle_range=(0.0, 1.0),\n", + " throttle_rate_limit=100,\n", + " throttle_time_constant=0.5,\n", + " return_controller=True,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Flight Simulation Data" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "test_flight = Flight(\n", + " rocket=HALCYON,\n", + " environment=env,\n", + " inclination=85,\n", + " heading=90,\n", + " rail_length=12,\n", + " time_overshoot=False,\n", + " terminate_on_apogee=True,\n", + " verbose=False,\n", + ")\n", + "# test_flight.plots.attitude_data()\n", + "# ===== Plot commanded throttle vs filtered actuator output =====\n", + "obs = np.array(throttle_controller.observed_variables)\n", + "\n", + "t_cmd = obs[:, 0]\n", + "th_cmd = obs[:, 1]\n", + "th_act = obs[:, 2]\n", + "\n", + "plt.figure()\n", + "plt.plot(t_cmd, th_cmd, label=\"Throttle command\")\n", + "plt.plot(t_cmd, th_act, label=\"Filtered throttle\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Throttle\")\n", + "plt.title(\"Throttle Command vs Actuator Output\")\n", + "plt.legend()\n", + "plt.grid()\n", + "plt.show()\n", + "\n", + "# ===== Then check thrust response =====\n", + "plt.figure()\n", + "plt.plot(\n", + " test_flight.net_thrust[:, 0], test_flight.net_thrust[:, 1], label=\"Effective Thrust\"\n", + ")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Thrust (N)\")\n", + "plt.title(\"Effective Thrust\")\n", + "plt.legend()\n", + "plt.grid()\n", + "plt.show()\n", + "\n", + "time1, ax, ay, az = zip(*accelerometer_clean.measured_data)\n", + "time2, gx, gy, gz = zip(*gyro_clean.measured_data)\n", + "\n", + "plt.plot(time2, gx, label=\"Gyroscope X\")\n", + "plt.plot(time2, gy, label=\"Gyroscope Y\")\n", + "plt.plot(time2, gz, label=\"Gyroscope Z\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Angular Velocity (rad/s)\")\n", + "plt.legend()\n", + "plt.grid()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "test_flight.prints.apogee_conditions()\n", + "test_flight.plots.trajectory_3d()\n", + "test_flight.plots.attitude_data()\n", + "\n", + "test_flight.plots.linear_kinematics_data()\n", + "test_flight.plots.angular_kinematics_data()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": ".venv (3.14.3)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.14.3" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/rocketpy/prints/roll_actuator_prints.py b/rocketpy/prints/roll_actuator_prints.py new file mode 100644 index 000000000..d0026b8d0 --- /dev/null +++ b/rocketpy/prints/roll_actuator_prints.py @@ -0,0 +1,35 @@ +class _RollActuatorPrints: + """Class that contains all roll actuator prints.""" + + def __init__(self, roll_actuator): + """Initializes _RollActuatorPrints class + + Parameters + ---------- + roll_actuator: rocketpy.RollActuator + Instance of the RollActuator class. + + Returns + ------- + None + """ + self.roll_actuator = roll_actuator + + def basics(self): + """Prints information of the roll actuator.""" + print(f"Information of {self.roll_actuator.name}:") + print("----------------------------------") + print(f"Torque demand rate: {self.roll_actuator.demand_rate} Hz") + print( + f"Torque range: {self.roll_actuator.actuator_range[0]:.2f} to {self.roll_actuator.actuator_range[1]:.2f} N·m" + ) + print(f"Torque rate limit: {self.roll_actuator.actuator_rate_limit} N·m/s") + print( + f"Torque clamping: {'Enabled' if self.roll_actuator.clamp else 'Disabled'}" + ) + print(f"Torque time constant: {self.roll_actuator.actuator_time_constant} sec") + print(f"Current roll torque: {self.roll_actuator.roll_torque:.2f} N·m") + + def all(self): + """Prints all information of the roll actuator.""" + self.basics() diff --git a/rocketpy/prints/throttle_actuator_prints.py b/rocketpy/prints/throttle_actuator_prints.py new file mode 100644 index 000000000..3d74d6cb3 --- /dev/null +++ b/rocketpy/prints/throttle_actuator_prints.py @@ -0,0 +1,35 @@ +class _ThrottleActuatorPrints: + def __init__(self, throttle_actuator): + """Initializes _ThrottleActuatorPrints class + + Parameters + ---------- + throttle_actuator: rocketpy.ThrottleActuator + Instance of the ThrottleActuator class. + + Returns + ------- + None + """ + self.throttle_actuator = throttle_actuator + + def basics(self): + """Prints information of the throttle actuator.""" + print(f"Information of {self.throttle_actuator.name}:") + print("----------------------------------") + print(f"Throttle demand rate: {self.throttle_actuator.demand_rate} Hz") + print( + f"Throttle range: {self.throttle_actuator.actuator_range[0]:.2f} to {self.throttle_actuator.actuator_range[1]:.2f}" + ) + print(f"Throttle rate limit: {self.throttle_actuator.actuator_rate_limit}") + print( + f"Throttle Clamping: {'Enabled' if self.throttle_actuator.clamp else 'Disabled'}" + ) + print( + f"Throttle time constant: {self.throttle_actuator.actuator_time_constant} sec" + ) + print(f"Current throttle: {self.throttle_actuator.throttle:.2f}") + + def all(self): + """Prints all information of the throttle actuator.""" + self.basics() diff --git a/rocketpy/prints/thrust_vector_actuator_prints.py b/rocketpy/prints/thrust_vector_actuator_prints.py new file mode 100644 index 000000000..a99f7989e --- /dev/null +++ b/rocketpy/prints/thrust_vector_actuator_prints.py @@ -0,0 +1,41 @@ +class _ThrustVectorActuatorPrints: + """Class that contains all thrust vector actuator prints.""" + + def __init__(self, thrust_vector_actuator): + """Initializes _ThrustVectorActuatorPrints class + + Parameters + ---------- + thrust_vector_actuator: rocketpy.ThrustVectorActuator + Instance of the thrust vector actuator class. + + Returns + ------- + None + """ + self.thrust_vector_actuator = thrust_vector_actuator + + def basics(self): + """Prints information of the thrust vector actuator.""" + print(f"Information of {self.thrust_vector_actuator.name}:") + print("----------------------------------") + print(f"Gimbal demand rate: {self.thrust_vector_actuator.demand_rate} Hz") + print( + f"Gimbal range: {self.thrust_vector_actuator.actuator_range[0]:.2f} to {self.thrust_vector_actuator.actuator_range[1]:.2f} deg" + ) + print( + f"Gimbal rate limit: {self.thrust_vector_actuator.actuator_rate_limit} deg/sec" + ) + print( + f"Gimbal clamping: {'Enabled' if self.thrust_vector_actuator.clamp else 'Disabled'}" + ) + print( + f"Gimbal time constant: {self.thrust_vector_actuator.actuator_time_constant} sec" + ) + print( + f"Current gimbal angle: {self.thrust_vector_actuator.gimbal_angle:.2f} deg" + ) + + def all(self): + """Prints all information of the thrust vector actuator.""" + self.basics() diff --git a/rocketpy/rocket/actuator/__init__.py b/rocketpy/rocket/actuator/__init__.py new file mode 100644 index 000000000..044590c5b --- /dev/null +++ b/rocketpy/rocket/actuator/__init__.py @@ -0,0 +1,4 @@ +from rocketpy.rocket.actuator.actuator import Actuator +from rocketpy.rocket.actuator.roll import RollActuator +from rocketpy.rocket.actuator.throttle import ThrottleActuator +from rocketpy.rocket.actuator.thrust_vector import ThrustVectorActuator diff --git a/rocketpy/rocket/actuator/actuator.py b/rocketpy/rocket/actuator/actuator.py new file mode 100644 index 000000000..eeb4cda9d --- /dev/null +++ b/rocketpy/rocket/actuator/actuator.py @@ -0,0 +1,164 @@ +from abc import ABC, abstractmethod + +import numpy as np + + +class Actuator(ABC): + """Abstract class used to define actuators. + + Actuators are used to model the dynamics of control systems such as + throttle, thrust vector, and roll control. They can be used to simulate the response of + the control system to changes in throttle, thrust vector, or roll torque commands.""" + + def __init__( + self, + name, + demand_rate=None, + actuator_range=(-np.inf, np.inf), + actuator_rate_limit=None, + clamp=True, + actuator_initial_output=0.0, + actuator_time_constant=None, + ): + """Initializes the Actuator class. + + Parameters + ---------- + name : str + Name of the actuator. + demand_rate : float, optional + Demand rate (Hz) of the actuator. Default is None for continuous-time actuator. + actuator_range : tuple, optional + Range of the actuator output. Default is (-np.Inf, np.Inf). + actuator_rate_limit : float, optional + Rate limit of the actuator per second. Default is None. + clamp : bool, optional + Whether to clamp the actuator output. Default is True. + actuator_initial_output : float, optional + Initial output of the actuator. Default is 0.0. + actuator_time_constant : float, optional + Time constant of the actuator, implemented as a discrete IIR filter. Default is None. + + Returns + ------- + None + """ + + self.name = name + + assert demand_rate > 0 or demand_rate is None, ( + "demand_rate must be positive or None." + ) + self.demand_rate = demand_rate + + assert actuator_range[0] <= actuator_range[1], ( + "actuator_range[0] must be <= actuator_range[1]." + ) + self.actuator_range = actuator_range + + assert actuator_rate_limit is None or actuator_rate_limit >= 0, ( + "actuator_rate_limit must be non-negative or None." + ) + self.actuator_rate_limit = actuator_rate_limit + + self.clamp = clamp + + assert actuator_time_constant is None or actuator_time_constant >= 0, ( + "actuator_time_constant must be non-negative or None." + ) + self.actuator_time_constant = actuator_time_constant + self._update_iir_coefficients() + + self.actuator_initial_output = actuator_initial_output + self._actuator_output = actuator_initial_output + + def _update_iir_coefficients(self): + """Updates the IIR filter coefficient based on time constant and + demand rate. Uses first-order discrete-time system: + y[n] = alpha * u[n] + (1 - alpha) * y[n-1] + where alpha = Ts / (tau + Ts) + """ + + if self.actuator_time_constant is not None and self.actuator_time_constant > 0: + if self.demand_rate is not None: + demand_period = 1.0 / self.demand_rate + self._alpha = demand_period / ( + self.actuator_time_constant + demand_period + ) + else: + print( + f"Warning: Actuator time constant currently only implemented on discrete controllers. '{self.name}' dynamics not applied." + ) + self._alpha = 1.0 # No filtering, direct pass-through + else: + self._alpha = 1.0 # No filtering, direct pass-through + + @property + def actuator_output(self): + return self._actuator_output + + @actuator_output.setter + def actuator_output(self, value): + """Sets the actuator output with optional clamping or warning. + + Parameters + ---------- + value : float + Desired actuator output. + + Returns + ------- + None + """ + # Apply first-order IIR actuator dynamics + value = self._alpha * value + (1 - self._alpha) * self._actuator_output + + # Apply rate limit if specified + if self.actuator_rate_limit is not None: + if self.demand_rate is not None: + max_change = self.actuator_rate_limit / self.demand_rate + change = value - self._actuator_output + if abs(change) > max_change: + value = self._actuator_output + np.sign(change) * max_change + print( + f"Warning: Actuator '{self.name}' output change {change:.3f} exceeds rate limit of {max_change:.3f} per time step." + ) + else: + print( + f"Warning: Actuator rate limit currently only implemented for discrete controllers. '{self.name}' rate limit not applied." + ) + + # Apply range limits if specified + if self.clamp: + value = np.clip(value, self.actuator_range[0], self.actuator_range[1]) + else: + if value < self.actuator_range[0] or value > self.actuator_range[1]: + print( + f"Warning: Actuator '{self.name}' output {value:.3f} exceeds range limits {self.actuator_range}." + ) + + self._actuator_output = value + + def _reset(self): + """Resets the actuator to its initial state. This method + is called at the beginning of each simulation to ensure the actuator + is in the correct state.""" + self._actuator_output = self.actuator_initial_output + + @abstractmethod + def info(self): + """Prints summarized information of the actuator. + + Returns + ------- + None + """ + + @abstractmethod + def all_info(self): + """Prints all information of the actuator. + + Returns + ------- + None + """ diff --git a/rocketpy/rocket/actuator/roll.py b/rocketpy/rocket/actuator/roll.py new file mode 100644 index 000000000..20127d581 --- /dev/null +++ b/rocketpy/rocket/actuator/roll.py @@ -0,0 +1,139 @@ +from rocketpy.prints.roll_actuator_prints import _RollActuatorPrints + +from .actuator import Actuator + + +class RollActuator(Actuator): + """Roll actuator class as a controllable component. Inherits from Actuator. + + This class represents a roll actuator that applies roll torque around the rocket's Z-axis. + Magic/hand-of-god roll torque is assumed. The roll torque is positive for counter-clockwise rotation when + viewed from the nose of the rocket. + This actuator is typically controlled by a controller function similar to air brakes + and is used by ``Flight`` to model roll control system. + + Attributes + ---------- + name : str + Name of the roll actuator. + demand_rate : float + Demand rate of the roll control in Hz. None indicates a continuous-time actuator. + actuator_range : float + Range of the roll control in N·m. + actuator_rate_limit : float + Rate limit of the roll control in N·m/s. The roll torque change is limited to this rate. + clamp : bool, optional + If True, roll torque is clamped to actuator_range. + If False, a warning is issued when roll torque exceeds the range. + actuator_time_constant : float + Time constant for the roll torque actuator dynamics (first-order IIR filter) in seconds. + actuator_initial_output : float + Initial roll torque in N·m. + roll_torque : float + Current roll torque output magnitude in N·m (Newton-meters). + Positive values indicate counter-clockwise rotation when viewed + from the nose of the rocket. + """ + + def __init__( + self, + name="Roll Control", + demand_rate=100, + max_roll_torque=0, + torque_rate_limit=None, + clamp=True, + initial_roll_torque=0.0, + roll_torque_time_constant=None, + ): + """Initializes the RollControl class. + + Parameters + ---------- + name : str, optional + Name of the roll actuator. Default is "Roll Control". + demand_rate : int, optional + Demand rate of the roll actuator in Hz. Default is 100 Hz. + None indicates a continuous-time actuator. + max_roll_torque : float, int + Maximum roll torque magnitude in N·m. Must be non-negative. + Default is 0 (no roll control). + torque_rate_limit : float, int + Maximum roll torque rate in N·m/s. Roll torque is limited to this + rate. Must be non-negative. Default is None (no limit). demand_rate must be specified if torque_rate_limit is not None. + clamp : bool, optional + If True, the simulation will clamp roll torque to the range + [-max_roll_torque, max_roll_torque] if it exceeds this range. + If False, the simulation will issue a warning if roll torque + exceeds the maximum value. Default is True. + initial_roll_torque : float, optional + Initial roll torque in N·m. Default is 0.0 (no torque). + roll_torque_time_constant : float, optional + Time constant for the roll torque actuator dynamics (first-order IIR + filter) in seconds. If None, no actuator dynamics are applied. + Must be non-negative. Default is None. demand_rate must be specified if roll_torque_time_constant is not None. + + Returns + ------- + None + """ + super().__init__( + name=name, + demand_rate=demand_rate, + actuator_range=(-max_roll_torque, max_roll_torque), + actuator_rate_limit=torque_rate_limit, + clamp=clamp, + actuator_initial_output=initial_roll_torque, + actuator_time_constant=roll_torque_time_constant, + ) + self.prints = _RollActuatorPrints(self) + + @property + def roll_torque(self): + """Returns the current roll torque in N·m.""" + return self.actuator_output + + @roll_torque.setter + def roll_torque(self, value): + """Sets the roll torque in N·m.""" + self.actuator_output = value + + def info(self): + """Prints summarized information of the roll control system. + + Returns + ------- + None + """ + self.prints.basics() + + def all_info(self): + """Prints all information of the roll control system. + + Returns + ------- + None + """ + self.info() + + def to_dict(self, **kwargs): # pylint: disable=unused-argument + return { + "name": self.name, + "demand_rate": self.demand_rate, + "max_roll_torque": self.actuator_range[1], + "torque_rate_limit": self.actuator_rate_limit, + "clamp": self.clamp, + "initial_roll_torque": self.actuator_initial_output, + "roll_torque_time_constant": self.actuator_time_constant, + } + + @classmethod + def from_dict(cls, data): + return cls( + name=data.get("name"), + demand_rate=data.get("demand_rate"), + max_roll_torque=data.get("max_roll_torque"), + torque_rate_limit=data.get("torque_rate_limit"), + clamp=data.get("clamp"), + initial_roll_torque=data.get("initial_roll_torque"), + roll_torque_time_constant=data.get("roll_torque_time_constant"), + ) diff --git a/rocketpy/rocket/actuator/throttle.py b/rocketpy/rocket/actuator/throttle.py new file mode 100644 index 000000000..34c6016fd --- /dev/null +++ b/rocketpy/rocket/actuator/throttle.py @@ -0,0 +1,143 @@ +from rocketpy.prints.throttle_actuator_prints import _ThrottleActuatorPrints + +from .actuator import Actuator + + +class ThrottleActuator(Actuator): + """Throttle actuator class as a controllable component. Inherits from Actuator. + + This class represents a throttle actuator that applies throttle around the rocket's engine. + The throttle is typically controlled by a controller function similar to air brakes + and is used by ``Flight`` to model throttle control system. + + This class represents a throttle actuator that throttles the rocket's engine. + The throttle is the fraction of the maximum thrust produced by the engine, ranging from 0 (no thrust) to 1 (full thrust). + This actuator is typically controlled by a controller function similar to air brakes + and is used by ``Flight`` to model throttle control system. + + Attributes + ---------- + name : str + Name of the throttle actuator. + demand_rate : float + Demand rate of the throttle actuator in Hz. None indicates a continuous-time actuator. + actuator_range : float + Range of the throttle actuator. + actuator_rate_limit : float + Rate limit of the throttle actuator in 1/s. The throttle change is limited to this rate. + clamp : bool, optional + If True, throttle is clamped to actuator_range. + If False, a warning is issued when throttle exceeds the range. + actuator_time_constant : float + Time constant for the throttle actuator dynamics (first-order IIR filter) in seconds. + actuator_initial_output : float + Initial throttle value. + throttle : float + Current throttle value. The throttle is the fraction of the maximum thrust produced by the engine + ranging from 0 (no thrust) to 1 (full thrust). + """ + + def __init__( + self, + name="Throttle Control", + demand_rate=100, + throttle_range=(0, 1), + throttle_rate_limit=None, + clamp=True, + initial_throttle=1.0, + throttle_time_constant=None, + ): + """Initializes the ThrottleActuator class. + + Parameters + ---------- + name : str, optional + Name of the throttle actuator. Default is "Throttle Control". + demand_rate : int, optional + Demand rate of the throttle actuator in Hz. Default is 100 Hz. + None indicates a continuous-time actuator. + throttle_range : tuple, optional + A tuple containing the minimum and maximum throttle values. Default is (0.0, 1.0). + throttle_rate_limit : float, int + Maximum throttle rate in 1/s. Throttle is limited to this + rate. Must be non-negative. Default is None (no limit). demand_rate must be specified if throttle_rate_limit is not None. + clamp : bool, optional + If True, the simulation will clamp throttle to the range + [throttle_range[0], throttle_range[1]] if it exceeds this range. + If False, the simulation will issue a warning if throttle + exceeds the maximum value. Default is True. + initial_throttle : float, optional + Initial throttle value. Default is 1.0 (full thrust). + throttle_time_constant : float, optional + Time constant for the throttle actuator dynamics (first-order IIR + filter) in seconds. If None, no actuator dynamics are applied. + Must be non-negative. Default is None. demand_rate must be specified if throttle_time_constant is not None. + + Returns + ------- + None + """ + super().__init__( + name=name, + demand_rate=demand_rate, + actuator_range=throttle_range, + actuator_rate_limit=throttle_rate_limit, + clamp=clamp, + actuator_initial_output=initial_throttle, + actuator_time_constant=throttle_time_constant, + ) + self.prints = _ThrottleActuatorPrints(self) + + @property + def throttle(self): + """Returns the current throttle value.""" + return self.actuator_output + + @throttle.setter + def throttle(self, value): + """Sets the throttle value.""" + self.actuator_output = value + + def info(self): + """Prints summarized information of the throttle control system. + + Returns + ------- + None + """ + self.prints.basics() + + def all_info(self): + """Prints all information of the throttle control system. + + Returns + ------- + None + """ + self.info() + + def to_dict(self, **kwargs): # pylint: disable=unused-argument + return { + "name": self.name, + "demand_rate": self.demand_rate, + "throttle_range": self.actuator_range, + "throttle_rate_limit": self.actuator_rate_limit, + "clamp": self.clamp, + "initial_throttle": self.actuator_initial_output, + "throttle_time_constant": self.actuator_time_constant, + } + + @classmethod + def from_dict(cls, data): + return cls( + name=data.get("name"), + demand_rate=data.get("demand_rate"), + throttle_range=( + data.get("throttle_range")[0], + data.get("throttle_range")[1], + ), + throttle_rate_limit=data.get("throttle_rate_limit"), + clamp=data.get("clamp"), + initial_throttle=data.get("initial_throttle"), + throttle_time_constant=data.get("throttle_time_constant"), + ) diff --git a/rocketpy/rocket/actuator/thrust_vector.py b/rocketpy/rocket/actuator/thrust_vector.py new file mode 100644 index 000000000..43808b4e7 --- /dev/null +++ b/rocketpy/rocket/actuator/thrust_vector.py @@ -0,0 +1,269 @@ +from rocketpy.prints.thrust_vector_actuator_prints import _ThrustVectorActuatorPrints + +from .actuator import Actuator + + +class ThrustVectorActuator(Actuator): + """Thrust vector actuator class as a controllable component. Inherits from Actuator. + + This class represents a thrust vector actuator that deflects the direction + of the thrust vector through gimbal angle. + This actuator is typically controlled by a controller function similar to air brakes + and is used by ``Flight`` to model thrust vector control (TVC). + + Attributes + ---------- + name : str + Name of the thrust vector actuator. + demand_rate : float + Demand rate of the thrust vector actuator in Hz. None indicates a continuous-time actuator. + actuator_range : float + Range of the thrust vector actuator in deg. + actuator_rate_limit : float + Rate limit of the thrust vector actuator in deg/sec. The thrust vector change is limited to this rate. + clamp : bool, optional + If True, thrust gimbal angle is clamped to actuator_range. + If False, a warning is issued when thrust vector exceeds the range. + actuator_time_constant : float + Time constant for the thrust vector actuator dynamics (first-order IIR filter) in seconds. + actuator_initial_output : float + Initial thrust vector gimbal angle in deg. + gimbal_angle : float + Current thrust vector gimbal angle in deg. + + """ + + def __init__( + self, + name="Thrust Vector Control", + demand_rate=100, + max_gimbal_angle=10, + gimbal_rate_limit=None, + clamp=True, + initial_gimbal_angle=0.0, + gimbal_time_constant=None, + ): + """Initializes the thrust vector actuator class. + + Parameters + ---------- + name : str, optional + Name of the thrust vector actuator. Default is "Thrust Vector Control". + demand_rate : int, optional + Demand rate of the thrust vector actuator in Hz. Default is 100 Hz. + None indicates a continuous-time actuator. + max_gimbal_angle : float, int + Maximum gimbal angle in deg. Must be non-negative. + Default is 10 deg. + gimbal_rate_limit : float, int + Maximum gimbal rate in deg/sec. Must be non-negative. + Default is None (no limit). demand_rate must be specified if gimbal_rate_limit is not None. + clamp : bool, optional + If True, the simulation will clamp gimbal angle to the range + [-max_gimbal_angle, max_gimbal_angle] if it exceeds this range. + If False, the simulation will issue a warning if gimbal angle + exceeds the maximum value. Default is True. + initial_gimbal_angle : float, optional + Initial gimbal angle in deg. Default is 0.0 (no gimbal). + roll_torque_time_constant : float, optional + Time constant for the roll torque actuator dynamics (first-order IIR + filter) in seconds. If None, no actuator dynamics are applied. + Must be non-negative. Default is None. demand_rate must be specified if roll_torque_time_constant is not None. + + + Returns + ------- + None + """ + + super().__init__( + name=name, + demand_rate=demand_rate, + actuator_range=(-max_gimbal_angle, max_gimbal_angle), + actuator_rate_limit=gimbal_rate_limit, + clamp=clamp, + actuator_initial_output=initial_gimbal_angle, + actuator_time_constant=gimbal_time_constant, + ) + self.prints = _ThrustVectorActuatorPrints(self) + + @property + def gimbal_angle(self): + """Returns the current gimbal angle in deg.""" + return self.actuator_output + + @gimbal_angle.setter + def gimbal_angle(self, value): + """Sets the gimbal angle in deg.""" + self.actuator_output = value + + def info(self): + """Prints summarized information of the thrust vector actuator. + + Returns + ------- + None + """ + self.prints.basics() + + def all_info(self): + """Prints all information of the thrust vector actuator. + + Returns + ------- + None + """ + self.info() + + def to_dict(self, **kwargs): # pylint: disable=unused-argument + return { + "name": self.name, + "demand_rate": self.demand_rate, + "max_gimbal_angle": self.actuator_range[1], + "gimbal_rate_limit": self.actuator_rate_limit, + "clamp": self.clamp, + "initial_gimbal_angle": self.actuator_initial_output, + "gimbal_time_constant": self.actuator_time_constant, + } + + @classmethod + def from_dict(cls, data): + return cls( + name=data.get("name"), + demand_rate=data.get("demand_rate"), + max_gimbal_angle=data.get("max_gimbal_angle"), + gimbal_rate_limit=data.get("gimbal_rate_limit"), + clamp=data.get("clamp"), + initial_gimbal_angle=data.get("initial_gimbal_angle"), + gimbal_time_constant=data.get("gimbal_time_constant"), + ) + + +class ThrustVectorActuator2D: + """Dual-axis thrust vector actuator class as a controllable component. + + This class represents dual-axis thrust vector actuator that deflects the direction + of the thrust vector through dual-axis gimbal angles. + This actuator is typically controlled by a controller function similar to air brakes + and is used by ``Flight`` to model thrust vector control (TVC). + + Attributes + ---------- + ThrustVectorActuator2D.name : str + Name of the dual-axis thrust vector actuator. + ThrustVectorActuator2D.demand_rate : float + Demand rate of the dual-axis thrust vector actuator in Hz. None indicates a continuous-time actuator. + ThrustVectorActuator2D.actuator_range : float + Range of the dual-axis thrust vector actuator in deg. + ThrustVectorActuator2D.actuator_rate_limit : float + Rate limit of the dual-axis thrust vector actuator in deg/sec. The thrust vector change is limited to this rate. + ThrustVectorActuator2D.clamp : bool, optional + If True, thrust vector gimbal angles are clamped to actuator_range. + If False, a warning is issued when thrust vector exceeds the max value. + ThrustVectorActuator2D.actuator_time_constant : float + Time constant for the thrust vector actuator dynamics (first-order IIR filter) in seconds. + ThrustVectorActuator2D.actuator_initial_output : float + Initial thrust gimbal angles in deg. + ThrustVectorActuator2D.actuator_output : float + Current thrust vector gimble angles in deg. + + """ + + def __init__( + self, + name="Thrust Vector Control", + demand_rate=100, + max_gimbal_angle=10, + gimbal_rate_limit=None, + clamp=True, + initial_gimbal_angle=0.0, + gimbal_time_constant=None, + ): + """Initializes the dual-axis thrust vector actuator class. + + Parameters + ---------- + name : str, optional + Name of the dual-axis thrust vector actuator. Default is "Thrust Vector Control". + demand_rate : int, optional + Demand rate of the dual-axis thrust vector actuator in Hz. Default is 100 Hz. + None indicates a continuous-time actuator. + max_gimbal_angle : float, int + Maximum gimbal angle in deg. Must be non-negative. + Default is 10 deg. + gimbal_rate_limit : float, int + Maximum gimbal rate in deg/sec. Must be non-negative. + Default is None (no limit). demand_rate must be specified if gimbal_rate_limit is not None. + clamp : bool, optional + If True, the simulation will clamp gimbal angles to the range + [-max_gimbal_angle, max_gimbal_angle] if it exceeds this range. + If False, the simulation will issue a warning if gimbal angle + exceeds the maximum value. Default is True. + initial_gimbal_angle : float, optional + Initial gimbal angle in deg. Default is 0.0 (no gimbal). + roll_torque_time_constant : float, optional + Time constant for the roll torque actuator dynamics (first-order IIR + filter) in seconds. If None, no actuator dynamics are applied. + Must be non-negative. Default is None. demand_rate must be specified if roll_torque_time_constant is not None. + + + Returns + ------- + None + """ + + self.x = ThrustVectorActuator( + name=name + " X-axis", + demand_rate=demand_rate, + max_gimbal_angle=max_gimbal_angle, + gimbal_rate_limit=gimbal_rate_limit, + clamp=clamp, + initial_gimbal_angle=initial_gimbal_angle, + gimbal_time_constant=gimbal_time_constant, + ) + self.y = ThrustVectorActuator( + name=name + " Y-axis", + demand_rate=demand_rate, + max_gimbal_angle=max_gimbal_angle, + gimbal_rate_limit=gimbal_rate_limit, + clamp=clamp, + initial_gimbal_angle=initial_gimbal_angle, + gimbal_time_constant=gimbal_time_constant, + ) + + @property + def gimbal_angle_x(self): + """Returns the current gimbal angle around the y-axis (yaw).""" + return self.x.gimbal_angle + + @gimbal_angle_x.setter + def gimbal_angle_x(self, value): + """Sets the gimbal angle in deg.""" + self.x.gimbal_angle = value + + @property + def gimbal_angle_y(self): + """Returns the current gimbal angle around the y-axis (yaw).""" + return self.y.gimbal_angle + + @gimbal_angle_y.setter + def gimbal_angle_y(self, value): + """Sets the gimbal angle in deg.""" + self.y.gimbal_angle = value + + @property + def gimbal_angles(self): + """Returns a tuple of the current gimbal angles (x, y) in degrees.""" + return (self.gimbal_angle_x, self.gimbal_angle_y) + + @gimbal_angles.setter + def gimbal_angles(self, value): + """Sets both gimbal angles from a tuple. + + Parameters + ---------- + value : tuple + Tuple of (gimbal_angle_x, gimbal_angle_y) in degrees. + """ + self.gimbal_angle_x = value[0] + self.gimbal_angle_y = value[1] diff --git a/rocketpy/rocket/rocket.py b/rocketpy/rocket/rocket.py index 84655b6d7..f850f65d7 100644 --- a/rocketpy/rocket/rocket.py +++ b/rocketpy/rocket/rocket.py @@ -12,6 +12,9 @@ from rocketpy.motors.empty_motor import EmptyMotor from rocketpy.plots.rocket_plots import _RocketPlots from rocketpy.prints.rocket_prints import _RocketPrints +from rocketpy.rocket.actuator.roll import RollActuator +from rocketpy.rocket.actuator.throttle import ThrottleActuator +from rocketpy.rocket.actuator.thrust_vector import ThrustVectorActuator2D from rocketpy.rocket.aero_surface import ( AirBrakes, EllipticalFins, @@ -1846,6 +1849,407 @@ def add_air_brakes( else: return air_brakes + def add_thrust_vector_control( + self, + controller_function, + sampling_rate, + max_gimbal_angle, + gimbal_rate_limit=None, + clamp=True, + initial_gimbal_angle=0.0, + gimbal_time_constant=None, + initial_observed_variables=None, + return_controller=False, + name="Thrust Vector Control", + controller_name="Thrust Vector Controller", + ): + """Creates a new thrust vector control (TVC) system, storing its + parameters such as gimbal angle maximum controller function, and + sampling rate. + + Parameters + ---------- + 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. + max_gimbal_angle : int, float + Maximum gimbal angle in degrees. Both x and y gimbal + angles are clamped to this range if clamp is True. Must be + non-negative. + gimbal_rate_limit : int, float + Maximum gimbal rate in degrees per second. Both x and y gimbal + angles are limited to this rate of change. Default is None, no rate limit. + clamp : bool, optional + If True, the simulation will clamp gimbal angles to the range + [-max_gimbal_angle, max_gimbal_angle]. If False, a warning is + issued when gimbal angles exceed the range. Default is True. + initial_gimbal_angle : int, float, tuple, list + The initial gimbal angle in degrees. If a single value is provided, + it is used for both x and y gimbal angles. If a tuple or list is + provided, the first element is used for the x-axis and the second + for the y-axis. Default is 0.0. + gimbal_time_constant : float, optional + Time constant for the gimbal dynamics in seconds. If None, no + gimbal dynamics are applied. Default is None. + 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 + thrust_vector_control system name. Has no impact in simulation, as it is only used to + display data in a more organized matter. Default is "Thrust Vector Control". + 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 "Thrust Vector Controller". + + Returns + ------- + thrust_vector_control : ThrustVectorActuator2D + ThrustVectorActuator2D object created. + controller : Controller, optional + Controller object created (only if return_controller is True). + """ + if hasattr(self, "thrust_vector_control"): + # pylint: disable=access-member-before-definition + print( + "Only one thrust_vector_control per rocket is currently supported. " + + "Overwriting previous thrust_vector_control and controllers." + ) + self._controllers = [ + controller + for controller in self._controllers + if not isinstance( + controller.interactive_objects, ThrustVectorActuator2D + ) + ] + + thrust_vector_control = ThrustVectorActuator2D( + name=name, + demand_rate=sampling_rate, + max_gimbal_angle=max_gimbal_angle, + gimbal_rate_limit=gimbal_rate_limit, + clamp=clamp, + initial_gimbal_angle=initial_gimbal_angle, + gimbal_time_constant=gimbal_time_constant, + ) + _controller = _Controller( + interactive_objects=thrust_vector_control, + controller_function=controller_function, + sampling_rate=sampling_rate, + initial_observed_variables=initial_observed_variables, + name=controller_name, + ) + self.thrust_vector_control = thrust_vector_control + self._add_controllers(_controller) + if return_controller: + return thrust_vector_control, _controller + else: + return thrust_vector_control + + def add_roll_control( + self, + controller_function, + sampling_rate, + max_roll_torque, + torque_rate_limit=None, + clamp=True, + initial_roll_torque=0.0, + roll_torque_time_constant=None, + initial_observed_variables=None, + return_controller=False, + name="Roll Control", + controller_name="Roll Controller", + ): + """Creates a new roll control system, storing its parameters such as + maximum roll torque, controller function, and sampling rate. + + Parameters + ---------- + controller_function : function, callable + An user-defined function responsible for controlling the roll control + 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 roll control + 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. + max_roll_torque : int, float + Maximum roll torque magnitude in N·m. Must be non-negative. + torque_rate_limit : int, float + Maximum rate of change of roll torque in N·m/s. Must be non-negative. + Default is None, which means no rate limit. + clamp : bool, optional + If True, the simulation will clamp roll torque to the range + [-max_roll_torque, max_roll_torque]. If False, a warning is + issued when roll torque exceeds the range. Default is True. + Initial_roll_torque : int, float + Initial roll torque in N·m. Default is 0.0. + roll_torque_time_constant : float, optional + Time constant for the roll torque dynamics in seconds. Default is None, no dynamics are applied. + 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 + Roll control system name. Has no impact in simulation, as it is only + used to display data in a more organized matter. Default is + "Roll Control". + 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 + "Roll Controller". + + Returns + ------- + roll_control : RollControl + RollControl object created. + controller : Controller, optional + Controller object created (only if return_controller is True). + """ + if hasattr(self, "roll_control"): + # pylint: disable=access-member-before-definition + print( + "Only one roll control per rocket is currently supported. " + + "Overwriting previous roll control and controllers." + ) + self._controllers = [ + controller + for controller in self._controllers + if not isinstance(controller.interactive_objects, RollActuator) + ] + + roll_control = RollActuator( + name=name, + demand_rate=sampling_rate, + max_roll_torque=max_roll_torque, + torque_rate_limit=torque_rate_limit, + clamp=clamp, + initial_roll_torque=initial_roll_torque, + roll_torque_time_constant=roll_torque_time_constant, + ) + _controller = _Controller( + interactive_objects=roll_control, + controller_function=controller_function, + sampling_rate=sampling_rate, + initial_observed_variables=initial_observed_variables, + name=controller_name, + ) + self.roll_control = roll_control + self._add_controllers(_controller) + if return_controller: + return roll_control, _controller + else: + return roll_control + + def add_throttle_control( + self, + controller_function, + sampling_rate, + throttle_range=(0, 1), + throttle_rate_limit=0, + clamp=True, + initial_throttle=1.0, + throttle_time_constant=None, + initial_observed_variables=None, + return_controller=False, + name="Throttle Control", + controller_name="Throttle Controller", + ): + """Creates a new throttle control system, storing its parameters such as + throttle limits, controller function, and sampling rate. + + Parameters + ---------- + controller_function : function, callable + An user-defined function responsible for controlling the throttle + 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 throttle + control 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. + throttle_range : tuple, optional + A tuple containing the minimum and maximum throttle values. Must be in the range [0, 1]. Default is (0.0, 1.0). + throttle_rate_limit : float, optional + Maximum throttle rate in 1/s. Throttle is limited to this rate. + Must be non-negative. Default is None, no throttle change limit. + clamp : bool, optional + If True, the simulation will clamp throttle values to the range + [throttle_range[0], throttle_range[1]]. If False, a warning is issued when + throttle values exceed the range. Default is True. + initial_throttle : float, optional + Initial throttle value at the start of the simulation. Must be within + the range [throttle_range[0], throttle_range[1]]. Default is 1.0. + throttle_time_constant : float, optional + Time constant for the throttle actuator dynamics in seconds. + If None, no actuator dynamics are applied. + 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 + Throttle control system name. Has no impact in simulation, as it is + only used to display data in a more organized matter. Default is + "Throttle Control". + 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 + "Throttle Controller". + + Returns + ------- + throttle_control : ThrottleControl + ThrottleControl object created. + controller : Controller, optional + Controller object created (only if return_controller is True). + """ + + if hasattr(self, "throttle_control"): + print( + "Only one throttle control per rocket is currently supported. " + + "Overwriting previous throttle control and controllers." + ) + self._controllers = [ + controller + for controller in self._controllers + if not isinstance(controller.interactive_objects, ThrottleActuator) + ] + + throttle_control = ThrottleActuator( + name=name, + demand_rate=sampling_rate, + throttle_range=throttle_range, + throttle_rate_limit=throttle_rate_limit, + clamp=clamp, + initial_throttle=initial_throttle, + throttle_time_constant=throttle_time_constant, + ) + + _controller = _Controller( + interactive_objects=throttle_control, + controller_function=controller_function, + sampling_rate=sampling_rate, + initial_observed_variables=initial_observed_variables, + name=controller_name, + ) + + self.throttle_control = throttle_control + self._add_controllers(_controller) + + if return_controller: + return throttle_control, _controller + else: + return throttle_control + def set_rail_buttons( self, upper_button_position, diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index 2293d9706..9a63af294 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -1578,10 +1578,16 @@ def __init_controllers(self): self._controllers = self.rocket._controllers[:] self.sensors = self.rocket.sensors.get_components() - # reset controllable object to initial state (only airbrakes for now) + # reset controllable objects to initial state (air brakes, thrust vector control, throttle control, and roll control) for air_brakes in self.rocket.air_brakes: air_brakes._reset() - + if hasattr(self.rocket, "thrust_vector_control"): + self.rocket.thrust_vector_control.x._reset() + self.rocket.thrust_vector_control.y._reset() + if hasattr(self.rocket, "roll_control"): + self.rocket.roll_control._reset() + if hasattr(self.rocket, "throttle_control"): + self.rocket.throttle_control._reset() self.sensor_data = {} for sensor in self.sensors: sensor._reset(self.rocket) # resets noise and measurement list @@ -1765,10 +1771,14 @@ def udot_rail1(self, t, u, post_processing=False): + self.rocket.motor.pressure_thrust(pressure), 0, ) + effective_thrust = net_thrust * getattr( + getattr(self.rocket, "throttle_control", None), "throttle", 1.0 + ) + rho = self.env.density.get_value_opt(z) R3 = -0.5 * rho * (free_stream_speed**2) * self.rocket.area * (drag_coeff) # Calculate Linear acceleration - a3 = (R3 + net_thrust) / total_mass_at_t - ( + a3 = (R3 + effective_thrust) / total_mass_at_t - ( e0**2 - e1**2 - e2**2 + e3**2 ) * self.env.gravity.get_value_opt(z) if a3 > 0: @@ -1863,9 +1873,47 @@ def u_dot(self, t, u, post_processing=False): # pylint: disable=too-many-locals + self.rocket.motor.pressure_thrust(pressure), 0, ) + # Throttle control + effective_thrust = net_thrust * getattr( + getattr(self.rocket, "throttle_control", None), "throttle", 1.0 + ) + + # 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 * net_thrust - M2 -= self.rocket.thrust_eccentricity_x * net_thrust + M1 += self.rocket.thrust_eccentricity_y * thrust3 + M2 -= self.rocket.thrust_eccentricity_x * thrust3 + else: # Motor stopped # Inertias @@ -1878,6 +1926,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 net_thrust = 0 # Retrieve important quantities @@ -2027,6 +2076,10 @@ def u_dot(self, t, u, post_processing=False): # pylint: disable=too-many-locals # Off center moment M3 += self.rocket.cp_eccentricity_x * R2 - self.rocket.cp_eccentricity_y * R1 + # Roll control moment + if hasattr(self.rocket, "roll_control"): + M3 += self.rocket.roll_control.roll_torque + # Calculate derivatives # Angular acceleration alpha1 = ( @@ -2106,7 +2159,7 @@ def u_dot(self, t, u, post_processing=False): # pylint: disable=too-many-locals + 2 * c * mass_flow_rate_at_t * omega1 ) / total_mass_at_t, - (R3 - b * propellant_mass_at_t * (alpha2 - omega1 * omega3) + net_thrust) + (R3 - b * propellant_mass_at_t * (alpha2 - omega1 * omega3) + thrust3) / total_mass_at_t, ] ax, ay, az = K @ Vector(L) @@ -2547,25 +2600,66 @@ def u_dot_generalized(self, t, u, post_processing=False): # pylint: disable=too M2 += N M3 += L + # Throttle control + effective_thrust = net_thrust * getattr( + getattr(self.rocket, "throttle_control", None), "throttle", 1.0 + ) + + # 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 * net_thrust + + self.rocket.thrust_eccentricity_y * thrust3 ) M2 -= ( self.rocket.cp_eccentricity_x * R3 - + self.rocket.thrust_eccentricity_x * net_thrust + + self.rocket.thrust_eccentricity_x * thrust3 ) M3 += self.rocket.cp_eccentricity_x * R2 - self.rocket.cp_eccentricity_y * R1 - weight_in_body_frame = Kt @ Vector( - [0, 0, -total_mass * self.env.gravity.get_value_opt(z)] + # Roll control moment + if hasattr(self.rocket, "roll_control"): + M3 += self.rocket.roll_control.roll_torque + + # Calculate weight with buoyancy: F_net = -total_mass * g + rho * V * g + gravity_accel = self.env.gravity.get_value_opt(z) + net_gravitational_force = ( + -total_mass * gravity_accel + rho * self.rocket.volume * gravity_accel ) + weight_in_body_frame = Kt @ Vector([0, 0, net_gravitational_force]) T00 = total_mass * r_CM T03 = 2 * total_mass_dot * (r_NOZ - r_CM) - 2 * total_mass * r_CM_dot T04 = ( - Vector([0, 0, net_thrust]) + Vector([0, 0, thrust3]) - total_mass * r_CM_ddot - 2 * total_mass_dot * r_CM_dot + total_mass_ddot * (r_NOZ - r_CM) @@ -2610,7 +2704,7 @@ def u_dot_generalized(self, t, u, post_processing=False): # pylint: disable=too if post_processing: self.__post_processed_variables.append( - [t, *v_dot, *w_dot, R1, R2, R3, M1, M2, M3, net_thrust] + [t, *v_dot, *w_dot, R1, R2, R3, M1, M2, M3, effective_thrust] ) return u_dot @@ -2903,19 +2997,19 @@ def R3(self): @funcify_method("Time (s)", "M1 (Nm)", "linear", "zero") def M1(self): """Aerodynamic moment acting along the x-axis of the rocket's body - frame as a function of time. Expressed in Newtons (N).""" + frame as a function of time. Expressed in Newtons (N-m).""" return self.__evaluate_post_process[:, [0, 10]] @funcify_method("Time (s)", "M2 (Nm)", "linear", "zero") def M2(self): """Aerodynamic moment acting along the y-axis of the rocket's body - frame as a function of time. Expressed in Newtons (N).""" + frame as a function of time. Expressed in Newtons (N-m).""" return self.__evaluate_post_process[:, [0, 11]] @funcify_method("Time (s)", "M3 (Nm)", "linear", "zero") def M3(self): - """Aerodynamic moment acting along the z-axis of the rocket's body - frame as a function of time. Expressed in Newtons (N).""" + """Aerodynamic moment and roll control moment acting along the z-axis + of the rocket's body frame as a function of time. Expressed in Newtons (N-m).""" return self.__evaluate_post_process[:, [0, 12]] @funcify_method("Time (s)", "Net Thrust (N)", "linear", "zero") @@ -3399,7 +3493,7 @@ def aerodynamic_bending_moment(self): @funcify_method("Time (s)", "Aerodynamic Spin Moment (Nm)", "spline", "zero") def aerodynamic_spin_moment(self): - """Aerodynamic spin moment as a Function of time.""" + """Aerodynamic spin moment and roll control moment as a Function of time.""" return self.M3 # Energy diff --git a/tests/unit/rocket/test_actuators.py b/tests/unit/rocket/test_actuators.py new file mode 100644 index 000000000..97933ac67 --- /dev/null +++ b/tests/unit/rocket/test_actuators.py @@ -0,0 +1,412 @@ +import pytest + +from rocketpy.rocket.actuator.roll import RollActuator +from rocketpy.rocket.actuator.throttle import ThrottleActuator +from rocketpy.rocket.actuator.thrust_vector import ( + ThrustVectorActuator, + ThrustVectorActuator2D, +) + + +class TestRollActuator: + """Test suite for RollActuator class.""" + + def test_initialization_defaults(self): + """Test RollActuator initialization with default parameters.""" + actuator = RollActuator() + assert actuator.name == "Roll Control" + assert actuator.demand_rate == 100 + assert actuator.actuator_range == (0, 0) + assert actuator.roll_torque == 0.0 + assert actuator.actuator_rate_limit is None + assert actuator.clamp is True + + def test_initialization_custom(self): + """Test RollActuator initialization with custom parameters.""" + actuator = RollActuator( + name="Custom Roll", + demand_rate=50, + max_roll_torque=10.0, + torque_rate_limit=5.0, + clamp=False, + initial_roll_torque=2.0, + ) + assert actuator.name == "Custom Roll" + assert actuator.demand_rate == 50 + assert actuator.actuator_range == (-10.0, 10.0) + assert actuator.roll_torque == 2.0 + assert actuator.actuator_rate_limit == 5.0 + assert actuator.clamp is False + + def test_roll_torque_property(self): + """Test roll torque getter and setter.""" + actuator = RollActuator(max_roll_torque=10.0) + actuator.roll_torque = 5.0 + assert actuator.roll_torque == 5.0 + + def test_roll_torque_clamping(self): + """Test roll torque clamping to range.""" + actuator = RollActuator(max_roll_torque=10.0, clamp=True) + actuator.actuator_output = 15.0 + assert actuator.roll_torque == 10.0 + actuator.actuator_output = -15.0 + assert actuator.roll_torque == -10.0 + + def test_to_dict(self): + """Test to_dict serialization.""" + actuator = RollActuator( + name="Test Roll", + max_roll_torque=5.0, + torque_rate_limit=2.0, + initial_roll_torque=1.0, + ) + data = actuator.to_dict() + assert data["name"] == "Test Roll" + assert data["max_roll_torque"] == 5.0 + assert data["torque_rate_limit"] == 2.0 + assert data["initial_roll_torque"] == 1.0 + + def test_from_dict(self): + """Test from_dict deserialization.""" + data = { + "name": "Test Roll", + "demand_rate": 50, + "max_roll_torque": 5.0, + "torque_rate_limit": 2.0, + "clamp": True, + "initial_roll_torque": 1.0, + "roll_torque_time_constant": None, + } + actuator = RollActuator.from_dict(data) + assert actuator.name == "Test Roll" + assert actuator.demand_rate == 50 + assert actuator.roll_torque == 1.0 + + def test_reset(self): + """Test actuator reset functionality.""" + actuator = RollActuator(max_roll_torque=10.0, initial_roll_torque=2.0) + actuator.roll_torque = 5.0 + actuator._reset() + assert actuator.roll_torque == 2.0 + + def test_info_methods(self): + """Test info and all_info methods.""" + actuator = RollActuator() + # Just verify methods exist and don't raise exceptions + actuator.info() + actuator.all_info() + + +class TestThrottleActuator: + """Test suite for ThrottleActuator class.""" + + def test_initialization_defaults(self): + """Test ThrottleActuator initialization with default parameters.""" + actuator = ThrottleActuator() + assert actuator.name == "Throttle Control" + assert actuator.demand_rate == 100 + assert actuator.actuator_range == (0, 1) + assert actuator.throttle == 0.0 + + def test_initialization_custom(self): + """Test ThrottleActuator initialization with custom parameters.""" + actuator = ThrottleActuator( + name="Custom Throttle", + demand_rate=50, + max_throttle=0.8, + throttle_rate_limit=0.1, + initial_throttle=0.5, + ) + assert actuator.name == "Custom Throttle" + assert actuator.actuator_range == (0, 0.8) + assert actuator.throttle == 0.5 + + def test_throttle_property(self): + """Test throttle getter and setter.""" + actuator = ThrottleActuator(max_throttle=1.0) + actuator.throttle = 0.5 + assert actuator.throttle == 0.5 + + def test_throttle_clamping(self): + """Test throttle clamping to range.""" + actuator = ThrottleActuator(max_throttle=1.0, clamp=True) + actuator.actuator_output = 1.5 + assert actuator.throttle == 1.0 + actuator.actuator_output = -0.5 + assert actuator.throttle == 0.0 + + def test_to_dict(self): + """Test to_dict serialization.""" + actuator = ThrottleActuator( + name="Test Throttle", + max_throttle=0.8, + throttle_rate_limit=0.1, + initial_throttle=0.3, + ) + data = actuator.to_dict() + assert data["name"] == "Test Throttle" + assert data["max_throttle"] == 0.8 + assert data["throttle_rate_limit"] == 0.1 + assert data["initial_throttle"] == 0.3 + + def test_from_dict(self): + """Test from_dict deserialization.""" + data = { + "name": "Test Throttle", + "demand_rate": 50, + "max_throttle": 0.8, + "throttle_rate_limit": 0.1, + "clamp": True, + "initial_throttle": 0.3, + "throttle_time_constant": None, + } + actuator = ThrottleActuator.from_dict(data) + assert actuator.name == "Test Throttle" + assert actuator.throttle == 0.3 + + def test_reset(self): + """Test actuator reset functionality.""" + actuator = ThrottleActuator(initial_throttle=0.3) + actuator.throttle = 0.7 + actuator._reset() + assert actuator.throttle == 0.3 + + def test_info_methods(self): + """Test info and all_info methods.""" + actuator = ThrottleActuator() + actuator.info() + actuator.all_info() + + +class TestThrustVectorActuator: + """Test suite for ThrustVectorActuator class.""" + + def test_initialization_defaults(self): + """Test ThrustVectorActuator initialization with default parameters.""" + actuator = ThrustVectorActuator() + assert actuator.name == "Thrust Vector Control" + assert actuator.demand_rate == 100 + assert actuator.actuator_range == (-10, 10) + assert actuator.gimbal_angle == 0.0 + + def test_initialization_custom(self): + """Test ThrustVectorActuator initialization with custom parameters.""" + actuator = ThrustVectorActuator( + name="Custom TVC", + demand_rate=50, + max_gimbal_angle=15.0, + gimbal_rate_limit=5.0, + initial_gimbal_angle=5.0, + ) + assert actuator.name == "Custom TVC" + assert actuator.actuator_range == (-15.0, 15.0) + assert actuator.gimbal_angle == 5.0 + + def test_gimbal_angle_property(self): + """Test gimbal angle getter and setter.""" + actuator = ThrustVectorActuator(max_gimbal_angle=10.0) + actuator.gimbal_angle = 5.0 + assert actuator.gimbal_angle == 5.0 + + def test_gimbal_angle_clamping(self): + """Test gimbal angle clamping to range.""" + actuator = ThrustVectorActuator(max_gimbal_angle=10.0, clamp=True) + actuator.actuator_output = 15.0 + assert actuator.gimbal_angle == 10.0 + actuator.actuator_output = -15.0 + assert actuator.gimbal_angle == -10.0 + + def test_to_dict(self): + """Test to_dict serialization.""" + actuator = ThrustVectorActuator( + name="Test TVC", + max_gimbal_angle=8.0, + gimbal_rate_limit=3.0, + initial_gimbal_angle=2.0, + ) + data = actuator.to_dict() + assert data["name"] == "Test TVC" + assert data["max_gimbal_angle"] == 8.0 + assert data["gimbal_rate_limit"] == 3.0 + assert data["initial_gimbal_angle"] == 2.0 + + def test_from_dict(self): + """Test from_dict deserialization.""" + data = { + "name": "Test TVC", + "demand_rate": 50, + "max_gimbal_angle": 8.0, + "gimbal_rate_limit": 3.0, + "clamp": True, + "initial_gimbal_angle": 2.0, + "gimbal_time_constant": None, + } + actuator = ThrustVectorActuator.from_dict(data) + assert actuator.name == "Test TVC" + assert actuator.gimbal_angle == 2.0 + + def test_reset(self): + """Test actuator reset functionality.""" + actuator = ThrustVectorActuator(initial_gimbal_angle=2.0) + actuator.gimbal_angle = 5.0 + actuator._reset() + assert actuator.gimbal_angle == 2.0 + + def test_info_methods(self): + """Test info and all_info methods.""" + actuator = ThrustVectorActuator() + actuator.info() + actuator.all_info() + + +class TestThrustVectorActuator2D: + """Test suite for ThrustVectorActuator2D class.""" + + def test_initialization_defaults(self): + """Test ThrustVectorActuator2D initialization with default parameters.""" + actuator = ThrustVectorActuator2D() + assert actuator.gimbal_angle_x == 0.0 + assert actuator.gimbal_angle_y == 0.0 + assert actuator.gimbal_angles == (0.0, 0.0) + + def test_initialization_custom(self): + """Test ThrustVectorActuator2D initialization with custom parameters.""" + actuator = ThrustVectorActuator2D( + name="Custom 2D TVC", + max_gimbal_angle=15.0, + initial_gimbal_angle=5.0, + ) + assert actuator.x.name == "Custom 2D TVC X-axis" + assert actuator.y.name == "Custom 2D TVC Y-axis" + assert actuator.gimbal_angles == (5.0, 5.0) + + def test_gimbal_angle_x_property(self): + """Test gimbal angle X getter and setter.""" + actuator = ThrustVectorActuator2D() + actuator.gimbal_angle_x = 5.0 + assert actuator.gimbal_angle_x == 5.0 + + def test_gimbal_angle_y_property(self): + """Test gimbal angle Y getter and setter.""" + actuator = ThrustVectorActuator2D() + actuator.gimbal_angle_y = 7.0 + assert actuator.gimbal_angle_y == 7.0 + + def test_gimbal_angles_property_get(self): + """Test gimbal angles tuple getter.""" + actuator = ThrustVectorActuator2D() + actuator.gimbal_angle_x = 3.0 + actuator.gimbal_angle_y = 4.0 + assert actuator.gimbal_angles == (3.0, 4.0) + + def test_gimbal_angles_property_set(self): + """Test gimbal angles tuple setter.""" + actuator = ThrustVectorActuator2D() + actuator.gimbal_angles = (5.0, 7.0) + assert actuator.gimbal_angle_x == 5.0 + assert actuator.gimbal_angle_y == 7.0 + assert actuator.gimbal_angles == (5.0, 7.0) + + def test_independent_axes(self): + """Test that X and Y axes are independent.""" + actuator = ThrustVectorActuator2D(max_gimbal_angle=10.0) + actuator.gimbal_angle_x = 5.0 + actuator.gimbal_angle_y = -3.0 + assert actuator.gimbal_angle_x == 5.0 + assert actuator.gimbal_angle_y == -3.0 + assert actuator.gimbal_angles == (5.0, -3.0) + + def test_clamping_individual_axes(self): + """Test clamping on individual axes.""" + actuator = ThrustVectorActuator2D(max_gimbal_angle=10.0, clamp=True) + # Test X axis clamping + actuator.x.actuator_output = 15.0 + assert actuator.gimbal_angle_x == 10.0 + # Test Y axis clamping + actuator.y.actuator_output = -15.0 + assert actuator.gimbal_angle_y == -10.0 + + +class TestActuatorDynamics: + """Test suite for actuator dynamics (rate limiting, time constants).""" + + def test_rate_limiting_roll(self): + """Test rate limiting on roll actuator.""" + actuator = RollActuator( + max_roll_torque=10.0, + demand_rate=100, + torque_rate_limit=5.0, + ) + # Change should be limited + actuator.actuator_output = 10.0 # Try to jump to 10 + # At 100 Hz demand rate: max_change = 5.0 / 100 = 0.05 + # So output should be clamped to 0.05 + assert actuator.roll_torque <= 0.1 # Small due to rate limit + + def test_no_rate_limiting_when_none(self): + """Test that no rate limiting occurs when set to None.""" + actuator = RollActuator( + max_roll_torque=10.0, + torque_rate_limit=None, + ) + actuator.actuator_output = 5.0 + assert actuator.roll_torque == 5.0 + + def test_time_constant_iir_filter(self): + """Test IIR filter behavior with time constant.""" + actuator = ThrottleActuator( + max_throttle=1.0, + demand_rate=100, + throttle_time_constant=0.1, + ) + # With time constant, output should be filtered + # alpha = Ts / (tau + Ts) = 0.01 / (0.1 + 0.01) ≈ 0.0909 + initial_output = actuator.throttle + actuator.actuator_output = 1.0 + filtered_output = actuator.throttle + # Output should be between initial and 1.0 due to filtering + assert initial_output < filtered_output < 1.0 + + +class TestActuatorValidation: + """Test suite for actuator parameter validation.""" + + def test_invalid_demand_rate_negative(self): + """Test that negative demand rate raises assertion error.""" + with pytest.raises(AssertionError): + RollActuator(demand_rate=-1) + + def test_invalid_range(self): + """Test that invalid range raises assertion error.""" + with pytest.raises(AssertionError): + RollActuator( + max_roll_torque=-5 + ) # This creates range (5, -5) which is invalid + + def test_invalid_time_constant_negative(self): + """Test that negative time constant raises assertion error.""" + with pytest.raises(AssertionError): + ThrottleActuator(throttle_time_constant=-0.1) + + def test_invalid_rate_limit_negative(self): + """Test that negative rate limit raises assertion error.""" + with pytest.raises(AssertionError): + ThrustVectorActuator(gimbal_rate_limit=-1.0) + + +class TestActuatorWarnings: + """Test suite for actuator warning conditions.""" + + def test_clamp_false_no_clamping(self): + """Test that output is not clamped when clamp=False.""" + actuator = RollActuator(max_roll_torque=10.0, clamp=False) + actuator.actuator_output = 15.0 + # Output should not be clamped when clamp=False + assert actuator.roll_torque == 15.0 + + def test_clamping_applied(self): + """Test that clamping is applied when clamp=True.""" + actuator = RollActuator(max_roll_torque=10.0, clamp=True) + actuator.actuator_output = 15.0 + # Output should be clamped to range + assert actuator.roll_torque == 10.0