From 70c43ed3e7bc851954c7e9bc66891a3442bef4df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 29 Aug 2024 15:41:59 -0700 Subject: [PATCH 1/4] Add rot2quat and quat2euler --- rednose/helpers/sympy_helpers.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/rednose/helpers/sympy_helpers.py b/rednose/helpers/sympy_helpers.py index 71a12d0..396d72d 100644 --- a/rednose/helpers/sympy_helpers.py +++ b/rednose/helpers/sympy_helpers.py @@ -27,6 +27,14 @@ def quat2rot(quats): return Rs +def rot2quat(Rs): + q0 = sp.sqrt(1 + Rs[:, 0, 0] + Rs[:, 1, 1] + Rs[: 2, 2]) / 2 + q1 = (Rs[:, 2, 1] - Rs[:, 1, 2]) / (4 * q0) + q2 = (Rs[:, 0, 2] - Rs[:, 2, 0]) / (4 * q0) + q3 = (Rs[:, 1, 0] - Rs[:, 0, 1]) / (4 * q0) + return sp.Matrix([q0, q1, q2, q3]) + + def euler2quat(eulers): eulers = np.array(eulers) if len(eulers.shape) > 1: @@ -52,9 +60,22 @@ def euler2quat(eulers): return quats.reshape(output_shape) +def quat2euler(quats): + q0, q1, q2, q3 = quats[:, 0], quats[:, 1], quats[:, 2], quats[:, 3] + gamma = sp.atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1**2 + q2**2)) + theta = sp.asin(2 * (q0*q2 - q3*q1)) + psi = sp.atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2**2 + q3**2)) + return sp.Matrix([gamma, theta, psi]) + + def euler2rot(eulers): return quat2rot(euler2quat(eulers)) + +def rot2euler(Rs): + return quat2euler(rot2quat(Rs)) + + rotations_from_quats = quat2rot From 212872f78e3d622da9625c96d51c9c4fc08d50ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 29 Aug 2024 16:07:37 -0700 Subject: [PATCH 2/4] Single element helpers --- rednose/helpers/sympy_helpers.py | 41 ++++++++++++++++---------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/rednose/helpers/sympy_helpers.py b/rednose/helpers/sympy_helpers.py index 396d72d..d3fbcc8 100644 --- a/rednose/helpers/sympy_helpers.py +++ b/rednose/helpers/sympy_helpers.py @@ -27,14 +27,6 @@ def quat2rot(quats): return Rs -def rot2quat(Rs): - q0 = sp.sqrt(1 + Rs[:, 0, 0] + Rs[:, 1, 1] + Rs[: 2, 2]) / 2 - q1 = (Rs[:, 2, 1] - Rs[:, 1, 2]) / (4 * q0) - q2 = (Rs[:, 0, 2] - Rs[:, 2, 0]) / (4 * q0) - q3 = (Rs[:, 1, 0] - Rs[:, 0, 1]) / (4 * q0) - return sp.Matrix([q0, q1, q2, q3]) - - def euler2quat(eulers): eulers = np.array(eulers) if len(eulers.shape) > 1: @@ -60,22 +52,10 @@ def euler2quat(eulers): return quats.reshape(output_shape) -def quat2euler(quats): - q0, q1, q2, q3 = quats[:, 0], quats[:, 1], quats[:, 2], quats[:, 3] - gamma = sp.atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1**2 + q2**2)) - theta = sp.asin(2 * (q0*q2 - q3*q1)) - psi = sp.atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2**2 + q3**2)) - return sp.Matrix([gamma, theta, psi]) - - def euler2rot(eulers): return quat2rot(euler2quat(eulers)) -def rot2euler(Rs): - return quat2euler(rot2quat(Rs)) - - rotations_from_quats = quat2rot @@ -84,9 +64,22 @@ def cross(x): ret[0, 1], ret[0, 2] = -x[2], x[1] ret[1, 0], ret[1, 2] = x[2], -x[0] ret[2, 0], ret[2, 1] = -x[1], x[0] + sp.expand return ret +def rot_to_euler(R): + return quat_to_euler(rot_to_quat(R)) + + +def quat_to_euler(quats): + q0, q1, q2, q3 = quats + gamma = sp.atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1**2 + q2**2)) + theta = sp.asin(2 * (q0*q2 - q3*q1)) + psi = sp.atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2**2 + q3**2)) + 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) @@ -118,6 +111,14 @@ def quat_rotate(q0, q1, q2, q3): [2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T +def rot_to_quat(R): + q0 = sp.sqrt(1 + R[ 0, 0] + R[1, 1] + R[2, 2]) / 2 + q1 = (R[2, 1] - R[1, 2]) / (4 * q0) + q2 = (R[0, 2] - R[2, 0]) / (4 * q0) + q3 = (R[1, 0] - R[0, 1]) / (4 * q0) + return sp.Matrix([q0, q1, q2, q3]) + + def quat_matrix_l(p): return sp.Matrix([[p[0], -p[1], -p[2], -p[3]], [p[1], p[0], -p[3], p[2]], From d63bae0210a539c2c21ac583aba80f0482fbac55 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 29 Aug 2024 16:38:01 -0700 Subject: [PATCH 3/4] Simplify --- rednose/helpers/sympy_helpers.py | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/rednose/helpers/sympy_helpers.py b/rednose/helpers/sympy_helpers.py index d3fbcc8..205452b 100644 --- a/rednose/helpers/sympy_helpers.py +++ b/rednose/helpers/sympy_helpers.py @@ -69,14 +69,9 @@ def cross(x): def rot_to_euler(R): - return quat_to_euler(rot_to_quat(R)) - - -def quat_to_euler(quats): - q0, q1, q2, q3 = quats - gamma = sp.atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1**2 + q2**2)) - theta = sp.asin(2 * (q0*q2 - q3*q1)) - psi = sp.atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2**2 + q3**2)) + 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]) @@ -111,14 +106,6 @@ def quat_rotate(q0, q1, q2, q3): [2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T -def rot_to_quat(R): - q0 = sp.sqrt(1 + R[ 0, 0] + R[1, 1] + R[2, 2]) / 2 - q1 = (R[2, 1] - R[1, 2]) / (4 * q0) - q2 = (R[0, 2] - R[2, 0]) / (4 * q0) - q3 = (R[1, 0] - R[0, 1]) / (4 * q0) - return sp.Matrix([q0, q1, q2, q3]) - - def quat_matrix_l(p): return sp.Matrix([[p[0], -p[1], -p[2], -p[3]], [p[1], p[0], -p[3], p[2]], From ddee08f108d097cddb3176bd7ec6a74e5adef70d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 29 Aug 2024 16:38:52 -0700 Subject: [PATCH 4/4] Remove trash --- rednose/helpers/sympy_helpers.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rednose/helpers/sympy_helpers.py b/rednose/helpers/sympy_helpers.py index 205452b..7f8813e 100644 --- a/rednose/helpers/sympy_helpers.py +++ b/rednose/helpers/sympy_helpers.py @@ -64,7 +64,6 @@ def cross(x): ret[0, 1], ret[0, 2] = -x[2], x[1] ret[1, 0], ret[1, 2] = x[2], -x[0] ret[2, 0], ret[2, 1] = -x[1], x[0] - sp.expand return ret