diff --git a/docs/notebooks/sensors_testing.ipynb b/docs/notebooks/sensors_testing.ipynb new file mode 100644 index 000000000..842558dbd --- /dev/null +++ b/docs/notebooks/sensors_testing.ipynb @@ -0,0 +1,484 @@ +{ + "cells": [ + { + "attachments": {}, + "cell_type": "markdown", + "metadata": { + "colab_type": "text", + "id": "nvAT8wcRNVEk" + }, + "source": [ + "# -----------\n", + "\n", + "notebook to test sensors... should not be merged" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": {}, + "colab_type": "code", + "id": "XGK9M8ecNVEp" + }, + "outputs": [], + "source": [ + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "\n", + "from rocketpy import Environment, SolidMotor, Rocket, Flight" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": {}, + "colab_type": "code", + "id": "uRa566HoNVE9" + }, + "outputs": [], + "source": [ + "%matplotlib inline" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": {}, + "colab_type": "code", + "id": "5kl-Je8dNVFI" + }, + "outputs": [], + "source": [ + "env = Environment(latitude=32.990254, longitude=-106.974998, elevation=1400)\n", + "env.set_atmospheric_model(\n", + " type=\"custom_atmosphere\", wind_u=[(0, 3), (10000, 3)], wind_v=[(0, 5), (10000, -5)]\n", + ")\n", + "env.info()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": {}, + "colab_type": "code", + "id": "Vx1dZObwNVFX" + }, + "outputs": [], + "source": [ + "Pro75M1670 = SolidMotor(\n", + " thrust_source=\"../../data/motors/Cesaroni_M1670.eng\",\n", + " dry_mass=1.815,\n", + " dry_inertia=(0.125, 0.125, 0.002),\n", + " nozzle_radius=33 / 1000,\n", + " grain_number=5,\n", + " grain_density=1815,\n", + " grain_outer_radius=33 / 1000,\n", + " grain_initial_inner_radius=15 / 1000,\n", + " grain_initial_height=120 / 1000,\n", + " grain_separation=5 / 1000,\n", + " grains_center_of_mass_position=0.397,\n", + " center_of_dry_mass_position=0.317,\n", + " nozzle_position=0,\n", + " burn_time=3.9,\n", + " throat_radius=11 / 1000,\n", + " coordinate_system_orientation=\"nozzle_to_combustion_chamber\",\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": {}, + "colab_type": "code", + "id": "D1fyK8u_NVFh" + }, + "outputs": [], + "source": [ + "calisto = Rocket(\n", + " radius=127 / 2000,\n", + " mass=14.426,\n", + " inertia=(6.321, 6.321, 0.034),\n", + " power_off_drag=\"../../data/calisto/powerOffDragCurve.csv\",\n", + " power_on_drag=\"../../data/calisto/powerOnDragCurve.csv\",\n", + " center_of_mass_without_motor=0,\n", + " coordinate_system_orientation=\"tail_to_nose\",\n", + ")\n", + "\n", + "rail_buttons = calisto.set_rail_buttons(\n", + " upper_button_position=0.0818,\n", + " lower_button_position=-0.618,\n", + " angular_position=45,\n", + ")\n", + "\n", + "calisto.add_motor(Pro75M1670, position=-1.255)\n", + "\n", + "nose_cone = calisto.add_nose(length=0.55829, kind=\"vonKarman\", position=1.278)\n", + "\n", + "fin_set = calisto.add_trapezoidal_fins(\n", + " n=4,\n", + " root_chord=0.120,\n", + " tip_chord=0.060,\n", + " span=0.110,\n", + " position=-1.04956,\n", + " cant_angle=0.5,\n", + " airfoil=(\"../../data/calisto/NACA0012-radians.csv\", \"radians\"),\n", + ")\n", + "\n", + "tail = calisto.add_tail(\n", + " top_radius=0.0635, bottom_radius=0.0435, length=0.060, position=-1.194656\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from rocketpy import Accelerometer, Gyroscope\n", + "\n", + "accel_noisy_nosecone = Accelerometer(\n", + " sampling_rate=100,\n", + " consider_gravity=False,\n", + " orientation=(60, 60, 60),\n", + " measurement_range=70,\n", + " resolution=0.4882,\n", + " noise_density=0.05,\n", + " random_walk_density=0.02,\n", + " constant_bias=1,\n", + " operating_temperature=25,\n", + " temperature_bias=0.02,\n", + " temperature_scale_factor=0.02,\n", + " cross_axis_sensitivity=0.02,\n", + " name=\"Accelerometer in Nosecone\",\n", + ")\n", + "accel_clean_cdm = Accelerometer(\n", + " sampling_rate=100,\n", + " consider_gravity=False,\n", + " orientation=[\n", + " [0.25, -0.0581, 0.9665],\n", + " [0.433, 0.8995, -0.0581],\n", + " [-0.8661, 0.433, 0.25],\n", + " ],\n", + " name=\"Accelerometer in CDM\",\n", + ")\n", + "calisto.add_sensor(accel_noisy_nosecone, 1.278)\n", + "calisto.add_sensor(accel_clean_cdm, -0.10482544178314143) # , 127/2000)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "accel_noisy_nosecone.prints.all()\n", + "accel_clean_cdm.prints.all() # should have the same rotation matrix" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "gyro_clean = Gyroscope(sampling_rate=100)\n", + "gyro_noisy = Gyroscope(\n", + " sampling_rate=100,\n", + " orientation=(180, 0, 0),\n", + " acceleration_sensitivity=0.02,\n", + " measurement_range=70,\n", + " resolution=0.4882,\n", + " noise_density=0.05,\n", + " random_walk_density=0.02,\n", + " constant_bias=1,\n", + " operating_temperature=25,\n", + " temperature_bias=0.02,\n", + " temperature_scale_factor=0.02,\n", + " cross_axis_sensitivity=0.02,\n", + ")\n", + "calisto.add_sensor(gyro_clean, -0.10482544178314143 + 0.5, 127 / 2000)\n", + "calisto.add_sensor(gyro_noisy, 1.278 - 0.4, 127 / 2000 - 127 / 4000)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "calisto.draw(plane=\"xz\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "calisto.draw(plane=\"yz\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def controller_function(\n", + " time, sampling_rate, state, state_history, observed_variables, air_brakes, sensors\n", + "):\n", + " # state = [x, y, z, vx, vy, vz, e0, e1, e2, e3, wx, wy, wz]\n", + " altitude_ASL = state[2]\n", + " altitude_AGL = altitude_ASL - env.elevation\n", + " vx, vy, vz = state[3], state[4], state[5]\n", + "\n", + " # Get winds in x and y directions\n", + " wind_x, wind_y = env.wind_velocity_x(altitude_ASL), env.wind_velocity_y(\n", + " altitude_ASL\n", + " )\n", + "\n", + " # Calculate Mach number\n", + " free_stream_speed = ((wind_x - vx) ** 2 + (wind_y - vy) ** 2 + (vz) ** 2) ** 0.5\n", + " mach_number = free_stream_speed / env.speed_of_sound(altitude_ASL)\n", + "\n", + " # Get previous state from state_history\n", + " previous_state = state_history[-1]\n", + " previous_vz = previous_state[5]\n", + "\n", + " # If we wanted to we could get the returned values from observed_variables:\n", + " # returned_time, deployment_level, drag_coefficient = observed_variables[-1]\n", + "\n", + " # Check if the rocket has reached burnout\n", + " accelerometer = sensors[0]\n", + " if accelerometer.measurement[2] > 0:\n", + " return None\n", + "\n", + " # If below 1500 meters above ground level, air_brakes are not deployed\n", + " if altitude_AGL < 1500:\n", + " air_brakes.deployment_level = 0\n", + "\n", + " # Else calculate the deployment level\n", + " else:\n", + " # Controller logic\n", + " new_deployment_level = (\n", + " air_brakes.deployment_level + 0.1 * vz + 0.01 * previous_vz**2\n", + " )\n", + "\n", + " # Limiting the speed of the air_brakes to 0.2 per second\n", + " # Since this function is called every 1/sampling_rate seconds\n", + " # the max change in deployment level per call is 0.2/sampling_rate\n", + " max_change = 0.2 / sampling_rate\n", + " lower_bound = air_brakes.deployment_level - max_change\n", + " upper_bound = air_brakes.deployment_level + max_change\n", + " new_deployment_level = min(max(new_deployment_level, lower_bound), upper_bound)\n", + "\n", + " air_brakes.deployment_level = new_deployment_level\n", + "\n", + " # Return variables of interest to be saved in the observed_variables list\n", + " return (\n", + " time,\n", + " air_brakes.deployment_level,\n", + " air_brakes.drag_coefficient(air_brakes.deployment_level, mach_number),\n", + " )" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "air_brakes = calisto.add_air_brakes(\n", + " drag_coefficient_curve=\"../../data/calisto/air_brakes_cd.csv\",\n", + " controller_function=controller_function,\n", + " sampling_rate=10,\n", + " reference_area=None,\n", + " clamp=True,\n", + " initial_observed_variables=[0, 0, 0],\n", + " override_rocket_drag=False,\n", + " name=\"AirBrakes\",\n", + " controller_name=\"AirBrakes Controller\",\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# air_brakes.all_info()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "colab": {}, + "colab_type": "code", + "id": "v__Ud2p2NVFx" + }, + "outputs": [], + "source": [ + "test_flight = Flight(\n", + " rocket=calisto,\n", + " environment=env,\n", + " rail_length=5.2,\n", + " inclination=85,\n", + " heading=0,\n", + " time_overshoot=False,\n", + " terminate_on_apogee=True,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "test_flight.altitude()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# get first column of every row as time from [(time,(ax,ay,az)),...] = a.measured_data\n", + "time1, ax, ay, az = zip(*accel_noisy_nosecone.measured_data)\n", + "time2, bx, by, bz = zip(*accel_clean_cdm.measured_data)\n", + "\n", + "\n", + "plt.plot(time1, ax, label=\"Noisy Accelerometer\")\n", + "plt.plot(time2, bx, label=\"Clean Accelerometer\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Acceleration ax (m/s^2)\")\n", + "plt.legend()\n", + "plt.show()\n", + "\n", + "plt.plot(time1, ay, label=\"Noisy Accelerometer\")\n", + "plt.plot(time2, by, label=\"Clean Accelerometer\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Acceleration ay (m/s^2)\")\n", + "plt.legend()\n", + "plt.show()\n", + "\n", + "plt.plot(time1, az, label=\"Noisy Accelerometer\")\n", + "plt.plot(time2, bz, label=\"Clean Accelerometer\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Acceleration az (m/s^2)\")\n", + "plt.legend()\n", + "plt.show()\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# now plot the total acceleration\n", + "\n", + "abs_a = (np.array(ax) ** 2 + np.array(ay) ** 2 + np.array(az) ** 2) ** 0.5\n", + "abs_b = (np.array(bx) ** 2 + np.array(by) ** 2 + np.array(bz) ** 2) ** 0.5\n", + "plt.plot(time1, abs_a, label=\"noisy\")\n", + "plt.plot(time2, abs_b, label=\"clean\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Acceleration (m/s^2)\")\n", + "plt.legend()\n", + "plt.xlim(0, 10)\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "time1, wx, wy, wz = zip(*gyro_noisy.measured_data)\n", + "time2, zx, zy, zz = zip(*gyro_clean.measured_data)\n", + "\n", + "plt.plot(time1, wx, label=\"Noisy Gyroscope\")\n", + "plt.plot(time2, zx, label=\"Clean Gyroscope\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Angular Velocity wx (rad/s)\")\n", + "plt.legend()\n", + "plt.show()\n", + "\n", + "plt.plot(time1, wy, label=\"Noisy Gyroscope\")\n", + "plt.plot(time2, zy, label=\"Clean Gyroscope\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Angular Velocity wy (rad/s)\")\n", + "plt.legend()\n", + "plt.show()\n", + "\n", + "plt.plot(time1, wz, label=\"Noisy Gyroscope\")\n", + "plt.plot(time2, zz, label=\"Clean Gyroscope\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Angular Velocity wz (rad/s)\")\n", + "plt.legend()\n", + "plt.show()\n", + "\n", + "# now plot the total angular velocity\n", + "\n", + "abs_w = (np.array(wx) ** 2 + np.array(wy) ** 2 + np.array(wz) ** 2) ** 0.5\n", + "abs_z = (np.array(zx) ** 2 + np.array(zy) ** 2 + np.array(zz) ** 2) ** 0.5\n", + "plt.plot(time1, abs_w, label=\"noisy\")\n", + "plt.plot(time2, abs_z, label=\"clean\")\n", + "plt.xlabel(\"Time (s)\")\n", + "plt.ylabel(\"Angular Velocity (rad/s)\")\n", + "plt.legend()\n", + "plt.xlim(0, 10)\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "colab": { + "name": "getting_started.ipynb", + "provenance": [], + "toc_visible": true + }, + "hide_input": false, + "kernelspec": { + "display_name": "Python 3.10.0 ('rocketpy_dev')", + "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.10.11" + }, + "vscode": { + "interpreter": { + "hash": "18e93d5347af13ace37d47ea4e2a2ad720f0331bd9cb28f9983f5585f4dfaa5c" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/rocketpy/__init__.py b/rocketpy/__init__.py index 10404b619..fe55dda41 100644 --- a/rocketpy/__init__.py +++ b/rocketpy/__init__.py @@ -37,4 +37,5 @@ Tail, TrapezoidalFins, ) +from .sensors import Accelerometer, Gyroscope, Sensors from .simulation import Flight diff --git a/rocketpy/control/controller.py b/rocketpy/control/controller.py index 81fc8b332..c2617f8eb 100644 --- a/rocketpy/control/controller.py +++ b/rocketpy/control/controller.py @@ -1,3 +1,5 @@ +from inspect import signature + from ..prints.controller_prints import _ControllerPrints @@ -52,6 +54,10 @@ def __init__( the controller function can interact with. The objects are listed in the same order as they are provided in the `interactive_objects`. + 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 This function will be called during the simulation at the specified sampling rate. The function should evaluate and change the interactive @@ -78,7 +84,7 @@ def __init__( None """ self.interactive_objects = interactive_objects - self.controller_function = controller_function + self.controller_function = self.__init_controller_function(controller_function) self.sampling_rate = sampling_rate self.name = name self.prints = _ControllerPrints(self) @@ -88,7 +94,44 @@ def __init__( else: self.observed_variables = [] - def __call__(self, time, state_vector, state_history): + def __init_controller_function(self, controller_function): + """Checks number of arguments of the controller function and initializes + it with the correct number of arguments. This is a workaround to allow + the controller function to receive sensors without breaking changes""" + sig = signature(controller_function) + if len(sig.parameters) == 6: + + def new_controller_function( + time, + sampling_rate, + state_vector, + state_history, + observed_variables, + interactive_objects, + sensors, + ): + return controller_function( + time, + sampling_rate, + state_vector, + state_history, + observed_variables, + interactive_objects, + ) + + elif len(sig.parameters) == 7: + new_controller_function = controller_function + else: + raise ValueError( + "The controller function must have 6 or 7 arguments. " + "The arguments must be in the following order: " + "(time, sampling_rate, state_vector, state_history, " + "observed_variables, interactive_objects, sensors)." + "Sensors argument is optional." + ) + return new_controller_function + + def __call__(self, time, state_vector, state_history, sensors): """Call the controller function. This is used by the simulation class. Parameters @@ -104,6 +147,11 @@ def __call__(self, time, state_vector, state_history): history is a list of every state vector of every step of the simulation. The state history is a list of lists, where each sublist is a state vector and is ordered from oldest to newest. + 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. Returns ------- @@ -116,6 +164,7 @@ def __call__(self, time, state_vector, state_history): state_history, self.observed_variables, self.interactive_objects, + sensors, ) if observed_variables is not None: self.observed_variables.append(observed_variables) diff --git a/rocketpy/mathutils/vector_matrix.py b/rocketpy/mathutils/vector_matrix.py index 332e1b680..2776b3aa7 100644 --- a/rocketpy/mathutils/vector_matrix.py +++ b/rocketpy/mathutils/vector_matrix.py @@ -2,6 +2,8 @@ from functools import cached_property from itertools import product +from rocketpy.tools import euler_to_quaternions, normalize_quaternions + class Vector: """Pure python basic R3 vector class designed for simple operations. @@ -152,7 +154,10 @@ def __len__(self): @cached_property def unit_vector(self): """R3 vector with the same direction of self, but normalized.""" - return self / abs(self) + try: + return self / abs(self) + except ZeroDivisionError: + return self @cached_property def cross_matrix(self): @@ -238,6 +243,30 @@ def __xor__(self, other): ] ) + def __and__(self, other): + """Element wise multiplication between two R3 vectors. Also known as + Hadamard product. + + Parameters + ---------- + other : Vector + R3 vector to be multiplied with self. + + Returns + ------- + Vector + R3 vector resulting from the element wise multiplication between + self and other. + + Examples + -------- + >>> v = Vector([1, 7, 3]) + >>> u = Vector([2, 5, 6]) + >>> (v & u) + Vector(2, 35, 18) + """ + return Vector([self.x * other[0], self.y * other[1], self.z * other[2]]) + def __matmul__(self, other): """Dot product between two R3 vectors.""" return self.x * other.x + self.y * other.y + self.z * other.z @@ -920,6 +949,47 @@ def dot(self, other): """ return self.__matmul__(other) + def round(self, decimals=0): + """Round all the values matrix to a given number of decimals. + + Parameters + ---------- + decimals : int, optional + Number of decimal places to round to. Defaults to 0. + + Returns + ------- + Matrix + The rounded matrix. + + Examples + -------- + >>> M = Matrix([[1.1234, 2.3456, 3.4567], [4.5678, 5.6789, 6.7890], [7.8901, 8.9012, 9.0123]]) + >>> M.round(2) + Matrix([1.12, 2.35, 3.46], + [4.57, 5.68, 6.79], + [7.89, 8.9, 9.01]) + """ + return Matrix( + [ + [ + round(self.xx, decimals), + round(self.xy, decimals), + round(self.xz, decimals), + ], + [ + round(self.yx, decimals), + round(self.yy, decimals), + round(self.yz, decimals), + ], + [ + round(self.zx, decimals), + round(self.zy, decimals), + round(self.zz, decimals), + ], + ] + ) + def __str__(self): return ( f"[{self.xx}, {self.xy}, {self.xz}]\n" @@ -955,34 +1025,66 @@ def transformation(quaternion): The quaternion representing the rotation from frame A to frame B. Example: (cos(phi/2), 0, 0, sin(phi/2)) represents a rotation of phi around the z-axis. - Note: the quaternion must be normalized. Returns ------- Matrix The transformation matrix from frame B to frame A. """ - q_w, q_x, q_y, q_z = quaternion + # normalize quaternion + q_w, q_x, q_y, q_z = normalize_quaternions(quaternion) + + # pre-compute common terms + q_x2 = q_x**2 + q_y2 = q_y**2 + q_z2 = q_z**2 + q_wx = q_w * q_x + q_wy = q_w * q_y + q_wz = q_w * q_z + q_xy = q_x * q_y + q_xz = q_x * q_z + q_yz = q_y * q_z return Matrix( [ [ - 1 - 2 * (q_y**2 + q_z**2), - 2 * (q_x * q_y - q_w * q_z), - 2 * (q_x * q_z + q_w * q_y), + 1 - 2 * (q_y2 + q_z2), + 2 * (q_xy - q_wz), + 2 * (q_xz + q_wy), ], [ - 2 * (q_x * q_y + q_w * q_z), - 1 - 2 * (q_x**2 + q_z**2), - 2 * (q_y * q_z - q_w * q_x), + 2 * (q_xy + q_wz), + 1 - 2 * (q_x2 + q_z2), + 2 * (q_yz - q_wx), ], [ - 2 * (q_x * q_z - q_w * q_y), - 2 * (q_y * q_z + q_w * q_x), - 1 - 2 * (q_x**2 + q_y**2), + 2 * (q_xz - q_wy), + 2 * (q_yz + q_wx), + 1 - 2 * (q_x2 + q_y2), ], ] ) + @staticmethod + def transformation_euler_angles(roll, pitch, yaw): + """Returns the transformation Matrix from frame B to frame A, where B + is rotated by the Euler angles roll, pitch and yaw with respect to A. + + Parameters + ---------- + roll : float + The roll angle in degrees. + pitch : float + The pitch angle in degrees. + yaw : float + The yaw angle in degrees. + + Returns + ------- + Matrix + The transformation matrix from frame B to frame A. + """ + return Matrix.transformation(euler_to_quaternions(roll, pitch, yaw)) + if __name__ == "__main__": import doctest diff --git a/rocketpy/plots/rocket_plots.py b/rocketpy/plots/rocket_plots.py index 2be7a4a73..0d7b5b130 100644 --- a/rocketpy/plots/rocket_plots.py +++ b/rocketpy/plots/rocket_plots.py @@ -3,6 +3,7 @@ import matplotlib.pyplot as plt import numpy as np +from rocketpy.mathutils.vector_matrix import Vector from rocketpy.motors import EmptyMotor, HybridMotor, LiquidMotor, SolidMotor from rocketpy.rocket.aero_surface import Fins, NoseCone, Tail @@ -168,7 +169,7 @@ def thrust_to_weight(self): return None - def draw(self, vis_args=None): + def draw(self, vis_args=None, plane="xz"): """Draws the rocket in a matplotlib figure. Parameters @@ -188,6 +189,9 @@ def draw(self, vis_args=None): } A full list of color names can be found at: https://matplotlib.org/stable/gallery/color/named_colors + plane : str, optional + Plane in which the rocket will be drawn. Default is 'xz'. Other + options is 'yz'. Used only for sensors representation. """ if vis_args is None: vis_args = { @@ -201,10 +205,8 @@ def draw(self, vis_args=None): "line_width": 1.0, } - # Create the figure and axis - _, ax = plt.subplots(figsize=(8, 6), facecolor="#EEEEEE") + fig, ax = plt.subplots(figsize=(8, 6), facecolor=vis_args["background"]) ax.set_aspect("equal") - ax.set_facecolor(vis_args["background"]) ax.grid(True, linestyle="--", linewidth=0.5) csys = self.rocket._csys @@ -216,6 +218,7 @@ def draw(self, vis_args=None): self._draw_motor(last_radius, last_x, ax, vis_args) self._draw_rail_buttons(ax, vis_args) self._draw_center_of_mass_and_pressure(ax) + self._draw_sensor(ax, self.rocket.sensors, plane, vis_args) plt.title("Rocket Representation") plt.xlim() @@ -552,6 +555,56 @@ def _draw_center_of_mass_and_pressure(self, ax): cp, 0, label="Static Center of Pressure", color="red", s=10, zorder=10 ) + def _draw_sensor(self, ax, sensors, plane, vis_args): + """Draw the sensor as a small thick line at the position of the sensor, + with a vector pointing in the direction normal of the sensor. Get the + normal vector from the sensor orientation matrix.""" + colors = plt.rcParams["axes.prop_cycle"].by_key()["color"] + for i, sensor_pos in enumerate(sensors): + sensor = sensor_pos[0] + pos = sensor_pos[1] + if plane == "xz": + # z position of the sensor is the x position in the plot + x_pos = pos[2] + normal_x = sensor.normal_vector.z + # x position of the sensor is the y position in the plot + y_pos = pos[0] + normal_y = sensor.normal_vector.x + elif plane == "yz": + # z position of the sensor is the x position in the plot + x_pos = pos[2] + normal_x = sensor.normal_vector.z + # y position of the sensor is the y position in the plot + y_pos = pos[1] + normal_y = sensor.normal_vector.y + else: + raise ValueError("Plane must be 'xz' or 'yz'.") + + # line length is 2/5 of the rocket radius + line_length = self.rocket.radius / 2.5 + + ax.plot( + [x_pos, x_pos], + [y_pos + line_length, y_pos - line_length], + linewidth=2, + color=colors[(i + 1) % len(colors)], + zorder=10, + label=sensor.name, + ) + ax.quiver( + x_pos, + y_pos, + normal_x, + normal_y, + color=colors[(i + 1) % len(colors)], + scale_units="xy", + angles="xy", + minshaft=2, + headwidth=2, + headlength=4, + zorder=10, + ) + def all(self): """Prints out all graphs available about the Rocket. It simply calls all the other plotter methods in this class. diff --git a/rocketpy/prints/sensors_prints.py b/rocketpy/prints/sensors_prints.py new file mode 100644 index 000000000..2d646a4f4 --- /dev/null +++ b/rocketpy/prints/sensors_prints.py @@ -0,0 +1,110 @@ +from abc import ABC, abstractmethod + + +class _SensorsPrints(ABC): + def __init__(self, sensor): + self.sensor = sensor + self.units = sensor.units + + def _print_aligned(self, label, value): + """Prints a label and a value aligned vertically.""" + print(f"{label:<26} {value}") + + def identity(self): + """Prints the identity of the sensor.""" + print("Identification:\n") + self._print_aligned("Name:", self.sensor.name) + self._print_aligned("Type:", self.sensor.__class__.__name__) + + def orientation(self): + """Prints the orientation of the sensor.""" + print("\nOrientation:\n") + self._print_aligned("Orientation:", self.sensor.orientation) + self._print_aligned("Normal Vector:", self.sensor.normal_vector) + print("Rotation Matrix:") + for row in self.sensor.rotation_matrix: + value = " ".join(f"{val:.2f}" for val in row) + value = [float(val) for val in value.split()] + self._print_aligned("", value) + + def quantization(self): + """Prints the quantization of the sensor.""" + print("\nQuantization:\n") + self._print_aligned( + "Measurement Range:", + f"{self.sensor.measurement_range[0]} to {self.sensor.measurement_range[1]} ({self.units})", + ) + self._print_aligned("Resolution:", f"{self.sensor.resolution} {self.units}/LSB") + + @abstractmethod + def noise(self): + """Prints the noise of the sensor.""" + pass + + def _general_noise(self): + """Prints the noise of the sensor.""" + print("\nNoise:\n") + self._print_aligned( + "Noise Density:", f"{self.sensor.noise_density} {self.units}/√Hz" + ) + self._print_aligned( + "Noise Variance:", f"{self.sensor.noise_variance} ({self.units})^2" + ) + self._print_aligned( + "Random Walk Density:", + f"{self.sensor.random_walk_density} {self.units}/√Hz", + ) + self._print_aligned( + "Random Walk Variance:", + f"{self.sensor.random_walk_variance} ({self.units})^2", + ) + self._print_aligned( + "Constant Bias:", f"{self.sensor.constant_bias} {self.units}" + ) + self._print_aligned( + "Operating Temperature:", f"{self.sensor.operating_temperature} °C" + ) + self._print_aligned( + "Temperature Bias:", f"{self.sensor.temperature_bias} {self.units}/°C" + ) + self._print_aligned( + "Temperature Scale Factor:", f"{self.sensor.temperature_scale_factor} %/°C" + ) + self._print_aligned( + "Cross Axis Sensitivity:", f"{self.sensor.cross_axis_sensitivity} %" + ) + + def all(self): + """Prints all information of the sensor.""" + self.identity() + self.orientation() + self.quantization() + self.noise() + + +class _AccelerometerPrints(_SensorsPrints): + """Class that contains all accelerometer prints.""" + + def __init__(self, accelerometer): + """Initialize the class.""" + super().__init__(accelerometer) + + def noise(self): + """Prints the noise of the sensor.""" + self._general_noise() + + +class _GyroscopePrints(_SensorsPrints): + """Class that contains all gyroscope prints.""" + + def __init__(self, gyroscope): + """Initialize the class.""" + super().__init__(gyroscope) + + def noise(self): + """Prints the noise of the sensor.""" + self._general_noise() + self._print_aligned( + "Acceleration Sensitivity:", + f"{self.sensor.acceleration_sensitivity} rad/s/g", + ) diff --git a/rocketpy/rocket/aero_surface.py b/rocketpy/rocket/aero_surface.py index d41240ac9..a5ea26638 100644 --- a/rocketpy/rocket/aero_surface.py +++ b/rocketpy/rocket/aero_surface.py @@ -1977,8 +1977,9 @@ def __init__( brakes drag coefficient will be used for the entire rocket instead. Default is False. deployment_level : float, optional - Current deployment level, ranging from 0 to 1. Deployment level is the - fraction of the total airbrake area that is Deployment. Default is 0. + Initial deployment level, ranging from 0 to 1. Deployment level is + the fraction of the total airbrake area that is deployed. Default + is 0. name : str, optional Name of the air brakes. Default is "AirBrakes". @@ -1996,6 +1997,7 @@ def __init__( self.reference_area = reference_area self.clamp = clamp self.override_rocket_drag = override_rocket_drag + self.initial_deployment_level = deployment_level self.deployment_level = deployment_level self.prints = _AirBrakesPrints(self) self.plots = _AirBrakesPlots(self) @@ -2022,6 +2024,12 @@ def deployment_level(self, value): ) self._deployment_level = value + def _reset(self): + """Resets the air brakes to their initial state. This is ran at the + beginning of each simulation to ensure the air brakes are in the correct + state.""" + self.deployment_level = self.initial_deployment_level + def evaluate_center_of_pressure(self): """Evaluates the center of pressure of the aerodynamic surface in local coordinates. diff --git a/rocketpy/rocket/components.py b/rocketpy/rocket/components.py index 1d1d33e56..66c9f09a7 100644 --- a/rocketpy/rocket/components.py +++ b/rocketpy/rocket/components.py @@ -23,11 +23,16 @@ def __init__(self): self.component_tuple = namedtuple("component_tuple", "component position") self._components = [] + # List of components and their positions to avoid extra for loops in + # simulation time + self.__component_list = [] + self.__position_list = [] + def __repr__(self): """Return a string representation of the Components instance.""" components_str = "\n".join( [ - f"\tComponent: {str(c.component):80} Position: {c.position:>6.3f}" + f"\tComponent: {str(c.component):80} Position: {c.position}" for c in self._components ] ) @@ -61,6 +66,8 @@ def add(self, component, position): ------- None """ + self.__component_list.append(component) + self.__position_list.append(position) self._components.append(self.component_tuple(component, position)) def get_by_type(self, component_type): @@ -103,6 +110,16 @@ def get_tuple_by_type(self, component_type): ] return component_type_list + def get_components(self): + """Return a list of all the components in the list of components. + + Returns + ------- + list[Component] + A list of all the components in the list of components. + """ + return self.__component_list + def get_positions(self): """Return a list of all the positions of the components in the list of components. @@ -113,7 +130,7 @@ def get_positions(self): A list of all the positions of the components in the list of components. """ - return [c.position for c in self._components] + return self.__position_list def remove(self, component): """Remove a component from the list of components. If more than one diff --git a/rocketpy/rocket/parachute.py b/rocketpy/rocket/parachute.py index 31f9252ed..c1bff2638 100644 --- a/rocketpy/rocket/parachute.py +++ b/rocketpy/rocket/parachute.py @@ -1,3 +1,5 @@ +from inspect import signature + import numpy as np from ..mathutils.function import Function @@ -31,6 +33,11 @@ class Parachute: `[x, y, z, vx, vy, vz, e0, e1, e2, e3, wx, wy, wz]`. + 4. 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. + The function should return True if the parachute ejection system should be triggered and False otherwise. @@ -177,12 +184,22 @@ def __evaluate_trigger_function(self, trigger): """This is used to set the triggerfunc attribute that will be used to interact with the Flight class. """ + # The parachute is deployed by a custom function if callable(trigger): - self.triggerfunc = trigger + # work around for having added sensors to parachute triggers + # to avoid breaking changes + triggerfunc = trigger + sig = signature(triggerfunc) + if len(sig.parameters) == 3: + + def triggerfunc(p, h, y, sensors): + return trigger(p, h, y) + + self.triggerfunc = triggerfunc elif isinstance(trigger, (int, float)): # The parachute is deployed at a given height - def triggerfunc(p, h, y): + def triggerfunc(p, h, y, sensors): # p = pressure considering parachute noise signal # h = height above ground level considering parachute noise signal # y = [x, y, z, vx, vy, vz, e0, e1, e2, e3, w1, w2, w3] @@ -192,7 +209,7 @@ def triggerfunc(p, h, y): elif trigger.lower() == "apogee": # The parachute is deployed at apogee - def triggerfunc(p, h, y): + def triggerfunc(p, h, y, sensors): # p = pressure considering parachute noise signal # h = height above ground level considering parachute noise signal # y = [x, y, z, vx, vy, vz, e0, e1, e2, e3, w1, w2, w3] diff --git a/rocketpy/rocket/rocket.py b/rocketpy/rocket/rocket.py index c074afc8d..c7bbd380a 100644 --- a/rocketpy/rocket/rocket.py +++ b/rocketpy/rocket/rocket.py @@ -4,6 +4,7 @@ from rocketpy.control.controller import _Controller from rocketpy.mathutils.function import Function +from rocketpy.mathutils.vector_matrix import Vector from rocketpy.motors.motor import EmptyMotor from rocketpy.plots.rocket_plots import _RocketPlots from rocketpy.prints.rocket_prints import _RocketPrints @@ -285,16 +286,11 @@ def __init__( self.thrust_eccentricity_y = 0 self.thrust_eccentricity_x = 0 - # Parachute, Aerodynamic and Rail buttons data initialization + # Parachute, Aerodynamic, Buttons, Controllers, Sensors data initialization self.parachutes = [] - - # Controllers data initialization self._controllers = [] - - # AirBrakes data initialization self.air_brakes = [] - - # Aerodynamic data initialization + self.sensors = Components() self.aerodynamic_surfaces = Components() self.rail_buttons = Components() @@ -1174,6 +1170,35 @@ def add_parachute( self.parachutes.append(parachute) return self.parachutes[-1] + def add_sensor(self, sensor, position, x_offset=0, y_offset=0): + """Adds a sensor to the rocket. + + Parameters + ---------- + sensor : Sensor + Sensor to be added to the rocket. + position : int, float, tuple + Position, in meters, of the sensor's coordinate system origin + relative to the user defined rocket coordinate system. + x_offset : int, float, optional + Distance in meters by which the sensor is to be translated in the + rocket's x direction relative to geometrical center line. + Default is 0. + y_offset : int, float, optional + Distance in meters by which the sensor is to be translated in the + rocket's y direction relative to geometrical center line. + Default is 0. + + Returns + ------- + None + """ + self.sensors.add(sensor, Vector([x_offset, y_offset, position])) + try: + sensor._attached_rockets[self] += 1 + except KeyError: + sensor._attached_rockets[self] = 1 + def add_air_brakes( self, drag_coefficient_curve, @@ -1242,6 +1267,10 @@ def add_air_brakes( the controller function can interact with. The objects are listed in the same order as they are provided in the `interactive_objects` + 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 This function will be called during the simulation at the specified sampling rate. The function should evaluate and change the observed @@ -1435,7 +1464,7 @@ def add_thrust_eccentricity(self, x, y): self.thrust_eccentricity_x = y return self - def draw(self, vis_args=None): + def draw(self, vis_args=None, plane="xz"): """Draws the rocket in a matplotlib figure. Parameters @@ -1455,9 +1484,11 @@ def draw(self, vis_args=None): } A full list of color names can be found at: https://matplotlib.org/stable/gallery/color/named_colors + plane : str, optional + Plane in which the rocket will be drawn. Default is 'xz'. Other + options is 'yz'. Used only for sensors representation. """ - self.plots.draw(vis_args) - return None + self.plots.draw(vis_args, plane) def info(self): """Prints out a summary of the data and graphs available about diff --git a/rocketpy/sensors/__init__.py b/rocketpy/sensors/__init__.py new file mode 100644 index 000000000..5bfe07805 --- /dev/null +++ b/rocketpy/sensors/__init__.py @@ -0,0 +1,3 @@ +from .accelerometer import Accelerometer +from .gyroscope import Gyroscope +from .sensors import Sensors diff --git a/rocketpy/sensors/accelerometer.py b/rocketpy/sensors/accelerometer.py new file mode 100644 index 000000000..f4a637b66 --- /dev/null +++ b/rocketpy/sensors/accelerometer.py @@ -0,0 +1,304 @@ +import json + +import numpy as np + +from ..mathutils.vector_matrix import Matrix, Vector +from ..prints.sensors_prints import _AccelerometerPrints +from ..sensors.sensors import Sensors + + +class Accelerometer(Sensors): + """Class for the accelerometer sensor + + Attributes + ---------- + consider_gravity : bool + Whether the sensor considers the effect of gravity on the acceleration. + prints : _AccelerometerPrints + Object that contains the print functions for the sensor. + sampling_rate : float + Sample rate of the sensor in Hz. + orientation : tuple, list + Orientation of the sensor in the rocket. + measurement_range : float, tuple + The measurement range of the sensor in m/s^2. + resolution : float + The resolution of the sensor in m/s^2/LSB. + noise_density : float, list + The noise density of the sensor in m/s^2/√Hz. + noise_variance : float, list + The variance of the noise of the sensor in (m/s^2)^2. + random_walk_density : float, list + The random walk density of the sensor in m/s^2/√Hz. + random_walk_variance : float, list + The variance of the random walk of the sensor in (m/s^2)^2. + constant_bias : float, list + The constant bias of the sensor in m/s^2. + operating_temperature : float + The operating temperature of the sensor in degrees Celsius. + temperature_bias : float, list + The temperature bias of the sensor in m/s^2/°C. + temperature_scale_factor : float, list + The temperature scale factor of the sensor in %/°C. + cross_axis_sensitivity : float + The cross axis sensitivity of the sensor in percentage. + name : str + The name of the sensor. + rotation_matrix : Matrix + The rotation matrix of the sensor from the sensor frame to the rocket + frame of reference. + normal_vector : Vector + The normal vector of the sensor in the rocket frame of reference. + _random_walk_drift : Vector + The random walk drift of the sensor in m/s^2. + measurement : float + The measurement of the sensor after quantization, noise and temperature + drift. + measured_data : list + The stored measured data of the sensor after quantization, noise and + temperature drift. + """ + + units = "m/s^2" + + def __init__( + self, + sampling_rate, + orientation=(0, 0, 0), + measurement_range=np.inf, + resolution=0, + noise_density=0, + noise_variance=1, + random_walk_density=0, + random_walk_variance=1, + constant_bias=0, + operating_temperature=25, + temperature_bias=0, + temperature_scale_factor=0, + cross_axis_sensitivity=0, + consider_gravity=False, + name="Accelerometer", + ): + """ + Initialize the accelerometer sensor + + Parameters + ---------- + sampling_rate : float + Sample rate of the sensor in Hz. + orientation : tuple, list, optional + Orientation of the sensor in the rocket. The orientation can be + given as: + - A list of length 3, where the elements are the Euler angles for + the rotation yaw (ψ), pitch (θ) and roll (φ) in radians. The + standard rotation sequence is z-y-x (3-2-1) is used, meaning the + sensor is first rotated by ψ around the x axis, then by θ around + the new y axis and finally by φ around the new z axis. + - A list of lists (matrix) of shape 3x3, representing the rotation + matrix from the sensor frame to the rocket frame. The sensor frame + of reference is defined as to have z axis along the sensor's normal + vector pointing upwards, x and y axes perpendicular to the z axis + and each other. + The rocket frame of reference is defined as to have z axis + along the rocket's axis of symmetry pointing upwards, x and y axes + perpendicular to the z axis and each other. A rotation around the x + axis configures a pitch, around the y axis a yaw and around z axis a + roll. Default is (0, 0, 0), meaning the sensor is aligned with all + of the rocket's axis. + measurement_range : float, tuple, optional + The measurement range of the sensor in the m/s^2. If a float, the + same range is applied both for positive and negative values. If a + tuple, the first value is the positive range and the second value is + the negative range. Default is np.inf. + resolution : float, optional + The resolution of the sensor in m/s^2/LSB. Default is 0, meaning no + quantization is applied. + noise_density : float, list, optional + The noise density of the sensor for a Gaussian white noise in m/s^2/√Hz. + Sometimes called "white noise drift", "angular random walk" for + gyroscopes, "velocity random walk" for accelerometers or + "(rate) noise density". Default is 0, meaning no noise is applied. + If a float or int is given, the same noise density is applied to all + axes. The values of each axis can be set individually by passing a + list of length 3. + noise_variance : float, list, optional + The noise variance of the sensor for a Gaussian white noise in (m/s^2)^2. + Default is 1, meaning the noise is normally distributed with a + standard deviation of 1 m/s^2. If a float or int is given, the same + variance is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. + random_walk_density : float, list, optional + The random walk of the sensor for a Gaussian random walk in m/s^2/√Hz. + Sometimes called "bias (in)stability" or "bias drift"". Default is 0, + meaning no random walk is applied. If a float or int is given, the + same random walk is applied to all axes. The values of each axis can + be set individually by passing a list of length 3. + random_walk_variance : float, list, optional + The random walk variance of the sensor for a Gaussian random walk in + (m/s^2)^2. Default is 1, meaning the noise is normally distributed + with a standard deviation of 1 m/s^2. If a float or int is given, + the same variance is applied to all axes. The values of each axis + can be set individually by passing a list of length 3. + constant_bias : float, list, optional + The constant bias of the sensor in m/s^2. Default is 0, meaning no + constant bias is applied. If a float or int is given, the same bias + is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. + operating_temperature : float, optional + The operating temperature of the sensor in degrees Celsius. At 25°C, + the temperature bias and scale factor are 0. Default is 25. + temperature_bias : float, list, optional + The temperature bias of the sensor in m/s^2/°C. Default is 0, + meaning no temperature bias is applied. If a float or int is given, + the same temperature bias is applied to all axes. The values of each + axis can be set individually by passing a list of length 3. + temperature_scale_factor : float, list, optional + The temperature scale factor of the sensor in %/°C. Default is 0, + meaning no temperature scale factor is applied. If a float or int is + given, the same temperature scale factor is applied to all axes. The + values of each axis can be set individually by passing a list of + length 3. + cross_axis_sensitivity : float, optional + Skewness of the sensor's axes in percentage. Default is 0, meaning + no cross-axis sensitivity is applied. + consider_gravity : bool, optional + If True, the sensor will consider the effect of gravity on the + acceleration. Default is False. + name : str, optional + The name of the sensor. Default is "Accelerometer". + + Returns + ------- + None + + See Also + -------- + TODO link to documentation on noise model + """ + super().__init__( + sampling_rate, + orientation, + measurement_range=measurement_range, + resolution=resolution, + noise_density=noise_density, + noise_variance=noise_variance, + random_walk_density=random_walk_density, + random_walk_variance=random_walk_variance, + constant_bias=constant_bias, + operating_temperature=operating_temperature, + temperature_bias=temperature_bias, + temperature_scale_factor=temperature_scale_factor, + cross_axis_sensitivity=cross_axis_sensitivity, + name=name, + ) + self.consider_gravity = consider_gravity + self.prints = _AccelerometerPrints(self) + + def measure(self, t, u, u_dot, relative_position, gravity, *args): + """Measure the acceleration of the rocket + + Parameters + ---------- + t : float + Current time + u : list + State vector of the rocket + u_dot : list + Derivative of the state vector of the rocket + relative_position : Vector + Position of the sensor relative to the rocket cdm + gravity : float + Acceleration due to gravity + """ + # Linear acceleration of rocket cdm in inertial frame + gravity = ( + Vector([0, 0, -gravity]) if self.consider_gravity else Vector([0, 0, 0]) + ) + a_I = Vector(u_dot[3:6]) + gravity + + # Vector from rocket cdm to sensor in rocket frame + r = relative_position + + # Angular velocity and accel of rocket + omega = Vector(u[10:13]) + omega_dot = Vector(u_dot[10:13]) + + # Measured acceleration at sensor position in inertial frame + A = ( + a_I + + Vector.cross(omega_dot, r) + + Vector.cross(omega, Vector.cross(omega, r)) + ) + # Transform to sensor frame + inertial_to_sensor = self._total_rotation_matrix @ Matrix.transformation( + u[6:10] + ) + A = inertial_to_sensor @ A + + # Apply noise + bias and quantize + A = self.apply_noise(A) + A = self.apply_temperature_drift(A) + A = self.quantize(A) + + self.measurement = tuple([*A]) + self._save_data((t, *A)) + + def export_measured_data(self, filename, format="csv"): + """ + Export the measured values to a file + + Parameters + ---------- + filename : str + Name of the file to export the values to + format : str + Format of the file to export the values to. Options are "csv" and + "json". Default is "csv". + + Returns + ------- + None + """ + if format.lower() not in ["json", "csv"]: + raise ValueError("Invalid format") + if format.lower() == "csv": + # if sensor has been added multiple times to the simulated rocket + if isinstance(self.measured_data[0], list): + print("Data saved to", end=" ") + for i, data in enumerate(self.measured_data): + with open(filename + f"_{i+1}", "w") as f: + f.write("t,ax,ay,az\n") + for t, ax, ay, az in data: + f.write(f"{t},{ax},{ay},{az}\n") + print(filename + f"_{i+1},", end=" ") + else: + with open(filename, "w") as f: + f.write("t,ax,ay,az\n") + for t, ax, ay, az in self.measured_data: + f.write(f"{t},{ax},{ay},{az}\n") + print(f"Data saved to {filename}") + return + if format.lower() == "json": + if isinstance(self.measured_data[0], list): + print("Data saved to", end=" ") + for i, data in enumerate(self.measured_data): + dict = {"t": [], "ax": [], "ay": [], "az": []} + for t, ax, ay, az in data: + dict["t"].append(t) + dict["ax"].append(ax) + dict["ay"].append(ay) + dict["az"].append(az) + with open(filename + f"_{i+1}", "w") as f: + json.dump(dict, f) + print(filename + f"_{i+1},", end=" ") + else: + dict = {"t": [], "ax": [], "ay": [], "az": []} + for t, ax, ay, az in self.measured_data: + dict["t"].append(t) + dict["ax"].append(ax) + dict["ay"].append(ay) + dict["az"].append(az) + with open(filename, "w") as f: + json.dump(dict, f) + print(f"Data saved to {filename}") + return diff --git a/rocketpy/sensors/gyroscope.py b/rocketpy/sensors/gyroscope.py new file mode 100644 index 000000000..26df61d4d --- /dev/null +++ b/rocketpy/sensors/gyroscope.py @@ -0,0 +1,335 @@ +import json + +import numpy as np + +from ..mathutils.vector_matrix import Matrix, Vector +from ..prints.sensors_prints import _GyroscopePrints +from ..sensors.sensors import Sensors + + +class Gyroscope(Sensors): + """Class for the gyroscope sensor + + Attributes + ---------- + acceleration_sensitivity : float, list + Sensitivity of the sensor to linear acceleration in rad/s/g. + prints : _GyroscopePrints + Object that contains the print functions for the sensor. + sampling_rate : float + Sample rate of the sensor in Hz. + orientation : tuple, list + Orientation of the sensor in the rocket. + measurement_range : float, tuple + The measurement range of the sensor in the rad/s. + resolution : float + The resolution of the sensor in rad/s/LSB. + noise_density : float, list + The noise density of the sensor in rad/s/√Hz. + noise_variance : float, list + The variance of the noise of the sensor in (rad/s)^2. + random_walk_density : float, list + The random walk density of the sensor in rad/s/√Hz. + random_walk_variance : float, list + The random walk variance of the sensor in (rad/s)^2. + constant_bias : float, list + The constant bias of the sensor in rad/s. + operating_temperature : float + The operating temperature of the sensor in degrees Celsius. + temperature_bias : float, list + The temperature bias of the sensor in rad/s/°C. + temperature_scale_factor : float, list + The temperature scale factor of the sensor in %/°C. + cross_axis_sensitivity : float + The cross axis sensitivity of the sensor in percentage. + name : str + The name of the sensor. + rotation_matrix : Matrix + The rotation matrix of the sensor from the sensor frame to the rocket + frame of reference. + normal_vector : Vector + The normal vector of the sensor in the rocket frame of reference. + _random_walk_drift : Vector + The random walk drift of the sensor in rad/s. + measurement : float + The measurement of the sensor after quantization, noise and temperature + drift. + measured_data : list + The stored measured data of the sensor after quantization, noise and + temperature drift. + """ + + units = "rad/s" + + def __init__( + self, + sampling_rate, + orientation=(0, 0, 0), + measurement_range=np.inf, + resolution=0, + noise_density=0, + noise_variance=1, + random_walk_density=0, + random_walk_variance=1, + constant_bias=0, + operating_temperature=25, + temperature_bias=0, + temperature_scale_factor=0, + cross_axis_sensitivity=0, + acceleration_sensitivity=0, + name="Gyroscope", + ): + """ + Initialize the gyroscope sensor + + Parameters + ---------- + sampling_rate : float + Sample rate of the sensor in Hz. + orientation : tuple, list, optional + Orientation of the sensor in the rocket. The orientation can be + given as: + - A list of length 3, where the elements are the Euler angles for + the rotation yaw (ψ), pitch (θ) and roll (φ) in radians. The + standard rotation sequence is z-y-x (3-2-1) is used, meaning the + sensor is first rotated by ψ around the x axis, then by θ around + the new y axis and finally by φ around the new z axis. + - A list of lists (matrix) of shape 3x3, representing the rotation + matrix from the sensor frame to the rocket frame. The sensor frame + of reference is defined as to have z axis along the sensor's normal + vector pointing upwards, x and y axes perpendicular to the z axis + and each other. + The rocket frame of reference is defined as to have z axis + along the rocket's axis of symmetry pointing upwards, x and y axes + perpendicular to the z axis and each other. Default is (0, 0, 0), + meaning the sensor is aligned with the rocket's axis of symmetry. + measurement_range : float, tuple, optional + The measurement range of the sensor in the rad/s. If a float, the + same range is applied both for positive and negative values. If a + tuple, the first value is the positive range and the second value is + the negative range. Default is np.inf. + resolution : float, optional + The resolution of the sensor in rad/s/LSB. Default is 0, meaning no + quantization is applied. + noise_density : float, list, optional + The noise density of the sensor for a Gaussian white noise in rad/s/√Hz. + Sometimes called "white noise drift", "angular random walk" for + gyroscopes, "velocity random walk" for the accelerometers or + "(rate) noise density". Default is 0, meaning no noise is applied. + If a float or int is given, the same noise density is applied to all + axes. The values of each axis can be set individually by passing a + list of length 3. + noise_variance : float, list, optional + The noise variance of the sensor for a Gaussian white noise in (rad/s)^2. + Default is 1, meaning the noise is normally distributed with a + standard deviation of 1 rad/s. If a float or int is given, the same + variance is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. + random_walk_density : float, list, optional + The random walk density of the sensor for a Gaussian random walk in + rad/s/√Hz. Sometimes called "bias (in)stability" or "bias drift"". + Default is 0, meaning no random walk is applied. If a float or int + is given, the same random walk is applied to all axes. The values of + each axis can be set individually by passing a list of length 3. + random_walk_variance : float, list, optional + The random walk variance of the sensor for a Gaussian random walk in + (rad/s)^2. Default is 1, meaning the random walk is normally + distributed with a standard deviation of 1 rad/s. If a float or int + is given, the same variance is applied to all axes. The values of + each axis can be set individually by passing a list of length 3. + constant_bias : float, list, optional + The constant bias of the sensor in rad/s. Default is 0, meaning no + constant bias is applied. If a float or int is given, the same bias + is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. + operating_temperature : float, optional + The operating temperature of the sensor in degrees Celsius. At 25°C, + the temperature bias and scale factor are 0. Default is 25. + temperature_sensitivity : float, list, optional + The temperature bias of the sensor in rad/s/°C. Default is 0, + meaning no temperature bias is applied. If a float or int is given, + the same temperature bias is applied to all axes. The values of each + axis can be set individually by passing a list of length 3. + temperature_scale_factor : float, list, optional + The temperature scale factor of the sensor in %/°C. Default is 0, + meaning no temperature scale factor is applied. If a float or int is + given, the same temperature scale factor is applied to all axes. The + values of each axis can be set individually by passing a list of + length 3. + cross_axis_sensitivity : float, optional + Skewness of the sensor's axes in percentage. Default is 0, meaning + no cross-axis sensitivity is applied. + of each axis can be set individually by passing a list of length 3. + acceleration_sensitivity : float, list, optional + Sensitivity of the sensor to linear acceleration in rad/s/g. Default + is 0, meaning no sensitivity to linear acceleration is applied. If a + float or int is given, the same sensitivity is applied to all axes. + The values of each axis can be set individually by passing a list of + length 3. + + Returns + ------- + None + + See Also + -------- + TODO link to documentation on noise model + """ + super().__init__( + sampling_rate, + orientation, + measurement_range=measurement_range, + resolution=resolution, + noise_density=noise_density, + noise_variance=noise_variance, + random_walk_density=random_walk_density, + random_walk_variance=random_walk_variance, + constant_bias=constant_bias, + operating_temperature=operating_temperature, + temperature_bias=temperature_bias, + temperature_scale_factor=temperature_scale_factor, + cross_axis_sensitivity=cross_axis_sensitivity, + name=name, + ) + self.acceleration_sensitivity = self._vectorize_input( + acceleration_sensitivity, "acceleration_sensitivity" + ) + self.prints = _GyroscopePrints(self) + + def measure(self, t, u, u_dot, relative_position, *args): + """Measure the angular velocity of the rocket + + Parameters + ---------- + t : float + Time at which the measurement is taken + u : list + State vector of the rocket + u_dot : list + Time derivative of the state vector of the rocket + relative_position : Vector + Vector from the rocket's center of mass to the sensor + """ + # Angular velocity of the rocket in the rocket frame + omega = Vector(u[10:13]) + + # Transform to sensor frame + inertial_to_sensor = self._total_rotation_matrix @ Matrix.transformation( + u[6:10] + ) + W = inertial_to_sensor @ omega + + # Apply noise + bias + # Apply noise + bias and quatize + W = self.apply_noise(W) + W = self.apply_temperature_drift(W) + + # Apply acceleration sensitivity + if self.acceleration_sensitivity != Vector.zeros(): + W += self.apply_acceleration_sensitivity( + omega, u_dot, relative_position, inertial_to_sensor + ) + + W = self.quantize(W) + + self.measurement = tuple([*W]) + self._save_data((t, *W)) + + def apply_acceleration_sensitivity( + self, omega, u_dot, relative_position, rotation_matrix + ): + """ + Apply acceleration sensitivity to the sensor measurement + + Parameters + ---------- + omega : Vector + The angular velocity to apply acceleration sensitivity to + u_dot : list + The time derivative of the state vector + relative_position : Vector + The vector from the rocket's center of mass to the sensor + rotation_matrix : Matrix + The rotation matrix from the rocket frame to the sensor frame + + Returns + ------- + Vector + The angular velocity with the acceleration sensitivity applied + """ + # Linear acceleration of rocket cdm in inertial frame + a_I = Vector(u_dot[3:6]) + + # Angular velocity and accel of rocket + omega_dot = Vector(u_dot[10:13]) + + # Acceleration felt in sensor + A = ( + a_I + + Vector.cross(omega_dot, relative_position) + + Vector.cross(omega, Vector.cross(omega, relative_position)) + ) + # Transform to sensor frame + A = rotation_matrix @ A + + return self.acceleration_sensitivity & A + + def export_measured_data(self, filename, format="csv"): + """ + Export the measured values to a file + + Parameters + ---------- + filename : str + Name of the file to export the values to + format : str + Format of the file to export the values to. Options are "csv" and + "json". Default is "csv". + + Returns + ------- + None + """ + if format.lower() not in ["csv", "json"]: + raise ValueError("Invalid format") + if format.lower() == "csv": + # if sensor has been added multiple times to the simulated rocket + if isinstance(self.measured_data[0], list): + print("Data saved to", end=" ") + for i, data in enumerate(self.measured_data): + with open(filename + f"_{i+1}", "w") as f: + f.write("t,wx,wy,wz\n") + for t, wx, wy, wz in data: + f.write(f"{t},{wx},{wy},{wz}\n") + print(filename + f"_{i+1},", end=" ") + else: + with open(filename, "w") as f: + f.write("t,wx,wy,wz\n") + for t, wx, wy, wz in self.measured_data: + f.write(f"{t},{wx},{wy},{wz}\n") + print(f"Data saved to {filename}") + return + if format.lower() == "json": + if isinstance(self.measured_data[0], list): + print("Data saved to", end=" ") + for i, data in enumerate(self.measured_data): + dict = {"t": [], "wx": [], "wy": [], "wz": []} + for t, wx, wy, wz in data: + dict["t"].append(t) + dict["wx"].append(wx) + dict["wy"].append(wy) + dict["wz"].append(wz) + with open(filename + f"_{i+1}", "w") as f: + json.dump(dict, f) + print(filename + f"_{i+1},", end=" ") + else: + dict = {"t": [], "wx": [], "wy": [], "wz": []} + for t, wx, wy, wz in self.measured_data: + dict["t"].append(t) + dict["wx"].append(wx) + dict["wy"].append(wy) + dict["wz"].append(wz) + with open(filename, "w") as f: + json.dump(dict, f) + print(f"Data saved to {filename}") + return diff --git a/rocketpy/sensors/sensors.py b/rocketpy/sensors/sensors.py new file mode 100644 index 000000000..eea0b9384 --- /dev/null +++ b/rocketpy/sensors/sensors.py @@ -0,0 +1,371 @@ +from abc import ABC, abstractmethod + +import numpy as np + +from rocketpy.mathutils.vector_matrix import Matrix, Vector + + +class Sensors(ABC): + """Abstract class for sensors + + Attributes + ---------- + sampling_rate : float + Sample rate of the sensor in Hz. + orientation : tuple, list + Orientation of the sensor in the rocket. + measurement_range : float, tuple + The measurement range of the sensor in the sensor units. + resolution : float + The resolution of the sensor in sensor units/LSB. + noise_density : float, list + The noise density of the sensor in sensor units/√Hz. + noise_variance : float, list + The variance of the noise of the sensor in sensor units^2. + random_walk_density : float, list + The random walk density of the sensor in sensor units/√Hz. + random_walk_variance : float, list + The variance of the random walk of the sensor in sensor units^2. + constant_bias : float, list + The constant bias of the sensor in sensor units. + operating_temperature : float + The operating temperature of the sensor in degrees Celsius. + temperature_bias : float, list + The temperature bias of the sensor in sensor units/°C. + temperature_scale_factor : float, list + The temperature scale factor of the sensor in %/°C. + cross_axis_sensitivity : float + The cross axis sensitivity of the sensor in percentage. + name : str + The name of the sensor. + rotation_matrix : Matrix + The rotation matrix of the sensor from the sensor frame to the rocket + frame of reference. + normal_vector : Vector + The normal vector of the sensor in the rocket frame of reference. + _random_walk_drift : Vector + The random walk drift of the sensor in sensor units. + measurement : float + The measurement of the sensor after quantization, noise and temperature + drift. + measured_data : list + The stored measured data of the sensor after quantization, noise and + temperature drift. + """ + + def __init__( + self, + sampling_rate, + orientation=(0, 0, 0), + measurement_range=np.inf, + resolution=0, + noise_density=0, + noise_variance=1, + random_walk_density=0, + random_walk_variance=1, + constant_bias=0, + operating_temperature=25, + temperature_bias=0, + temperature_scale_factor=0, + cross_axis_sensitivity=0, + name="Sensor", + ): + """ + Initialize the accelerometer sensor + + Parameters + ---------- + sampling_rate : float + Sample rate of the sensor + orientation : tuple, list, optional + Orientation of the sensor in the rocket. The orientation can be + given as: + - A list of length 3, where the elements are the Euler angles for + the rotation yaw (ψ), pitch (θ) and roll (φ) in radians. The + standard rotation sequence is z-y-x (3-2-1) is used, meaning the + sensor is first rotated by ψ around the x axis, then by θ around + the new y axis and finally by φ around the new z axis. + TODO: x and y are not defined in the rocket class. User has no + way to know which axis is which. + - A list of lists (matrix) of shape 3x3, representing the rotation + matrix from the sensor frame to the rocket frame. The sensor frame + of reference is defined as to have z axis along the sensor's normal + vector pointing upwards, x and y axes perpendicular to the z axis + and each other. + The rocket frame of reference is defined as to have z axis + along the rocket's axis of symmetry pointing upwards, x and y axes + perpendicular to the z axis and each other. Default is (0, 0, 0), + meaning the sensor is aligned with the rocket's axis of symmetry. + measurement_range : float, tuple, optional + The measurement range of the sensor in the sensor units. If a float, + the same range is applied both for positive and negative values. If + a tuple, the first value is the positive range and the second value + is the negative range. Default is np.inf. + resolution : float, optional + The resolution of the sensor in sensor units/LSB. Default is 0, + meaning no quantization is applied. + noise_density : float, list, optional + The noise density of the sensor for a Gaussian white noise in sensor + units/√Hz. Sometimes called "white noise drift", + "angular random walk" for gyroscopes, "velocity random walk" for + accelerometers or "(rate) noise density". Default is 0, meaning no + noise is applied. If a float or int is given, the same noise density + is applied to all axes. The values of each axis can be set + individually by passing a list of length 3. + noise_variance : float, list, optional + The noise variance of the sensor for a Gaussian white noise in + sensor units^2. Default is 1, meaning the noise is normally + distributed with a standard deviation of 1 unit. If a float or int + is given, the same noise variance is applied to all axes. The values + of each axis can be set individually by passing a list of length 3. + random_walk_density : float, list, optional + The random walk density of the sensor for a Gaussian random walk in + sensor units/√Hz. Sometimes called "bias (in)stability" or + "bias drift". Default is 0, meaning no random walk is applied. + If a float or int is given, the same random walk is applied to all + axes. The values of each axis can be set individually by passing a + list of length 3. + random_walk_variance : float, list, optional + The random walk variance of the sensor for a Gaussian random walk in + sensor units^2. Default is 1, meaning the noise is normally + distributed with a standard deviation of 1 unit. If a float or int + is given, the same random walk variance is applied to all axes. The + values of each axis can be set individually by passing a list of + length 3. + constant_bias : float, list, optional + The constant bias of the sensor in sensor units. Default is 0, + meaning no constant bias is applied. If a float or int is given, the + same constant bias is applied to all axes. The values of each axis + can be set individually by passing a list of length 3. + operating_temperature : float, optional + The operating temperature of the sensor in degrees Celsius. At 25°C, + the temperature bias and scale factor are 0. Default is 25. + temperature_bias : float, list, optional + The temperature bias of the sensor in sensor units/°C. Default is 0, + meaning no temperature bias is applied. If a float or int is given, + the same temperature bias is applied to all axes. The values of each + axis can be set individually by passing a list of length 3. + temperature_scale_factor : float, list, optional + The temperature scale factor of the sensor in %/°C. Default is 0, + meaning no temperature scale factor is applied. If a float or int is + given, the same temperature scale factor is applied to all axes. The + values of each axis can be set individually by passing a list of + length 3. + cross_axis_sensitivity : float, optional + Skewness of the sensor's axes in percentage. Default is 0, meaning + no cross-axis sensitivity is applied. + name : str, optional + The name of the sensor. Default is "Sensor". + + Returns + ------- + None + + See Also + -------- + TODO link to documentation on noise model + """ + self.sampling_rate = sampling_rate + self.orientation = orientation + self.resolution = resolution + self.operating_temperature = operating_temperature + self.noise_density = self._vectorize_input(noise_density, "noise_density") + self.noise_variance = self._vectorize_input(noise_variance, "noise_variance") + self.random_walk_density = self._vectorize_input( + random_walk_density, "random_walk_density" + ) + self.random_walk_variance = self._vectorize_input( + random_walk_variance, "random_walk_variance" + ) + self.constant_bias = self._vectorize_input(constant_bias, "constant_bias") + self.temperature_bias = self._vectorize_input( + temperature_bias, "temperature_bias" + ) + self.temperature_scale_factor = self._vectorize_input( + temperature_scale_factor, "temperature_scale_factor" + ) + self.cross_axis_sensitivity = cross_axis_sensitivity + self.name = name + self._random_walk_drift = Vector([0, 0, 0]) + self.measurement = None + self.measured_data = [] + self._counter = 0 + self._save_data = self._save_data_single + + # handle measurement range + if isinstance(measurement_range, (tuple, list)): + if len(measurement_range) != 2: + raise ValueError("Invalid measurement range format") + self.measurement_range = measurement_range + elif isinstance(measurement_range, (int, float)): + self.measurement_range = (-measurement_range, measurement_range) + else: + raise ValueError("Invalid measurement range format") + + # rotation matrix and normal vector + if any(isinstance(row, (tuple, list)) for row in orientation): # matrix + self.rotation_matrix = Matrix(orientation) + elif len(orientation) == 3: # euler angles + self.rotation_matrix = Matrix.transformation_euler_angles( + *orientation + ).round(12) + else: + raise ValueError("Invalid orientation format") + self.normal_vector = Vector( + [ + self.rotation_matrix[0][2], + self.rotation_matrix[1][2], + self.rotation_matrix[2][2], + ] + ).unit_vector + + # cross axis sensitivity matrix + _cross_axis_matrix = 0.01 * Matrix( + [ + [100, self.cross_axis_sensitivity, self.cross_axis_sensitivity], + [self.cross_axis_sensitivity, 100, self.cross_axis_sensitivity], + [self.cross_axis_sensitivity, self.cross_axis_sensitivity, 100], + ] + ) + + # compute total rotation matrix given cross axis sensitivity + self._total_rotation_matrix = self.rotation_matrix @ _cross_axis_matrix + + # map which rocket(s) the sensor is attached to and how many times + self._attached_rockets = {} + + def __repr__(self): + return f"{self.name}" + + def __call__(self, *args, **kwargs): + return self.measure(*args, **kwargs) + + def _vectorize_input(self, value, name): + if isinstance(value, (int, float)): + return Vector([value, value, value]) + elif isinstance(value, (tuple, list)): + return Vector(value) + else: + raise ValueError(f"Invalid {name} format") + + def _reset(self, simulated_rocket): + """Reset the sensor data for a new simulation.""" + self._random_walk_drift = Vector([0, 0, 0]) + self.measured_data = [] + if self._attached_rockets[simulated_rocket] > 1: + self.measured_data = [ + [] for _ in range(self._attached_rockets[simulated_rocket]) + ] + self._save_data = self._save_data_multiple + else: + self._save_data = self._save_data_single + + def _save_data_single(self, data): + """Save the measured data to the sensor data list for a sensor that is + added only once to the simulated rocket.""" + self.measured_data.append(data) + + def _save_data_multiple(self, data): + """Save the measured data to the sensor data list for a sensor that is + added multiple times to the simulated rocket.""" + self.measured_data[self._counter].append(data) + # counter for cases where the sensor is added multiple times in a rocket + self._counter += 1 + if self._counter == len(self.measured_data): + self._counter = 0 + + @abstractmethod + def measure(self, *args, **kwargs): + pass + + @abstractmethod + def export_measured_data(self): + pass + + def quantize(self, value): + """ + Quantize the sensor measurement + + Parameters + ---------- + value : float + The value to quantize + + Returns + ------- + float + The quantized value + """ + x = min(max(value.x, self.measurement_range[0]), self.measurement_range[1]) + y = min(max(value.y, self.measurement_range[0]), self.measurement_range[1]) + z = min(max(value.z, self.measurement_range[0]), self.measurement_range[1]) + if self.resolution != 0: + x = round(x / self.resolution) * self.resolution + y = round(y / self.resolution) * self.resolution + z = round(z / self.resolution) * self.resolution + return Vector([x, y, z]) + + def apply_noise(self, value): + """ + Add noise to the sensor measurement + + Parameters + ---------- + value : float + The value to add noise to + + Returns + ------- + float + The value with added noise + """ + # white noise + white_noise = ( + Vector( + [np.random.normal(0, self.noise_variance[i] ** 0.5) for i in range(3)] + ) + & self.noise_density + ) * self.sampling_rate**0.5 + + # random walk + self._random_walk_drift = ( + self._random_walk_drift + + ( + Vector( + [ + np.random.normal(0, self.random_walk_variance[i] ** 0.5) + for i in range(3) + ] + ) + & self.random_walk_density + ) + / self.sampling_rate**0.5 + ) + + # add noise + value += white_noise + self._random_walk_drift + self.constant_bias + + return value + + def apply_temperature_drift(self, value): + """ + Apply temperature drift to the sensor measurement + + Parameters + ---------- + value : float + The value to apply temperature drift to + + Returns + ------- + float + The value with applied temperature drift + """ + # temperature drift + value += (self.operating_temperature - 25) * self.temperature_bias + # temperature scale factor + scale_factor = ( + Vector([1, 1, 1]) + + (self.operating_temperature - 25) / 100 * self.temperature_scale_factor + ) + return value & scale_factor diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index badbc31e4..5d8028224 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -1,3 +1,4 @@ +import json import math import warnings from copy import deepcopy @@ -593,7 +594,6 @@ def __init__( if self.rail_length <= 0: raise ValueError("Rail length must be a positive value.") self.parachutes = self.rocket.parachutes[:] - self._controllers = self.rocket._controllers[:] self.inclination = inclination self.heading = heading self.max_time = max_time @@ -606,8 +606,8 @@ def __init__( self.terminate_on_apogee = terminate_on_apogee self.name = name self.equations_of_motion = equations_of_motion - # Flight initialization + self.__init_controllers() self.__init_post_process_variables() self.__init_solution_monitors() self.__init_equations_of_motion() @@ -662,17 +662,20 @@ def __init__( # Initialize phase time nodes phase.TimeNodes = TimeNodes() # Add first time node to permanent list - phase.TimeNodes.add_node(phase.t, [], []) + phase.TimeNodes.add_node(phase.t, [], [], []) # Add non-overshootable parachute time nodes if self.time_overshoot is False: phase.TimeNodes.add_parachutes( self.parachutes, phase.t, phase.time_bound ) + phase.TimeNodes.add_sensors( + self.rocket.sensors, phase.t, phase.time_bound + ) phase.TimeNodes.add_controllers( self._controllers, phase.t, phase.time_bound ) # Add lst time node to permanent list - phase.TimeNodes.add_node(phase.time_bound, [], []) + phase.TimeNodes.add_node(phase.time_bound, [], [], []) # Sort time nodes phase.TimeNodes.sort() # Merge equal time nodes @@ -702,8 +705,28 @@ def __init__( for callback in node.callbacks: callback(self) + if self.sensors: + # u_dot for all sensors + u_dot = phase.derivative(self.t, self.y_sol) + for sensor, position in node._component_sensors: + relative_position = position - self.rocket._csys * Vector( + [0, 0, self.rocket.center_of_dry_mass_position] + ) + sensor.measure( + self.t, + self.y_sol, + u_dot, + relative_position, + self.env.gravity(self.solution[-1][3]), + ) + for controller in node._controllers: - controller(self.t, self.y_sol, self.solution) + controller( + self.t, + self.y_sol, + self.solution, + self.sensors, + ) for parachute in node.parachutes: # Calculate and save pressure signal @@ -718,7 +741,9 @@ def __init__( self.env.barometric_height(pressure + noise) - self.env.elevation ) - if parachute.triggerfunc(pressure + noise, hAGL, self.y_sol): + if parachute.triggerfunc( + pressure + noise, hAGL, self.y_sol, self.sensors + ): # print('\nEVENT DETECTED') # print('Parachute Triggered') # print('Name: ', parachute.name, ' | Lag: ', parachute.lag) @@ -750,7 +775,7 @@ def __init__( ) # Prepare to leave loops and start new flight phase phase.TimeNodes.flush_after(node_index) - phase.TimeNodes.add_node(self.t, [], []) + phase.TimeNodes.add_node(self.t, [], [], []) phase.solver.status = "finished" # Save parachute event self.parachute_events.append([self.t, parachute]) @@ -868,7 +893,7 @@ def __init__( ) # Prepare to leave loops and start new flight phase phase.TimeNodes.flush_after(node_index) - phase.TimeNodes.add_node(self.t, [], []) + phase.TimeNodes.add_node(self.t, [], [], []) phase.solver.status = "finished" # Check for apogee event @@ -902,7 +927,7 @@ def __init__( self.FlightPhases.add_phase(self.t) # Prepare to leave loops and start new flight phase phase.TimeNodes.flush_after(node_index) - phase.TimeNodes.add_node(self.t, [], []) + phase.TimeNodes.add_node(self.t, [], [], []) phase.solver.status = "finished" # Check for impact event if self.y_sol[2] < self.env.elevation: @@ -965,7 +990,7 @@ def __init__( self.FlightPhases.add_phase(self.t) # Prepare to leave loops and start new flight phase phase.TimeNodes.flush_after(node_index) - phase.TimeNodes.add_node(self.t, [], []) + phase.TimeNodes.add_node(self.t, [], [], []) phase.solver.status = "finished" # List and feed overshootable time nodes @@ -977,7 +1002,7 @@ def __init__( self.parachutes, self.solution[-2][0], self.t ) # Add last time node (always skipped) - overshootable_nodes.add_node(self.t, [], []) + overshootable_nodes.add_node(self.t, [], [], []) if len(overshootable_nodes) > 1: # Sort overshootable time nodes overshootable_nodes.sort() @@ -1026,7 +1051,10 @@ def __init__( ) if parachute.triggerfunc( - pressure + noise, hAGL, overshootable_node.y + pressure + noise, + hAGL, + overshootable_node.y, + self.sensors, ): # print('\nEVENT DETECTED') # print('Parachute Triggered') @@ -1069,15 +1097,23 @@ def __init__( overshootable_index ) phase.TimeNodes.flush_after(node_index) - phase.TimeNodes.add_node(self.t, [], []) + phase.TimeNodes.add_node(self.t, [], [], []) phase.solver.status = "finished" # Save parachute event self.parachute_events.append( [self.t, parachute] ) + # If controlled flight, post process must be done on sim time + if self._controllers: + phase.derivative(self.t, self.y_sol, post_processing=True) + self.t_final = self.t self._calculate_pressure_signal() + if self._controllers: + self.__cache_post_process_variables() + if self.sensors: + self.__cache_sensor_data() if verbose: print("Simulation Completed at Time: {:3.4f} s".format(self.t)) @@ -1089,6 +1125,25 @@ def __init_post_process_variables(self): self._bearing = Function(0) self._latitude = Function(0) self._longitude = Function(0) + # Initialize state derivatives, force and atmospheric arrays + self.ax_list = [] + self.ay_list = [] + self.az_list = [] + self.alpha1_list = [] + self.alpha2_list = [] + self.alpha3_list = [] + self.R1_list = [] + self.R2_list = [] + self.R3_list = [] + self.M1_list = [] + self.M2_list = [] + self.M3_list = [] + self.pressure_list = [] + self.density_list = [] + self.dynamic_viscosity_list = [] + self.speed_of_sound_list = [] + self.wind_velocity_x_list = [] + self.wind_velocity_y_list = [] def __init_solution_monitors(self): # Initialize solution monitors @@ -1161,6 +1216,11 @@ def __init_flight_state(self): self.out_of_rail_time = self.initial_solution[0] self.out_of_rail_time_index = 0 self.initial_derivative = self.u_dot_generalized + if self._controllers: + # Handle post process during simulation, get initial accel/forces + self.initial_derivative( + self.t_initial, self.initial_solution[1:], post_processing=True + ) def __init_solver_monitors(self): # Initialize solver monitors @@ -1181,10 +1241,59 @@ def __init_equations_of_motion(self): if self.equations_of_motion == "solid_propulsion": self.u_dot_generalized = self.u_dot - def __init_equations_of_motion(self): - """Initialize equations of motion.""" - if self.equations_of_motion == "solid_propulsion": - self.u_dot_generalized = self.u_dot + def __init_controllers(self): + """Initialize controllers and sensors""" + self._controllers = self.rocket._controllers[:] + self.sensors = self.rocket.sensors.get_components() + if self._controllers or self.sensors: + if self.time_overshoot == True: + self.time_overshoot = False + warnings.warn( + "time_overshoot has been set to False due to the presence " + "of controllers or sensors. " + ) + # reset controllable object to initial state (only airbrakes for now) + for air_brakes in self.rocket.air_brakes: + air_brakes._reset() + + self.sensor_data = {} + for sensor in self.sensors: + sensor._reset(self.rocket) # resets noise and measurement list + self.sensor_data[sensor] = [] + + def __cache_post_process_variables(self): + """Cache post-process variables for simulations with controllers.""" + self.__retrieve_arrays = [ + self.ax_list, + self.ay_list, + self.az_list, + self.alpha1_list, + self.alpha2_list, + self.alpha3_list, + self.R1_list, + self.R2_list, + self.R3_list, + self.M1_list, + self.M2_list, + self.M3_list, + self.pressure_list, + self.density_list, + self.dynamic_viscosity_list, + self.speed_of_sound_list, + self.wind_velocity_x_list, + self.wind_velocity_y_list, + ] + + def __cache_sensor_data(self): + """Cache sensor data for simulations with sensors.""" + sensor_data = {} + sensors = [] + for sensor in self.sensors: + # skip sensors that are used more then once in the rocket + if sensor not in sensors: + sensors.append(sensor) + sensor_data[sensor] = sensor.measured_data[:] + self.sensor_data = sensor_data @cached_property def effective_1rl(self): @@ -1261,11 +1370,6 @@ def udot_rail1(self, t, u, post_processing=False): e0dot, e1dot, e2dot, e3dot, alpha1, alpha2, alpha3]. """ - # Check if post processing mode is on - if post_processing: - # Use u_dot post processing code - return self.u_dot_generalized(t, u, True) - # Retrieve integration data x, y, z, vx, vy, vz, e0, e1, e2, e3, omega1, omega2, omega3 = u @@ -1296,6 +1400,17 @@ def udot_rail1(self, t, u, post_processing=False): else: ax, ay, az = 0, 0, 0 + if post_processing: + # Use u_dot post processing code for forces, moments and env data + self.u_dot_generalized(t, u, post_processing=True) + # Save feasible accelerations + self.ax_list[-1] = [t, ax] + self.ay_list[-1] = [t, ay] + self.az_list[-1] = [t, az] + self.alpha1_list[-1] = [t, 0] + self.alpha2_list[-1] = [t, 0] + self.alpha3_list[-1] = [t, 0] + return [vx, vy, vz, ax, ay, az, 0, 0, 0, 0, 0, 0, 0] def udot_rail2(self, t, u, post_processing=False): @@ -1585,6 +1700,13 @@ def u_dot(self, t, u, post_processing=False): ] if post_processing: + # Accelerations + self.ax_list.append([t, ax]) + self.ay_list.append([t, ay]) + self.az_list.append([t, az]) + self.alpha1_list.append([t, alpha1]) + self.alpha2_list.append([t, alpha2]) + self.alpha3_list.append([t, alpha3]) # Dynamics variables self.R1_list.append([t, R1]) self.R2_list.append([t, R2]) @@ -1860,6 +1982,13 @@ def u_dot_generalized(self, t, u, post_processing=False): u_dot = [*r_dot, *v_dot, *e_dot, *w_dot] if post_processing: + # Accelerations + self.ax_list.append([t, v_dot[0]]) + self.ay_list.append([t, v_dot[1]]) + self.az_list.append([t, v_dot[2]]) + self.alpha1_list.append([t, w_dot[0]]) + self.alpha2_list.append([t, w_dot[1]]) + self.alpha3_list.append([t, w_dot[2]]) # Dynamics variables self.R1_list.append([t, R1]) self.R2_list.append([t, R2]) @@ -1943,7 +2072,16 @@ def u_dot_parachute(self, t, u, post_processing=False): ay = Dy / (mp + ma) az = (Dz - 9.8 * mp) / (mp + ma) + u_dot = [vx, vy, vz, ax, ay, az, 0, 0, 0, 0, 0, 0, 0] + if post_processing: + # Accelerations + self.ax_list.append([t, ax]) + self.ay_list.append([t, ay]) + self.az_list.append([t, az]) + self.alpha1_list.append([t, 0]) + self.alpha2_list.append([t, 0]) + self.alpha3_list.append([t, 0]) # Dynamics variables self.R1_list.append([t, Dx]) self.R2_list.append([t, Dy]) @@ -1952,13 +2090,20 @@ def u_dot_parachute(self, t, u, post_processing=False): self.M2_list.append([t, 0]) self.M3_list.append([t, 0]) # Atmospheric Conditions - self.wind_velocity_x_list.append([t, self.env.wind_velocity_x(z)]) - self.wind_velocity_y_list.append([t, self.env.wind_velocity_y(z)]) - self.density_list.append([t, self.env.density(z)]) - self.dynamic_viscosity_list.append([t, self.env.dynamic_viscosity(z)]) - self.pressure_list.append([t, self.env.pressure(z)]) - self.speed_of_sound_list.append([t, self.env.speed_of_sound(z)]) - + self.wind_velocity_x_list.append( + [t, self.env.wind_velocity_x.get_value_opt(z)] + ) + self.wind_velocity_y_list.append( + [t, self.env.wind_velocity_y.get_value_opt(z)] + ) + self.density_list.append([t, self.env.density.get_value_opt(z)]) + self.dynamic_viscosity_list.append( + [t, self.env.dynamic_viscosity.get_value_opt(z)] + ) + self.pressure_list.append([t, self.env.pressure.get_value_opt(z)]) + self.speed_of_sound_list.append( + [t, self.env.speed_of_sound.get_value_opt(z)] + ) return [vx, vy, vz, ax, ay, az, 0, 0, 0, 0, 0, 0, 0] @cached_property @@ -2791,31 +2936,20 @@ def longitude(self): return np.column_stack((self.time, longitude)) @cached_property - def retrieve_acceleration_arrays(self): - """Retrieve acceleration arrays from the integration scheme - - Parameters - ---------- + def __retrieve_arrays(self): + """post processing function to retrieve arrays from the integration + scheme and store them in lists for further analysis. Returns ------- - ax: list - acceleration in x direction - ay: list - acceleration in y direction - az: list - acceleration in z direction - alpha1: list - angular acceleration in x direction - alpha2: list - angular acceleration in y direction - alpha3: list - angular acceleration in z direction + temp_values: list + List containing the following arrays: ``ax`` , ``ay`` , ``az`` , + ``alpha1`` , ``alpha2`` , ``alpha3`` , ``R1`` , ``R2`` , ``R3`` , + ``M1`` , ``M2`` , ``M3`` , ``pressure`` , ``density`` , + ``dynamic_viscosity`` , ``speed_of_sound`` , ``wind_velocity_x`` , + ``wind_velocity_y``. """ - # Initialize acceleration arrays - ax, ay, az = [[0, 0]], [[0, 0]], [[0, 0]] - alpha1, alpha2, alpha3 = [[0, 0]], [[0, 0]], [[0, 0]] - # Go through each time step and calculate accelerations + # Go through each time step and calculate forces and atmospheric values # Get flight phases for phase_index, phase in self.time_iterator(self.FlightPhases): init_time = phase.t @@ -2824,23 +2958,60 @@ def retrieve_acceleration_arrays(self): # Call callback functions for callback in phase.callbacks: callback(self) - # Loop through time steps in flight phase - for step in self.solution: # Can be optimized - if init_time < step[0] <= final_time: - # Get derivatives - u_dot = current_derivative(step[0], step[1:]) - # Get accelerations - ax_value, ay_value, az_value = u_dot[3:6] - alpha1_value, alpha2_value, alpha3_value = u_dot[10:] - # Save accelerations - ax.append([step[0], ax_value]) - ay.append([step[0], ay_value]) - az.append([step[0], az_value]) - alpha1.append([step[0], alpha1_value]) - alpha2.append([step[0], alpha2_value]) - alpha3.append([step[0], alpha3_value]) - - return ax, ay, az, alpha1, alpha2, alpha3 + # find index of initial and final time of phase in solution array + init_time_index = find_closest(self.time, init_time) + final_time_index = find_closest(self.time, final_time) + 1 + # Loop through time steps solution array + for step in self.solution[init_time_index:final_time_index]: + if init_time != step[0] or ( + init_time == self.t_initial and step[0] == self.t_initial + ): + # Call derivatives in post processing mode + current_derivative(step[0], step[1:], post_processing=True) + + temp_values = [ + self.ax_list, + self.ay_list, + self.az_list, + self.alpha1_list, + self.alpha2_list, + self.alpha3_list, + self.R1_list, + self.R2_list, + self.R3_list, + self.M1_list, + self.M2_list, + self.M3_list, + self.pressure_list, + self.density_list, + self.dynamic_viscosity_list, + self.speed_of_sound_list, + self.wind_velocity_x_list, + self.wind_velocity_y_list, + ] + + return temp_values + + @cached_property + def retrieve_acceleration_arrays(self): + """Retrieve acceleration arrays from the integration scheme + + Returns + ------- + ax_list: list + acceleration in x direction + ay_list: list + acceleration in y direction + az_list: list + acceleration in z direction + alpha1_list: list + angular acceleration in x direction + alpha2_list: list + angular acceleration in y direction + alpha3_list: list + angular acceleration in z direction + """ + return self.__retrieve_arrays[:6] @cached_property def retrieve_temporary_values_arrays(self): @@ -2877,54 +3048,7 @@ def retrieve_temporary_values_arrays(self): self.wind_velocity_y_list: list Wind velocity in y direction at each time step. """ - - # Initialize force and atmospheric arrays - self.R1_list = [] - self.R2_list = [] - self.R3_list = [] - self.M1_list = [] - self.M2_list = [] - self.M3_list = [] - self.pressure_list = [] - self.density_list = [] - self.dynamic_viscosity_list = [] - self.speed_of_sound_list = [] - self.wind_velocity_x_list = [] - self.wind_velocity_y_list = [] - - # Go through each time step and calculate forces and atmospheric values - # Get flight phases - for phase_index, phase in self.time_iterator(self.FlightPhases): - init_time = phase.t - final_time = self.FlightPhases[phase_index + 1].t - current_derivative = phase.derivative - # Call callback functions - for callback in phase.callbacks: - callback(self) - # Loop through time steps in flight phase - for step in self.solution: # Can be optimized - if init_time < step[0] <= final_time or ( - init_time == self.t_initial and step[0] == self.t_initial - ): - # Call derivatives in post processing mode - u_dot = current_derivative(step[0], step[1:], post_processing=True) - - temporary_values = [ - self.R1_list, - self.R2_list, - self.R3_list, - self.M1_list, - self.M2_list, - self.M3_list, - self.pressure_list, - self.density_list, - self.dynamic_viscosity_list, - self.speed_of_sound_list, - self.wind_velocity_x_list, - self.wind_velocity_y_list, - ] - - return temporary_values + return self.__retrieve_arrays[6:] def get_controller_observed_variables(self): """Retrieve the observed variables related to air brakes from the @@ -3260,7 +3384,31 @@ class attributes which are instances of the Function class. Usage encoding="utf-8", ) - return + def export_sensor_data(self, file_name, sensor=None): + """Exports sensors data to a file. The file format can be either .csv or + .json. + + Parameters + ---------- + file_name : str + The file name or path of the exported file. Example: flight_data.csv + Do not use forbidden characters, such as / in Linux/Unix and + `<, >, :, ", /, \\, | ?, *` in Windows. + sensor : Sensor, optional + The sensor to export data. If None, all sensors data will be exported. + Default is None. + """ + if sensor is None: + data_dict = {} + for used_sensor, measured_data in self.sensor_data.items(): + data_dict[used_sensor.name] = measured_data + else: + # export data of only that sensor + data_dict = {} + data_dict[sensor.name] = self.sensor_data[sensor] + with open(file_name, "w") as file: + json.dump(data_dict, file) + print("Sensor data exported to", file_name) def export_kml( self, @@ -3566,6 +3714,8 @@ def __init__(self, t, derivative=None, callbacks=None, clear=True): self.derivative = derivative self.callbacks = callbacks[:] if callbacks is not None else [] self.clear = clear + self.time_bound = None + self.TimeNodes = None def __repr__(self): if self.derivative is None: @@ -3594,8 +3744,8 @@ def __repr__(self): def add(self, time_node): self.list.append(time_node) - def add_node(self, t, parachutes, callbacks): - self.list.append(self.TimeNode(t, parachutes, callbacks)) + def add_node(self, t, parachutes, controllers, sensors): + self.list.append(self.TimeNode(t, parachutes, controllers, sensors)) def add_parachutes(self, parachutes, t_init, t_end): # Iterate over parachutes @@ -3603,7 +3753,7 @@ def add_parachutes(self, parachutes, t_init, t_end): # Calculate start of sampling time nodes pcDt = 1 / parachute.sampling_rate parachute_node_list = [ - self.TimeNode(i * pcDt, [parachute], []) + self.TimeNode(i * pcDt, [parachute], [], []) for i in range( math.ceil(t_init / pcDt), math.floor(t_end / pcDt) + 1 ) @@ -3616,7 +3766,7 @@ def add_controllers(self, controllers, t_init, t_end): # Calculate start of sampling time nodes controller_time_step = 1 / controller.sampling_rate controller_node_list = [ - self.TimeNode(i * controller_time_step, [], [controller]) + self.TimeNode(i * controller_time_step, [], [controller], []) for i in range( math.ceil(t_init / controller_time_step), math.floor(t_end / controller_time_step) + 1, @@ -3624,6 +3774,22 @@ def add_controllers(self, controllers, t_init, t_end): ] self.list += controller_node_list + def add_sensors(self, sensors, t_init, t_end): + # Iterate over sensors + for sensor_component_tuple in sensors: + # Calculate start of sampling time nodes + sensor_time_step = 1 / sensor_component_tuple.component.sampling_rate + sensor_node_list = [ + self.TimeNode( + i * sensor_time_step, [], [], [sensor_component_tuple] + ) + for i in range( + math.ceil(t_init / sensor_time_step), + math.floor(t_end / sensor_time_step) + 1, + ) + ] + self.list += sensor_node_list + def sort(self): self.list.sort(key=(lambda node: node.t)) @@ -3637,6 +3803,8 @@ def merge(self): if abs(node.t - self.tmp_list[-1].t) < 1e-7: self.tmp_list[-1].parachutes += node.parachutes self.tmp_list[-1].callbacks += node.callbacks + self.tmp_list[-1]._component_sensors += node._component_sensors + self.tmp_list[-1]._controllers += node._controllers # Add new node to tmp list if there is none with the same time else: self.tmp_list.append(node) @@ -3647,11 +3815,12 @@ def flush_after(self, index): del self.list[index + 1 :] class TimeNode: - def __init__(self, t, parachutes, controllers): + def __init__(self, t, parachutes, controllers, sensors): self.t = t self.parachutes = parachutes self.callbacks = [] self._controllers = controllers + self._component_sensors = sensors def __repr__(self): return ( @@ -3659,5 +3828,9 @@ def __repr__(self): + str(self.t) + " | Parachutes: " + str(len(self.parachutes)) + + " | Controllers: " + + str(len(self._controllers)) + + " | Sensors: " + + str(len(self._component_sensors)) + "}" ) diff --git a/rocketpy/tools.py b/rocketpy/tools.py index ccecffd4f..0cbd16628 100644 --- a/rocketpy/tools.py +++ b/rocketpy/tools.py @@ -1,6 +1,7 @@ import functools import importlib import importlib.metadata +import math import re import time from bisect import bisect_left @@ -16,7 +17,7 @@ def tuple_handler(value): """Transforms the input value into a tuple that - represents a range. If the input is an input or float, + represents a range. If the input is an int or float, the output is a tuple from zero to the input value. If the input is a tuple or list, the output is a tuple with the same range. @@ -467,6 +468,60 @@ def quaternions_to_nutation(e1, e2): return (180 / np.pi) * 2 * np.arcsin(-((e1**2 + e2**2) ** 0.5)) +def euler_to_quaternions(yaw, pitch, roll): + """Calculates the quaternions (Euler parameters) from the Euler angles in + yaw, pitch, and roll sequence (3-2-1). + + Parameters + ---------- + yaw : float + Euler angle due to yaw (phi) in degrees + pitch : float + Euler angle due to pitch (theta) in degrees + roll : float + Euler angle due to roll (psi) in degrees + + Returns + ------- + tuple + Tuple containing the Euler parameters e0, e1, e2, e3 + """ + phi = math.radians(yaw) + theta = math.radians(pitch) + psi = math.radians(roll) + cr = math.cos(phi / 2) + sr = math.sin(phi / 2) + cp = math.cos(theta / 2) + sp = math.sin(theta / 2) + cy = math.cos(psi / 2) + sy = math.sin(psi / 2) + e0 = cr * cp * cy + sr * sp * sy + e1 = sr * cp * cy - cr * sp * sy + e2 = cr * sp * cy + sr * cp * sy + e3 = cr * cp * sy - sr * sp * cy + return e0, e1, e2, e3 + + +def normalize_quaternions(quaternions): + """Normalizes the quaternions (Euler parameters) to have unit magnitude. + + Parameters + ---------- + quaternions : tuple + Tuple containing the Euler parameters e0, e1, e2, e3 + + Returns + ------- + tuple + Tuple containing the Euler parameters e0, e1, e2, e3 + """ + q_w, q_x, q_y, q_z = quaternions + q_norm = (q_w**2 + q_x**2 + q_y**2 + q_z**2) ** 0.5 + if q_norm == 0: + return 1, 0, 0, 0 + return q_w / q_norm, q_x / q_norm, q_y / q_norm, q_z / q_norm + + if __name__ == "__main__": import doctest diff --git a/tests/conftest.py b/tests/conftest.py index 4766b570a..e8eda67ce 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -13,6 +13,7 @@ "tests.fixtures.rockets.rocket_fixtures", "tests.fixtures.surfaces.surface_fixtures", "tests.fixtures.units.numerical_fixtures", + "tests.fixtures.sensors.sensors_fixtures", ] diff --git a/tests/fixtures/flight/flight_fixtures.py b/tests/fixtures/flight/flight_fixtures.py index c8f73d52c..9976ddac2 100644 --- a/tests/fixtures/flight/flight_fixtures.py +++ b/tests/fixtures/flight/flight_fixtures.py @@ -158,3 +158,33 @@ def flight_calisto_air_brakes(calisto_air_brakes_clamp_on, example_plain_env): time_overshoot=False, terminate_on_apogee=True, ) + + +@pytest.fixture +def flight_calisto_accel_gyro(calisto_accel_gyro, example_plain_env): + """A rocketpy.Flight object of the Calisto rocket. This uses the calisto + with an ideal accelerometer and a gyroscope. The environment is the simplest + possible, with no parameters set. + + Parameters + ---------- + calisto_accel_gyro : rocketpy.Rocket + An object of the Rocket class. + example_plain_env : rocketpy.Environment + An object of the Environment class. + + Returns + ------- + rocketpy.Flight + A rocketpy.Flight object of the Calisto rocket in a more complex + condition. + """ + return Flight( + rocket=calisto_accel_gyro, + environment=example_plain_env, + rail_length=5.2, + inclination=85, + heading=0, + time_overshoot=False, + terminate_on_apogee=True, + ) diff --git a/tests/fixtures/rockets/rocket_fixtures.py b/tests/fixtures/rockets/rocket_fixtures.py index 3a1c82988..0161f3950 100644 --- a/tests/fixtures/rockets/rocket_fixtures.py +++ b/tests/fixtures/rockets/rocket_fixtures.py @@ -243,6 +243,44 @@ def calisto_air_brakes_clamp_off(calisto_robust, controller_function): return calisto +@pytest.fixture +def calisto_accel_gyro( + calisto, + calisto_nose_cone, + calisto_tail, + calisto_trapezoidal_fins, + ideal_accelerometer, + ideal_gyroscope, +): + """Create an object class of the Rocket class to be used in the tests. This + is the same Calisto rocket that was defined in the calisto fixture, but with + an ideal accelerometer and a gyroscope added at the center of dry mass. + Meaning the readings will be the same as the values saved on a Flight object. + + Parameters + ---------- + calisto : rocketpy.Rocket + An object of the Rocket class. This is a pytest fixture. + accelerometer : rocketpy.Accelerometer + An object of the Accelerometer class. This is a pytest fixture. + gyroscope : rocketpy.Gyroscope + An object of the Gyroscope class. This is a pytest fixture. + + Returns + ------- + rocketpy.Rocket + An object of the Rocket class + """ + calisto.add_surfaces(calisto_nose_cone, 1.160) + calisto.add_surfaces(calisto_tail, -1.313) + calisto.add_surfaces(calisto_trapezoidal_fins, -1.168) + # double sensors to test using same instance twice + calisto.add_sensor(ideal_accelerometer, -0.1180124376577797) + calisto.add_sensor(ideal_accelerometer, -0.1180124376577797) + calisto.add_sensor(ideal_gyroscope, -0.1180124376577797) + return calisto + + @pytest.fixture # old name: dimensionless_rocket def dimensionless_calisto(kg, m, dimensionless_cesaroni_m1670): """The dimensionless version of the Calisto rocket. This is the same rocket diff --git a/tests/fixtures/sensors/__init__.py b/tests/fixtures/sensors/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/tests/fixtures/sensors/sensors_fixtures.py b/tests/fixtures/sensors/sensors_fixtures.py new file mode 100644 index 000000000..c32a41124 --- /dev/null +++ b/tests/fixtures/sensors/sensors_fixtures.py @@ -0,0 +1,83 @@ +import pytest + +from rocketpy import Accelerometer, Gyroscope + + +@pytest.fixture +def noisy_rotated_accelerometer(): + """Returns an accelerometer with all parameters set to non-default values, + i.e. with noise and rotation.""" + # mpu6050 approx values, variances are made up + return Accelerometer( + sampling_rate=100, + orientation=(60, 60, 60), + noise_density=[0, 0.03, 0.05], + noise_variance=1.01, + random_walk_density=[0, 0.01, 0.02], + random_walk_variance=[1, 1, 1.05], + constant_bias=[0, 0.3, 0.5], + operating_temperature=25, + temperature_bias=[0, 0.01, 0.02], + temperature_scale_factor=[0, 0.01, 0.02], + cross_axis_sensitivity=0.5, + consider_gravity=True, + name="Accelerometer", + ) + + +@pytest.fixture +def noisy_rotated_gyroscope(): + """Returns a gyroscope with all parameters set to non-default values, + i.e. with noise and rotation.""" + # mpu6050 approx values, variances are made up + return Gyroscope( + sampling_rate=100, + orientation=(-60, -60, -60), + noise_density=[0, 0.03, 0.05], + noise_variance=1.01, + random_walk_density=[0, 0.01, 0.02], + random_walk_variance=[1, 1, 1.05], + constant_bias=[0, 0.3, 0.5], + operating_temperature=25, + temperature_bias=[0, 0.01, 0.02], + temperature_scale_factor=[0, 0.01, 0.02], + cross_axis_sensitivity=0.5, + acceleration_sensitivity=[0, 0.0008, 0.0017], + name="Gyroscope", + ) + + +@pytest.fixture +def quantized_accelerometer(): + """Returns an accelerometer with all parameters set to non-default values, + i.e. with noise and rotation.""" + return Accelerometer( + sampling_rate=100, + measurement_range=2, + resolution=0.4882, + ) + + +@pytest.fixture +def quantized_gyroscope(): + """Returns a gyroscope with all parameters set to non-default values, + i.e. with noise and rotation.""" + return Gyroscope( + sampling_rate=100, + measurement_range=15, + resolution=0.4882, + ) + + +@pytest.fixture +def ideal_accelerometer(): + return Accelerometer( + sampling_rate=100, + ) + + +@pytest.fixture +def ideal_gyroscope(): + return Gyroscope( + sampling_rate=100, + ) diff --git a/tests/test_sensors.py b/tests/test_sensors.py new file mode 100644 index 000000000..99ae7a0dd --- /dev/null +++ b/tests/test_sensors.py @@ -0,0 +1,63 @@ +import json +import os + +import numpy as np + +from rocketpy.mathutils.vector_matrix import Vector +from rocketpy.rocket.components import Components +from rocketpy.sensors.accelerometer import Accelerometer +from rocketpy.sensors.gyroscope import Gyroscope + + +def test_sensor_on_rocket(calisto_accel_gyro): + """Test the sensor on the rocket. + + Parameters + ---------- + calisto_accel_gyro : Rocket + Pytest fixture for the calisto rocket with an accelerometer and a gyroscope. + """ + sensors = calisto_accel_gyro.sensors + assert isinstance(sensors, Components) + assert isinstance(sensors[0].component, Accelerometer) + assert isinstance(sensors[1].position, Vector) + assert isinstance(sensors[2].component, Gyroscope) + assert isinstance(sensors[2].position, Vector) + + +def test_ideal_sensors(flight_calisto_accel_gyro): + """Test the ideal sensors. All types of sensors are here to reduce + testing time. + + Parameters + ---------- + flight_calisto_accel_gyro : Flight + Pytest fixture for the flight of the calisto rocket with an ideal accelerometer and a gyroscope. + """ + accelerometer = flight_calisto_accel_gyro.rocket.sensors[0].component + time, ax, ay, az = zip(*accelerometer.measured_data[0]) + ax = np.array(ax) + ay = np.array(ay) + az = np.array(az) + a = np.sqrt(ax**2 + ay**2 + az**2) + sim_accel = flight_calisto_accel_gyro.acceleration(time) + + # tolerance is bounded to numerical errors in the transformation matrixes + assert np.allclose(a, sim_accel, atol=1e-12) + # check if both added accelerometer instances saved the same data + assert ( + flight_calisto_accel_gyro.sensors[0].measured_data[0] + == flight_calisto_accel_gyro.sensors[0].measured_data[1] + ) + + gyroscope = flight_calisto_accel_gyro.rocket.sensors[2].component + time, wx, wy, wz = zip(*gyroscope.measured_data) + wx = np.array(wx) + wy = np.array(wy) + wz = np.array(wz) + w = np.sqrt(wx**2 + wy**2 + wz**2) + flight_wx = np.array(flight_calisto_accel_gyro.w1(time)) + flight_wy = np.array(flight_calisto_accel_gyro.w2(time)) + flight_wz = np.array(flight_calisto_accel_gyro.w3(time)) + sim_w = np.sqrt(flight_wx**2 + flight_wy**2 + flight_wz**2) + assert np.allclose(w, sim_w, atol=1e-12) diff --git a/tests/unit/test_flight.py b/tests/unit/test_flight.py index e6ab6b8b8..10ecbe4fe 100644 --- a/tests/unit/test_flight.py +++ b/tests/unit/test_flight.py @@ -1,3 +1,4 @@ +import json import os from unittest.mock import patch @@ -286,3 +287,44 @@ def test_out_of_rail_stability_margin(flight_calisto_custom_wind): flight_calisto_custom_wind.out_of_rail_time ) assert np.isclose(res, 2.14, atol=0.1) + + +def test_export_sensor_data(flight_calisto_accel_gyro): + """Test the export of sensor data. + + Parameters + ---------- + flight_calisto_accel_gyro : Flight + Pytest fixture for the flight of the calisto rocket with an ideal accelerometer and a gyroscope. + """ + flight_calisto_accel_gyro.export_sensor_data("test_sensor_data.json") + # read the json and parse as dict + filename = "test_sensor_data.json" + with open(filename, "r") as f: + data = f.read() + sensor_data = json.loads(data) + # convert list of tuples into list of lists to compare with the json + flight_calisto_accel_gyro.sensors[0].measured_data[0] = [ + list(measurement) + for measurement in flight_calisto_accel_gyro.sensors[0].measured_data[0] + ] + flight_calisto_accel_gyro.sensors[1].measured_data[1] = [ + list(measurement) + for measurement in flight_calisto_accel_gyro.sensors[1].measured_data[1] + ] + flight_calisto_accel_gyro.sensors[2].measured_data = [ + list(measurement) + for measurement in flight_calisto_accel_gyro.sensors[2].measured_data + ] + assert ( + sensor_data["Accelerometer"][0] + == flight_calisto_accel_gyro.sensors[0].measured_data[0] + ) + assert ( + sensor_data["Accelerometer"][1] + == flight_calisto_accel_gyro.sensors[1].measured_data[1] + ) + assert ( + sensor_data["Gyroscope"] == flight_calisto_accel_gyro.sensors[2].measured_data + ) + os.remove(filename) diff --git a/tests/unit/test_sensors.py b/tests/unit/test_sensors.py new file mode 100644 index 000000000..ff746e4ae --- /dev/null +++ b/tests/unit/test_sensors.py @@ -0,0 +1,315 @@ +import json +import os + +import numpy as np +import pytest +from pytest import approx + +from rocketpy.mathutils.vector_matrix import Matrix, Vector +from rocketpy.tools import euler_to_quaternions + +# calisto standard simulation no wind solution index 200 +TIME = 3.338513236767685 +U = [ + 0.02856482783411794, + 50.919436628139216, + 1898.9056294848442, + 0.021620542063162787, + 30.468683793837055, + 284.19140267225384, + -0.0076008223256743114, + 0.0004430927976100488, + 0.05330950836930627, + 0.9985245671704497, + 0.0026388673982115224, + 0.00010697759229808481, + 19.72526891699468, +] +U_DOT = [ + 0.021620542063162787, + 30.468683793837055, + 284.19140267225384, + 0.0009380154986373648, + 1.4853035773069556, + 4.377014845613867, + -9.848086239924413, + 0.5257087555505318, + -0.0030529818895471124, + -0.07503444684343626, + 0.028008532884449017, + -0.052789015849051935, + 2.276425320359305, +] +GRAVITY = 9.81 + + +@pytest.mark.parametrize( + "sensor", + [ + "noisy_rotated_accelerometer", + "quantized_accelerometer", + "noisy_rotated_gyroscope", + "quantized_gyroscope", + ], +) +def test_sensors_prints(sensor, request): + """Test the print methods of the Sensor class. Checks if all attributes are + printed correctly. + """ + sensor = request.getfixturevalue(sensor) + sensor.prints.all() + assert True + + +def test_rotation_matrix(noisy_rotated_accelerometer): + """Test the rotation_matrix property of the Accelerometer class. Checks if + the rotation matrix is correctly calculated. + """ + # values from external source + expected_matrix = np.array( + [ + [0.2500000, -0.0580127, 0.9665064], + [0.4330127, 0.8995190, -0.0580127], + [-0.8660254, 0.4330127, 0.2500000], + ] + ) + rotation_matrix = np.array(noisy_rotated_accelerometer.rotation_matrix.components) + assert np.allclose(expected_matrix, rotation_matrix, atol=1e-8) + + +def test_quantization(quantized_accelerometer): + """Test the quantize method of the Sensor class. Checks if returned values + are as expected. + """ + # expected values calculated by hand + assert quantized_accelerometer.quantize(Vector([3, 3, 3])) == Vector( + [1.9528, 1.9528, 1.9528] + ) + assert quantized_accelerometer.quantize(Vector([-3, -3, -3])) == Vector( + [-1.9528, -1.9528, -1.9528] + ) + assert quantized_accelerometer.quantize(Vector([1, 1, 1])) == Vector( + [0.9764, 0.9764, 0.9764] + ) + + +@pytest.mark.parametrize( + "sensor", + [ + "ideal_accelerometer", + "ideal_gyroscope", + ], +) +def test_measured_data(sensor, request): + """Test the measured_data property of the Sensors class. Checks if + the measured data is treated properly when the sensor is added once or more + than once to the rocket. + """ + sensor = request.getfixturevalue(sensor) + + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + assert len(sensor.measured_data) == 1 + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + assert len(sensor.measured_data) == 2 + assert all(isinstance(i, tuple) for i in sensor.measured_data) + + # check case when sensor is added more than once to the rocket + sensor.measured_data = [ + sensor.measured_data[:], + sensor.measured_data[:], + ] + sensor._save_data = sensor._save_data_multiple + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + assert len(sensor.measured_data) == 2 + assert len(sensor.measured_data[0]) == 3 + assert len(sensor.measured_data[1]) == 2 + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + assert len(sensor.measured_data[0]) == 3 + assert len(sensor.measured_data[1]) == 3 + + +def test_noisy_rotated_accelerometer(noisy_rotated_accelerometer): + """Test the measure method of the Accelerometer class. Checks if saved + measurement is (ax,ay,az) and if measured_data is [(t, (ax,ay,az)), ...] + """ + + # calculate acceleration at sensor position in inertial frame + relative_position = Vector([0.4, 0.4, 1]) + a_I = Vector(U_DOT[3:6]) + Vector([0, 0, -GRAVITY]) + omega = Vector(U[10:13]) + omega_dot = Vector(U_DOT[10:13]) + accel = ( + a_I + + Vector.cross(omega_dot, relative_position) + + Vector.cross(omega, Vector.cross(omega, relative_position)) + ) + + # calculate total rotation matrix + cross_axis_sensitivity = Matrix( + [ + [1, 0.005, 0.005], + [0.005, 1, 0.005], + [0.005, 0.005, 1], + ] + ) + sensor_rotation = Matrix.transformation(euler_to_quaternions(60, 60, 60)) + total_rotation = sensor_rotation @ cross_axis_sensitivity + rocket_rotation = Matrix.transformation(U[6:10]) + # expected measurement without noise + ax, ay, az = total_rotation @ (rocket_rotation @ accel) + # expected measurement with constant bias + ax += 0.5 + ay += 0.5 + az += 0.5 + + # check last measurement considering noise error bounds + noisy_rotated_accelerometer.measure(TIME, U, U_DOT, relative_position, GRAVITY) + assert noisy_rotated_accelerometer.measurement == approx([ax, ay, az], rel=0.1) + assert len(noisy_rotated_accelerometer.measurement) == 3 + assert noisy_rotated_accelerometer.measured_data[0][1:] == approx( + [ax, ay, az], rel=0.1 + ) + assert noisy_rotated_accelerometer.measured_data[0][0] == TIME + + +def test_noisy_rotated_gyroscope(noisy_rotated_gyroscope): + """Test the measure method of the Gyroscope class. Checks if saved + measurement is (wx,wy,wz) and if measured_data is [(t, (wx,wy,wz)), ...] + """ + # calculate acceleration at sensor position in inertial frame + relative_position = Vector([0.4, 0.4, 1]) + omega = Vector(U[10:13]) + # calculate total rotation matrix + cross_axis_sensitivity = Matrix( + [ + [1, 0.005, 0.005], + [0.005, 1, 0.005], + [0.005, 0.005, 1], + ] + ) + sensor_rotation = Matrix.transformation(euler_to_quaternions(-60, -60, -60)) + total_rotation = sensor_rotation @ cross_axis_sensitivity + rocket_rotation = Matrix.transformation(U[6:10]) + # expected measurement without noise + wx, wy, wz = total_rotation @ (rocket_rotation @ omega) + # expected measurement with constant bias + wx += 0.5 + wy += 0.5 + wz += 0.5 + + # check last measurement considering noise error bounds + noisy_rotated_gyroscope.measure(TIME, U, U_DOT, relative_position, GRAVITY) + assert noisy_rotated_gyroscope.measurement == approx([wx, wy, wz], rel=0.3) + assert len(noisy_rotated_gyroscope.measurement) == 3 + assert noisy_rotated_gyroscope.measured_data[0][1:] == approx([wx, wy, wz], rel=0.3) + assert noisy_rotated_gyroscope.measured_data[0][0] == TIME + + +@pytest.mark.parametrize( + "sensor, expected_string", + [ + ("ideal_accelerometer", "t,ax,ay,az\n"), + ("ideal_gyroscope", "t,wx,wy,wz\n"), + ], +) +def test_export_data_csv(sensor, expected_string, request): + """Test the export_data method of accelerometer. Checks if the data is + exported correctly. + + Parameters + ---------- + flight_calisto_accel_gyro : Flight + Pytest fixture for the flight of the calisto rocket with an ideal accelerometer and a gyroscope. + """ + sensor = request.getfixturevalue(sensor) + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + + file_name = "sensors.csv" + + sensor.export_measured_data(file_name, format="csv") + + with open(file_name, "r") as file: + contents = file.read() + + expected_data = expected_string + for t, x, y, z in sensor.measured_data: + expected_data += f"{t},{x},{y},{z}\n" + + assert contents == expected_data + + # check exports for accelerometers added more than once to the rocket + sensor.measured_data = [ + sensor.measured_data[:], + sensor.measured_data[:], + ] + sensor.export_measured_data(file_name, format="csv") + with open(file_name + "_1", "r") as file: + contents = file.read() + assert contents == expected_data + + with open(file_name + "_2", "r") as file: + contents = file.read() + assert contents == expected_data + + os.remove(file_name) + os.remove(file_name + "_1") + os.remove(file_name + "_2") + + +@pytest.mark.parametrize( + "sensor, expected_string", + [ + ("ideal_accelerometer", ("ax", "ay", "az")), + ("ideal_gyroscope", ("wx", "wy", "wz")), + ], +) +def test_export_data_json(sensor, expected_string, request): + """Test the export_data method of the accelerometer. Checks if the data is + exported correctly. + + Parameters + ---------- + flight_calisto_accel_gyro : Flight + Pytest fixture for the flight of the calisto rocket with an ideal + accelerometer and a gyroscope. + """ + sensor = request.getfixturevalue(sensor) + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + sensor.measure(TIME, U, U_DOT, Vector([0, 0, 0]), GRAVITY) + + file_name = "sensors.json" + + sensor.export_measured_data(file_name, format="json") + + contents = json.load(open(file_name, "r")) + + expected_data = { + "t": [], + expected_string[0]: [], + expected_string[1]: [], + expected_string[2]: [], + } + for t, x, y, z in sensor.measured_data: + expected_data["t"].append(t) + expected_data[expected_string[0]].append(x) + expected_data[expected_string[1]].append(y) + expected_data[expected_string[2]].append(z) + + assert contents == expected_data + + # check exports for accelerometers added more than once to the rocket + sensor.measured_data = [ + sensor.measured_data[:], + sensor.measured_data[:], + ] + sensor.export_measured_data(file_name, format="json") + contents = json.load(open(file_name + "_1", "r")) + assert contents == expected_data + + contents = json.load(open(file_name + "_2", "r")) + assert contents == expected_data + + os.remove(file_name) + os.remove(file_name + "_1") + os.remove(file_name + "_2") diff --git a/tests/unit/test_tools.py b/tests/unit/test_tools.py new file mode 100644 index 000000000..476451c85 --- /dev/null +++ b/tests/unit/test_tools.py @@ -0,0 +1,16 @@ +import numpy as np +import pytest + +from rocketpy.tools import euler_to_quaternions + + +@pytest.mark.parametrize( + "angles, expected_quaternions", + [((0, 0, 0), (1, 0, 0, 0)), ((90, 90, 90), (0.7071068, 0, 0.7071068, 0))], +) +def test_euler_to_quaternions(angles, expected_quaternions): + q0, q1, q2, q3 = euler_to_quaternions(*angles) + assert round(q0, 7) == expected_quaternions[0] + assert round(q1, 7) == expected_quaternions[1] + assert round(q2, 7) == expected_quaternions[2] + assert round(q3, 7) == expected_quaternions[3] diff --git a/tests/unit/test_tools_matrix.py b/tests/unit/test_tools_matrix.py index b43818450..bd33c4940 100644 --- a/tests/unit/test_tools_matrix.py +++ b/tests/unit/test_tools_matrix.py @@ -242,3 +242,21 @@ def test_matrix_transformation(): q1, q2, q3 = np.sin(phi / 2) * n matrix = Matrix.transformation((q0, q1, q2, q3)) assert matrix @ Vector([0, 0, 1]) == Vector([0, -1, 0]) + + +def test_matrix_transformation_euler_angles(): + phi = 90 + theta = 0 + psi = 0 + matrix = Matrix.transformation_euler_angles(phi, theta, psi) + matrix = matrix.round(12) + # Check that the matrix is orthogonal + assert matrix @ matrix.transpose == Matrix.identity() + # Check that the matrix rotates the vector correctly + assert matrix @ Vector([0, 0, 1]) == Vector([0, -1, 0]) + + +def test_matrix_round(): + matrix = [[2e-10, -2e-10, 0], [5.1234, -5.1234, 0], [0, 0, 9]] + matrix = Matrix(matrix).round(3) + assert matrix == Matrix([[0, 0, 0], [5.123, -5.123, 0], [0, 0, 9]])