1
$\begingroup$

I am using quaternions to describe 3D rotations which parametrized by Euler angles, and as a preliminary task I am trying to implement conversion routines that go between Euler angles and quaternions.

Using the XYZ (1,2,3) Tait-Bryan angle description, I have implemented the following function to obtain the quaternion that corresponds to a rotation of angle $\theta$ around the vector $v$:

qa[v_, θ_] := Quaternion[Cos[θ/2], Sin[θ/2] v[[1]], Sin[θ/2] v[[2]], Sin[θ/2] v[[3]]];

The quaternion thus generated can be multiplied as follows to obtain the rotated of a vector $v$

q[θ1_, θ2_, θ3_] := qa[{0, 0, 1}, θ3] ** qa[{0, 1, 0}, θ2] ** qa[{1, 0, 0}, θ1];
quat[v_] := Quaternion[0, v[[1]], v[[2]], v[[3]]];

q1[θ1_, θ2_, θ3_] := Conjugate[q[θ1, θ2, θ3]];

RotQuat[v_, θ_] := q1[θ[[1]], θ[[2]], θ[[3]]] ** quat[v] ** q[θ[[1]], θ[[2]], θ[[3]]]

where $\theta_i$ are the Euler angles, $q1$ is the inverse unit quaternion of $q$.

I now want to be able to invert the function q[θ[[1]], θ[[2]], θ[[3]]], to obtain the Euler angles from the unit quaternion $q$.

To do this, I have used the formulae given by Diebel here, Wikipedia (via Blanco, here):

QuatToEuler[q_] := {ArcTan[(2 q[[3]] q[[4]] + 2 q[[1]] q[[2]]), (q[[1]]^2 + 
q[[4]]^2 - (q[[3]]^2 + q[[2]]^2))], -ArcSin[
2 q[[2]] q[[4]] - 2 q[[1]] q[[3]]], 
ArcTan[(2 q[[2]] q[[3]] + 2 q[[1]] q[[4]]), (-q[[4]]^2 - q[[3]]^2 + 
q[[2]]^2 + q[[1]]^2)]}

But I get unsatisfactory results. For instance:

 q[π/4, 0, 0]
 % // QuatToEuler // FullSimplify
Quaternion[Cos[π/8], Sin[π/8], 0, 0] 

{π/4, 0, π/2}

which is clearly not the desired output {$\pi$/4,0,0}.

$\endgroup$
1
  • 1
    $\begingroup$ Diebel's equation (290) uses atan2, which could be expecting arguments (y,x). MMA wants (x,y). $\endgroup$
    – LouisB
    Apr 30, 2019 at 21:57

1 Answer 1

3
$\begingroup$

As Louis notes in a comment, ArcTan[] in Mathematica takes the reverse convention of putting x first before y, in contrast to atan2() implementations in most other languages.

Adapting formula 290 from here:

qtoea[qq_] /; QuaternionQ[ToQuaternion[qq]] := 
  With[{q = Apply[List, ToQuaternion[qq]], ns = (#.# &)},
       {ArcTan[ns[q[[{1, 4}]]] - ns[q[[{2, 3}]]], 2 (q[[1]] q[[2]] + q[[3]] q[[4]])], 
        ArcSin[2 (q[[1]] q[[3]] - q[[2]] q[[4]])], 
        ArcTan[ns[q[[{1, 2}]]] - ns[q[[{3, 4}]]], 2 (q[[1]] q[[4]] + q[[2]] q[[3]])]}]

qtoea[q[π/4, 0, 0]]
   {π/4, 0, 0}

We can also take the long route of converting the quaternion to a rotation matrix so that you can use EulerAngles[] on it, as a check:

(* https://mathematica.stackexchange.com/a/169512 *)
Needs["Quaternions`"];

quaternionToRotation[qq_] /; QuaternionQ[ToQuaternion[qq]] :=
    Module[{q = ToQuaternion[qq], aim, r},
           aim = AbsIJK[q]; r = Re[q];
           If[aim == 0, Return[IdentityMatrix[3], Module]];
           First[LinearAlgebra`Private`MatrixPolynomial[
                 {Prepend[2 aim {r, aim}/(r^2 + aim^2), 1]},
                 -LeviCivitaTensor[3, List].(Rest[List @@ q]/aim)]]]

EulerAngles[quaternionToRotation[q[π/4, 0, 0]], {1, 2, 3}] // FullSimplify
   {π/4, 0, 0}
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.