Add new convinience method for euler angles

armarx_core/math/transform.py

@@ -53,6 +53,8 @@ class Transform:
             self.rot_quat = value
         elif value.shape == (3, 3):
             self.rot_mat = value
+        elif value.shape == (3, ):
+            self.rot_mat = Transform.euler_to_rotation_matrix(value[0], value[1], value[2])
         else:
             raise ValueError(f"Invalid rotation value: {value}")
 
@@ -172,3 +174,33 @@ class Transform:
                 f"Expected {name} of shape {shape_str}, but got array of shape {value.shape}."
             )
         return value
+
+    @classmethod
+    def euler_to_rotation_matrix(cls, roll: float, pitch: float, yaw: float) -> np.ndarray:
+        """
+        Converts roll, pitch, yaw (in radians) to a 3x3 rotation matrix.
+
+        Parameters:
+            roll (float): Rotation around the x-axis (φ)
+            pitch (float): Rotation around the y-axis (θ)
+            yaw (float): Rotation around the z-axis (ψ)
+
+        Returns:
+            np.ndarray: 3x3 rotation matrix
+        """
+        # Compute individual rotation matrices
+        R_x = np.array([[1, 0, 0],
+                        [0, np.cos(roll), -np.sin(roll)],
+                        [0, np.sin(roll), np.cos(roll)]])
+
+        R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)],
+                        [0, 1, 0],
+                        [-np.sin(pitch), 0, np.cos(pitch)]])
+
+        R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0],
+                        [np.sin(yaw), np.cos(yaw), 0],
+                        [0, 0, 1]])
+
+        # Compute full rotation matrix: R = Rz * Ry * Rx
+        R = R_z @ R_y @ R_x
+        return R