Converting quaternion to Euler Angles

Question

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}.

Answer 1

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}