ENH: Adds Sensors classes

.vscode/settings.json

@@ -132,11 +132,14 @@
         "github",
         "Glauert",
         "gmaps",
+        "Gnss",
         "Gomes",
         "Gonçalvez",
         "grav",
         "Guilherme",
         "Haim",
+        "headlength",
+        "headwidth",
         "hemis",
         "hgtprs",
         "hgtsfc",
@@ -245,6 +248,7 @@
         "reversesort",
         "reynolds",
         "rightarrow",
+        "rmul",
         "ROABs",
         "rocketpy",
         "rocketusage",

CHANGELOG.md

@@ -32,7 +32,8 @@ Attention: The newest changes should be on top -->
 
 ### Added
 
-- DOC : Cavour Flight Example [#682](https://github.com/RocketPy-Team/RocketPy/pull/682)
+- ENH: Adds Sensors classes [683](https://github.com/RocketPy-Team/RocketPy/pull/683)
+- DOC: Cavour Flight Example [#682](https://github.com/RocketPy-Team/RocketPy/pull/682)
 - DOC: Halcyon Flight Example [#681](https://github.com/RocketPy-Team/RocketPy/pull/681)
 - ENH: Adds GenericMotor.load_from_eng_file() method [#676](https://github.com/RocketPy-Team/RocketPy/pull/676)
 - ENH: Introducing local sensitivity analysis [#575](https://github.com/RocketPy-Team/RocketPy/pull/575)

docs/reference/classes/sensors/abstract/index.rst

@@ -0,0 +1,11 @@
+Sensors Abstract Classes
+========================
+
+.. toctree::
+   :maxdepth: 1
+   :caption: Contents:
+
+   Sensor Class <sensor>
+   Inertial Sensor Class <inertial_sensor>
+   Scalar Sensor Class <scalar_sensor>
+

docs/reference/classes/sensors/abstract/inertial_sensor.rst

@@ -0,0 +1,5 @@
+Inertial Class
+--------------
+
+.. autoclass:: rocketpy.sensors.InertialSensor
+    :members:

docs/reference/classes/sensors/abstract/scalar_sensor.rst

@@ -0,0 +1,5 @@
+Scalar Sensor Class
+-------------------
+
+.. autoclass:: rocketpy.sensors.ScalarSensor
+    :members:

docs/reference/classes/sensors/abstract/sensor.rst

@@ -0,0 +1,5 @@
+Sensor Class
+------------
+
+.. autoclass:: rocketpy.sensors.Barometer
+    :members:

docs/reference/classes/sensors/accelerometer.rst

@@ -0,0 +1,5 @@
+Accelerometer Class
+---------------
+
+.. autoclass:: rocketpy.sensors.Accelerometer
+    :members:

docs/reference/classes/sensors/barometer.rst

@@ -0,0 +1,5 @@
+Barometer Class
+---------------
+
+.. autoclass:: rocketpy.sensors.Barometer
+    :members:

docs/reference/classes/sensors/gnss_receiver.rst

@@ -0,0 +1,5 @@
+GNSS Receiver Class
+-------------------
+
+.. autoclass:: rocketpy.sensors.GnssReceiver
+    :members:

docs/reference/classes/sensors/gyroscope.rst

@@ -0,0 +1,5 @@
+Gyroscope Class
+---------------
+
+.. autoclass:: rocketpy.sensors.Gyroscope
+    :members:

docs/reference/classes/sensors/index.rst

@@ -0,0 +1,12 @@
+Sensor Classes
+==============
+
+.. toctree::
+   :maxdepth: 1
+   :caption: Contents:
+
+   Sensors Abstract Classes <abstract/index>
+   Accelerometer Class <accelerometer>
+   Barometer Class <barometer>
+   Gyroscope Class <gyroscope>
+   GNSS Receiver Class <gnss>

docs/user/index.rst

@@ -24,6 +24,7 @@ RocketPy's User Guide
    Compare Flights Class<compare_flights.rst>
    Deployable Payload <deployable.rst>
    Air Brakes Example <airbrakes.rst>
+   ../notebooks/sensors.ipynb
    ../matlab/matlab.rst
 
 .. toctree::

rocketpy/__init__.py

@@ -38,6 +38,7 @@
     TrapezoidalFins,
 )
 from .sensitivity import SensitivityModel
+from .sensors import Accelerometer, Barometer, GnssReceiver, Gyroscope
 from .simulation import Flight, MonteCarlo
 from .stochastic import (
     StochasticEllipticalFins,

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,45 @@ 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:
+
+            # pylint: disable=unused-argument
+            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 +148,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 +165,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)

rocketpy/environment/tools.py

@@ -532,7 +532,7 @@ def utm_to_geodesic(  # pylint: disable=too-many-locals,too-many-statements
     x, y, utm_zone, hemis, semi_major_axis=6378137.0, flattening=1 / 298.257223563
 ):
     # NOTE: already documented in the Environment class.
-    # TODO: deprecated the static method from the environment class, use only this one.
+    # TODO: deprecate the static method from the environment class, use only this one.
 
     if hemis == "N":
         y = y + 10000000

rocketpy/mathutils/vector_matrix.py

@@ -2,6 +2,8 @@
 from functools import cached_property
 from itertools import product
 
+from rocketpy.tools import euler321_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
@@ -914,6 +943,47 @@ def dot(self, other):
         """
         return self @ (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"
@@ -949,7 +1019,6 @@ 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
         -------
@@ -960,27 +1029,60 @@ def transformation(quaternion):
         ---------
         https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
         """
-        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(euler321_to_quaternions(roll, pitch, yaw))
+
 
 if __name__ == "__main__":
     import doctest