Rotate vector so it aligns with Z, by only rotating around two axes

linear algebraroboticstrigonometry

My math skills are very rusty and I'm struggling with this question. I have a robot that can rotate around the Z axis and around the X axis and I'm trying to get the angles for the joints so that a given vector is pointing down Z (Z negative). All the vectors I have are already pointing down (so they have some negative Z component) and we can assume all vectors I my situation are able to rotated so that they point only down.

The approach that I currenlty have (which doesn't work), is as follows:

To get the angle of the Z-axis rotation, I do the following:

  • Project the Vector $v$ to the $xy$ plane: $V_{xy} = (V_x, V_y, 0)$
  • Make $V_{xy}$ a unit vector
  • Take the dot product of $[0,1,0]$ and $V_{xy}$ and arccos the result to to get the angle

I then do the same for the X axis rotation, but then projecting to $V_{yz}$ by making X component 0 and then taking the dot product with $[0,0,-1]$

Especially the rotation of the X axis seems to be always off.
So my question would be what am I doing wrong here? And are there any more elegant ways to do this?

Best Answer

For the Z axis rotation, you want to rotate through an angle whose tangent is $\frac{x}{y}$, so the rotation matrix is $$ \langle x,y,z\rangle \cdot \begin{pmatrix} \frac{y}{\sqrt{x^2+y^2}}& \frac{x}{\sqrt{x^2+y^2}}& 0 \\ -\frac{x}{\sqrt{x^2+y^2}} & \frac{y}{\sqrt{x^2+y^2}} & 0 \\ 0 & 0 & 1 \end{pmatrix} = \langle 0, \sqrt{x^2+y^2}, z\rangle.$$ For the second rotation, the rotation around the $x$-axis is clockwise through the complement of the angle whose tangent is $\frac{\sqrt{x^2+y^2}}{z}$. For a general point $\langle 0, y, z\rangle$, the associated rotation matrix is $$\begin{pmatrix} 1 & 0 & 0 \\ 0 & -\frac{z}{\sqrt{y^2+z^2}} & -\frac{y}{\sqrt{y^2+z^2}} \\ 0 & \frac{y}{\sqrt{y^2+z^2}} & \frac{z}{\sqrt{y^2+z^2}} \end{pmatrix}.$$ Substituting $\sqrt{x^2+y^2}$ for $y$ in this matrix gives for the $x$-axis rotation matrix $$\begin{pmatrix} 1 & 0 & 0 \\ 0 & -\frac{z}{\sqrt{x^2+y^2+z^2}} & -\frac{\sqrt{x^2+y^2}}{\sqrt{x^2+y^2+z^2}} \\ 0 & \frac{\sqrt{x^2+y^2}}{\sqrt{x^2+y^2+z^2}} & \frac{z}{\sqrt{x^2+y^2+z^2}} \end{pmatrix}.$$ Multiplying $\langle x, y, z\rangle$ by these matrices in turn gives the vector $\langle 0, 0, \frac{-x^2 - y^2 + z^2}{\sqrt{x^2 + y^2 + z^2}}\rangle$.

Related Question