Get two rotation angles from two axis rotations

55 Views Asked by At

How can I get two rotation angles first around axis C (blue), then around axis A (red), to obtain Vector V` (black line). The default vector position is V (purple line) ?

The vector V` direction is known and angles for C and A coordinate systems are not known.

Figure

Better explanation

1

There are 1 best solutions below

4
On

If $R_C(\theta_1)$ and $R_A(\theta_2)$ are rotation matrices, about axis $C$ and $A$ respectively, then $V' = R_A R_C V$. Let $C$ and $A$ be unit vectors along the axes of rotation, respectively, then the form of $R_C$ and $R_A$ is known and given by (this is known as the Rodrigues Rotation Formula)

$ R_C(\theta_1) = C C^T + (I - C C^T) \cos \theta_1 + S_C \sin \theta_1 \hspace{36pt} (1) $

$ R_A(\theta_2) = A A^T + (I - A A^T) \cos \theta_2 + S_A \sin \theta_2 \hspace{36pt} (2) $

(where the skew-symmetric matrix $S_C$ is such that $S_C v = C \times v $, and similarly for $S_A$ )

Let $Q = R_C V $, then

$Q = R_A^T V' = R_C V \hspace{36pt} (3)$

Here we note that when you rotate a vector about an axis, the rotated vector maintains its angle with the axis for any rotation angle. Hence we want to choose the rotation angle about axis $C$ (which is the first axis) such that it results in a vector that has the same angle with axis $A$ as vector $V'$. This can expressed using the dot product as:

$ A^T R_C V = A^T V' \hspace{36pt} (4)$

Plugging in this equation the expression for $R_C$ gives a simple trigonometric equation in $\theta_1$

By a similar argument, we deduce that

$ C^T R_A^T V' = C^T V \hspace{36pt} (5) $

which results in a simple trigonometric equation in $\theta_2$ that can be readily solved. At the end you will have two values for $\theta_1$ and two values for $\theta_2$, however there are only two possible combinations of $\theta_1$ and $\theta_2$ that will send $V$ to $V'$, so for the first value of $\theta_1$ construct $W = R_A(\theta_2) R_C(\theta_1) V $ and choose the value of $\theta_2 $ from two possible values that will result in $W = V'$, then the other value of $\theta_2$ corresponds to the other value of $\theta_1$.

Edit: The following explains how to solve equation $(4)$ for $\theta_1$.

Plug in the expression for $R_C$ given by $(1)$ into $(4)$, you get

$A^T (C C^T + (I - C C^T) \cos \theta_1 + S_C \sin \theta_1) V = A^T V' \hspace{36pt} (6)$

Equation $(6)$ has the form:

$ c_1 \cos \theta_1 + c_2 \sin \theta_1 = c_3 \hspace{36pt} (7)$

where $c_1 = A^T (I - C C^T) V, c_2 = A^T S_C V , c_3 = A^T V' - A^T C C^T V $

To solve equation $(7)$, divide through by $\sqrt{c_1^2 + c_2^2} $, resulting in,

$ \dfrac{c_1}{\sqrt{c_1^2+c_2^2}} \cos \theta_1 + \dfrac{c_2}{\sqrt{c_1^2 + c_2^2}} \sin \theta_1 = \dfrac{c_3 }{\sqrt{c_1^2 + c_2^2}}\hspace{36pt} (8)$

Note that $ \dfrac{c_1}{\sqrt{c_1^2+c_2^2}} $ and $\dfrac{c_2}{\sqrt{c_1^2 + c_2^2}}$ are a $\cos$ and $\sin$ pair, thus find the angle $\phi$ such that

$\cos \phi =\dfrac{c_1}{\sqrt{c_1^2+c_2^2}} $ and $\sin \phi = \dfrac{c_2}{\sqrt{c_1^2 + c_2^2}}$

Angle $\phi $ can be found (in a programming environment) with the function $\text{ATAN2}(c_1, c_2)$ which is a standard math library function.

Now we can re-write equation $(8)$ as

$ \cos \phi \cos \theta_1 + \sin \phi \sin \theta_1 = \dfrac{c_3}{\sqrt{c_1^2 + c_2^2}} \hspace{36pt} (9) $

The left hand side is just $\cos(\theta_1 - \phi)$, hence,

$ \cos( \theta_1 - \phi ) = \dfrac{c_3}{\sqrt{c_1^2 + c_2^2}} \hspace{36pt} (10) $

whose two solutions are given by,

$\theta_1 = \phi \pm \cos^{-1} \dfrac{c_3}{\sqrt{c_1^2 + c_2^2}} \hspace{36pt} (11) $

The exact same procedure is used to solve equation $(5)$ for $\theta_2$. Note that $R_A^T (\theta_2) = R_A(- \theta_2) $.

To know which of the two values of $\theta_2$ to use with the first value of $\theta_1 $, plug in the values in equation $(3)$, and see if its is satisfied, if not then you use the other value of $\theta_2$. Thus you now have one pair of $(\theta_1, \theta_2)$ , then the other pair is obtained by the other solution of $\theta_1 $ and $\theta_2$.