From 05fd60278a95190cc5a61f40730c82eaa1d986e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 29 Aug 2024 20:52:00 -0700 Subject: [PATCH] Add rot_to_euler sympy helper (#46) * Add rot2quat and quat2euler * Single element helpers * Simplify * Remove trash --- rednose/helpers/sympy_helpers.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rednose/helpers/sympy_helpers.py b/rednose/helpers/sympy_helpers.py index 71a12d0..7f8813e 100644 --- a/rednose/helpers/sympy_helpers.py +++ b/rednose/helpers/sympy_helpers.py @@ -55,6 +55,7 @@ def euler2quat(eulers): def euler2rot(eulers): return quat2rot(euler2quat(eulers)) + rotations_from_quats = quat2rot @@ -66,6 +67,13 @@ def cross(x): return ret +def rot_to_euler(R): + gamma = sp.atan2(R[2, 1], R[2, 2]) + theta = sp.asin(-R[2, 0]) + psi = sp.atan2(R[1, 0], R[0, 0]) + return sp.Matrix([gamma, theta, psi]) + + def rot_matrix(roll, pitch, yaw): cr, sr = np.cos(roll), np.sin(roll) cp, sp = np.cos(pitch), np.sin(pitch)