ENH: Sensors

rocketpy/__init__.py

@@ -37,4 +37,5 @@
     Tail,
     TrapezoidalFins,
 )
+from .sensors import Accelerometer, Gyroscope, Sensors
 from .simulation import Flight

rocketpy/control/controller.py

@@ -1,3 +1,5 @@
+from inspect import signature
+
 from ..prints.controller_prints import _ControllerPrints
 
 
@@ -52,6 +54,10 @@
                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 @@
         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 @@
         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 @@
             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 @@
             state_history,
             self.observed_variables,
             self.interactive_objects,
+            sensors,
         )
         if observed_variables is not None:
             self.observed_variables.append(observed_variables)

rocketpy/mathutils/vector_matrix.py

@@ -1,7 +1,7 @@
 from cmath import isclose
 from itertools import product
 
-from rocketpy.tools import cached_property
+from rocketpy.tools import cached_property, euler_to_quaternions
 
 
 class Vector:
@@ -153,7 +153,10 @@
     @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):
@@ -239,6 +242,29 @@
             ]
         )
 
+    def __and__(self, other):
+        """Element wise multiplication between two R3 vectors.
+
+        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
@@ -984,6 +1010,27 @@
             ]
         )
 
+    @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 radians.
+        pitch : float
+            The pitch angle in radians.
+        yaw : float
+            The yaw angle in radians.
+
+        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