From 34b8326c7adfbd8f341d242788df4051a5154db3 Mon Sep 17 00:00:00 2001 From: Grant Sanderson Date: Thu, 27 Sep 2018 17:35:40 -0700 Subject: [PATCH] angle_axis_from_quaternion --- utils/space_ops.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/utils/space_ops.py b/utils/space_ops.py index dfb049fd..1ddf6f7f 100644 --- a/utils/space_ops.py +++ b/utils/space_ops.py @@ -34,6 +34,17 @@ def quaternion_from_angle_axis(angle, axis): ) +def angle_axis_from_quaternion(quaternion): + axis = normalize( + quaternion[1:], + fall_back=np.array([1, 0, 0]) + ) + angle = 2 * np.arccos(quaternion[0]) + if angle > TAU / 2: + angle = TAU - angle + return angle, axis + + def quaternion_conjugate(quaternion): result = np.array(quaternion) result[1:] *= -1