Expressing a global point in different frames

Question

I am trying to express a vector defined in global coordinates in different frames using homogeneous transformation matrices.

There are in total 3 frames. Frame 1 lies in the origin and is fixed to the world. Frame 2 is fixed to a floating body, with 6 degrees of freedom. Frame 3 is attached to the same floating body and has only a pure translation with respect to Frame 2.

The vector $R_0$ is the vector that I'm trying to express in Frame 2 to obtain $R_2$. For this I have constructed two homogeneous transformation matrices:

$H_{01} = \begin{bmatrix}R_{01} & T_{01}\\\mathbf{0} & 1\end{bmatrix}$ and $H_{01} = \begin{bmatrix}I & T_{12}\\\mathbf{0} & 1\end{bmatrix}$.

With these transformation matrices i should be able to transform a vector expressed in frame 0 to frame 2. I did this doing the following:

$R_2 = (H_{01}H_{12})^{-1}R_0$

Then I plotted the frames and vector to come to the conclusion that its not quite right whenever I put in a rotation matrix $R_{01}$. To illustrate my situation some images:

If I put $R_{01}$ is equal to anyhting but the identity matrix my vector does not end up in the red dot (the fixed point expressed in world coordinates).

Edit:

From what i can see, the vector that I am obtaining looks correct but is now plotted in global space and not in the frame from which is should be plotted. So basically my question becomes: how can I rotate $R_2$ back in order to plot the vector such that it ends up in the reference point?

Answer 1

I would suggest you to clarify your notations and not use capital letter for vectors but only for matrices, calling a vector $\mathbf{R}_*$ and the rotation matrix $\mathbf{R}_{*@}$ can lead to confusions, even for you. More over your index notation are not consistent.

That being said it seems that your math are right (provided that the index you wrote are the one I guessed). So your trouble is most likely in plotting, if you express you original vector in the reference frame you get the red dot, if you want to check the resulting vector "$\mathbf{R}_2$" you need to pre-multiply it by the two transformations (in order to be in your third frame).

Note that you can rewrite the inverse of a transformation matrix in a more efficient way ${}_0\mathbf{T}^1 = \begin{bmatrix} {}_0\mathbf{R}^1& {}_0\mathbf{p}^1\\ 0 & 1 \end{bmatrix}$ and ${{}_0\mathbf{T}^1}^{-1} = \begin{bmatrix} {{}_0\mathbf{R}^1}^{T} & -{{}_0\mathbf{R}^1}^{T} {}_0\mathbf{p}^1\\ 0 & 1 \end{bmatrix}$