The Relative Euler angles sensor calculates the Euler angles from the Relative Rotation Matrix that defines the rotation from frame_a to frame_b. The algorithm  is described in the Algorithm section.

1. Only one set of results is given by the sensor. Given the set of outputs ${\mathrm{\θ}}_1\, {\mathrm{\θ}}_2\,$and ${\mathrm{\θ}}_3$, the alternate solutions can be obtained as:

     - For the non-classical sequences (e.g. [1,2,3] or [2,3,1]):           ${\mathrm{\θ}}_1\pm \mathrm{\π}\, -{\mathrm{\θ}}_2\pm \mathrm{\π}\,$${\mathrm{\θ}}_3\pm \mathrm{\π}$.

     - For classical or repeating sequences (e.g. [3,1,3] or [1,2,1]):    ${\mathrm{\θ}}_1\pm \mathrm{\π}\, -{\mathrm{\θ}}_2\,$          ${\mathrm{\θ}}_3\pm \mathrm{\π}$.

2. The output angles may become discontinuous if the rotation of the body goes through the singularity of the selected Euler Angle rotation sequence. For classical (repeating) sequences the singularity is at $\sin \left({\mathrm{\θ}}_2\right)\=0$. For non-classical (non-repeating) sequences the singularity is at $\cos \left({\mathrm{\θ}}_2\right)\=0$.

3. The output angles may also become discontinuous if they are close to the boundary of $\pm \mathrm{\π}\.$

(The algorithm below is based on: G. Meyer and H. Q. Lee, "A Method for Expanding a Direction Cosine Matrix Into an Euler Sequence of Rotations," NASA TM X-1384, 1967)

Let

      I = RotSeq[1];

      J = RotSeq[2];

      K = RotSeq[3];

and rotation matrix A be given by:

    A = transpose(frame_a.R.T);

where frame_a.R.T is the absolute orientation matrix of ${\mathrm{frame}}_a$.

        if I <> K then

            L = I - mod(J,3);

            C = if L == 2 then -1 else L;

            TH[1] = -atan2(A[K,J]*C,A[K,K]);

            X[1] = -sin(TH[1]);

            Y[1] = cos(TH[1]);

            X[3] = A[I,K]*X[1] - A[I,J]*C*Y[1];

            Y[3] = A[J,J]*Y[1] - A[J,K]*C*X[1];

            X[2] = -A[K,I]*C;

            Y[2] = A[I,I]*Y[3] + A[J,I]*C*X[3];

        else

            N = 6 - (K + J);

            L = N - mod(I,3);

            C = if L == 2 then -1 else L;

            TH[1] = -atan2(A[K,J],A[K,N]*C);

            X[1] = -sin(TH[1]);

            Y[1] = cos(TH[1]);

            X[3] = -A[N,N]*X[1] + A[N,J]*C*Y[1];

            Y[3] = A[J,J]*Y[1] - A[J,N]*C*X[1];

            X[2] = A[J,I]*X[3] - A[N,I]*C*Y[3];

            Y[2] = A[K,K];

        end if;

        TH[2] = -atan2(X[2],Y[2]);

        TH[3] = -atan2(X[3],Y[3]);

        end EQs;

Multibody Sensors

Multibody Overview