[Math] Angle definition confusion in Rodrigues rotation matrix

matricespythonrotations

The Rodrigues rotation formula gives us the following way to rotate a vector $\vec{v}$ by some angle $\theta$ about an arbitrary axis $\vec{k}$:

$\vec{v}_\text{rot} = \vec{v}\cos(\theta) + (\vec{k} \times \vec{v})\sin(\theta) + \vec{k}(\vec{k}\cdot\vec{v})(1-\cos\theta)$

Let's call this the "vector notation" There is also a way to obtain the corresponding rotation matrix $\textbf{R}$, as such:

$\textbf{R} = \textbf{I} + (\sin\theta)\textbf{K} + (1-\cos\theta)\textbf{K}^2$

$\>\>\>\>\>\downarrow$

$\vec{v}_\text{rot} = \textbf{R}\vec{v}$

where $\textbf{K}$ is the cross-product matrix of the rotation axis:

$\textbf{K} = \begin{pmatrix} 0 & -k_z & k_y \\
k_z & 0 & -k_x \\
-k_y & k_x & 0\end{pmatrix}$

Let's call this the "matrix notation"

My issue is that I cannot seem to get the "vector" and "matrix" notations to agree…

Let's say we have the vector

$\vec{v} = \begin{pmatrix} \sqrt{1/3} \\ \sqrt{1/3} \\ \sqrt{1/3} \end{pmatrix}$

That we would like to rotate to lie on the x-axis:

$\vec{v}_\text{rot} = \begin{pmatrix} |\vec{v}| \\ 0 \\ 0 \end{pmatrix} = \begin{pmatrix} 1 \\ 0 \\ 0 \end{pmatrix}$

then our axis and angle of rotation are:

$\vec{k} = \dfrac{\vec{v}\times\vec{v}_\text{rot}}{|\vec{v}\times\vec{v}_\text{rot}|} = \begin{pmatrix} 0 \\ \sqrt{2}/2 \\ \sqrt{2}/2 \end{pmatrix}\>\>\>\>, \>\>\theta = \cos^{-1}\left( \dfrac{\vec{v}\cdot\vec{v}_\text{rot}}{|\vec{v}||\vec{v}_\text{rot}|}\right) = 0.95532 \text{rad}$

The "vector notation" statement above does indeed give the expected answer (I've implemented it in Python with NumPy):

import numpy as np
v = np.array([1/3, 1/3, 1/3])**(1/2)
vr_desired = np.array([1, 0, 0])
theta = np.arccos(np.dot(v, vr_desired) / np.linalg.norm(v)*np.linalg.norm(vr_desired))
k = np.cross(v, vr_desired) / np.linalg.norm(np.cross(v, vr_desired))

vr_according_to_vector_form = v*np.cos(theta) + (np.cross(k,v)*np.sin(theta)) + k*(np.dot(k,v))*(1.0-np.cos(theta))
print("v_rot = {}".format(vr_according_to_vector_form))

where the result is 

v_rot = [ 1.  0.  0.]

as expected.

Now, trying the "matrix notation":

K = np.array([[0, -k[2], k[1]],[k[2], 0, -k[0]],[-k[1], k[0], 0]])
I = np.eye(3)
R = I + np.sin(theta)*K + (1-np.cos(theta))*(K**2)

vr_according_to_matrix_form = np.matmul(R,v)
print("v_rot = {}".format(vr_according_to_matrix_form))

I get the wrong answer:

v_rot = [ 1.48803387  0.3660254   0.3660254 ]

I can only imagine that the angle $\theta$ is being defined differently in the vector and matrix forms of the Rodrigues formula… I am confident in this diagnosis because if I rotate the axis of rotation itself, I do get the same axis back, which seems to indeed indicate that it is simply an issue with $\theta$ that I'm not understanding, and that there isn't any issue in my declaration of $\textbf{R}$…

print("Rk - k = {}".format(np.matmul(R,k) - k))

gives

Rk - k = [ 0.  0.  0.]

FYI, I've been using the this wikipedia resource: https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

There, it says that $\vec{v}$ in the vector formula is rotated "by an angle θ according to the right hand rule,", and in the matrix formula, it is rotated "through an angle θ anticlockwise about the axis $\vec{k}$". Those sound like identical statements to me, though.

Best Answer

The problem is that K**2is squaring the matrix componentwise, not using matrix multiplication.

Related Question