From 1cc21d4859b001e5afe43d90a3b3bd8ac264f778 Mon Sep 17 00:00:00 2001
From: birr <timo.birr@kit.edu>
Date: Tue, 11 Mar 2025 19:13:24 +0100
Subject: [PATCH] Add new convinience method for euler angles
---
armarx_core/math/transform.py | 32 ++++++++++++++++++++++++++++++++
1 file changed, 32 insertions(+)
diff --git a/armarx_core/math/transform.py b/armarx_core/math/transform.py
index 1bec776c..2fdd664b 100644
--- a/armarx_core/math/transform.py
+++ b/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
--
GitLab