Find orientation through Transformation matrix

I have a robot with 3 rotational joints that I am trying to simulate in a program I am creating. So I have 4 frames, one base frame, and each joint has a frame. I have 3 transformation functions to go from frame 1 or 2 or 3 to frame 0.

By using the transformation matrix, I want to know how much each frame has been rotated (by the X,Y and Z axis) compared with the base frame. Any suggestions?

The reason I want this is because I have made some simple 3D shapes that represent each joint. By using the DH parameters I made my transformation matrices. When ever I change my θ (it does not mater how the θ changes, it just does), I want the whole structure to update. I take the translation from the last column. Now I want to get the rotations.

• Can you please add a brief example that shows what you're trying to do? I'm having a bit of a hard time understanding from the text what exactly you're trying to do. Are you trying to determine $\theta_1$, $\theta_2$, and $\theta_3$ given a 4x4 transformation matrix from frame 3 to frame 0? This is how the question reads to me, but if that's the case then how are you getting the transformation matrix to begin with? It seems that you're building the final transform matrix joint-by-joint, but then you want to use that matrix to find the joint angles - didn't you just use them to make the matrix? – Chuck Feb 8 '16 at 15:36
• Do you simply want to convert the rotation matrix into Euler angles or some other format? – Ben Feb 8 '16 at 16:11

Based on your question, i understood that you have transformation matrices and you need to find Euler angles.

$$\begin{array}{ccc|c} r11&r12&r13&X\\ r21&r22&r23&Y\\ r31&r32&r33&Z\\ \cdots&\cdots&\cdots&1 \end{array}$$ the first three rows and columns will give us a Rotation matrix. You will get X,Y,Z rotation angles by comparing the above mentioned rotation matrix with $$\begin{matrix} cos(\psi)cos(\theta)&-sin(\psi)cos(\phi) + cos(\psi)sin(\theta)sin(\phi)&sin(\psi)sin(\phi)+cos(\psi)sin(\theta)cos(\phi)\\ sin(\psi)cos(\theta)&cos(\psi)cos(\phi)+sin(\psi)sin(\theta)sin(\phi)&-cos(\psi)sin(\phi)+sin(\psi)sin(\theta)cos(\phi)\\ -sin(\theta)&cos(\theta)sin(\phi)&cos(\theta)cos(\phi) \end{matrix}$$

I suggest to use quaternions, they will be of form e0 + e Here e0 is the cos(angle/2) and e is the unit vector of axis of rotation multiplied by sin(angle/2).

• Edited it sorry, forgot to put the cos(angle/2) there. – the_parzival Feb 8 '16 at 20:56
• Thanks a lot man :). I found these formulas that I think are based on your idea. nghiaho.com/?page_id=846 . – Dimitris Pantelis Feb 11 '16 at 18:04

An intuitive way to think about this is to recognize that the upper left 3x3 rotational submatrices of the 4x4 transformation matrices are really just the direction cosines of each of the unit vectors of the moving frames, with respect to the base frame. So, for example, consider the $T_{02}$ transformation matrix. This would be found by multiplying $T_{01} T_{12}$, which maps the second moving frame back to the global, or $0$, frame. The first column of the upper left 3x3 of $T_{02}$ is just the cosine mapping of $\hat x_2$ (the unit vector of the $2$ frame's $x$ axis) into its components in the $0$ frame. Similarly, the second column contains the direction cosines of $\hat y_2$ onto the base frame, and the third does the same for $\hat z_2$.

Mapping With Coordinates Systems

Suppose you have an robotic arm with 3 links:

Using generalized coordinates:

$\hspace{2.5em}$ $\vec{q}$ = $[q_{1}\hspace{1em}q_{2}]^{T}$ $\hspace{1.5em}$ [Generalized coordinate]

We can evaluate the system as following:

The kinematic equation that maps the origin to point A is:

$\hspace{5.em}$ $_{o}\vec{r}_{OA}$ = $_{o}\vec{r}_{O1}$ + $_{o}\vec{r}_{12}$ + $_{o}\vec{r}_{2A}$ $\hspace{1.5em}$ [kinematic equation]

We have two rotations over joint 1 and 2. Substituting in the equation above:

$\hspace{5.em}$ $_{o}\vec{r}_{OA}$ = $_{0}\vec{r}_{O1}$ + $R{(q_{1})_{01}}$ $_{1}\vec{r}_{12}$ + $R{(q_{1}+q_{2})_{12}}$ $_{2}\vec{r}_{2A}$

Where $R(\bullet)$ is the rotation matrix.

If you want to map between point 1 to A:

$\hspace{5.em}$ $_{1}\vec{r}_{1A}$ = $_{1}\vec{r}_{12}$ + $_{1}\vec{r}_{2A}$ $\hspace{1.5em}$ [New kinematic equation]

$\hspace{5.em}$ $_{1}\vec{r}_{1A}$ = $R{(q_{1})_{01}}$ $_{1}\vec{r}_{12}$ + $R{(q_{1}+q_{2})_{12}}$ $_{2}\vec{r}_{2A}$

It's pretty straightforward! For more on direct kinematics here.

The term $R{(q_{1})_{01}}$ $_{1}\vec{r}_{12}$ should be something like:

$\hspace{5.em}$ $R{(q_{1})_{01}}$ $_{1}\vec{r}_{12}$ = $\begin{bmatrix} cos(q_{1}) & -sin(q_{1}) & 0 \\ sin(q_{1}) & cos(q_{1}) & 0 \\ 0 & 0 & 1\end{bmatrix}$ $\begin{bmatrix} l_{1} \\ 0 \\ 0 \end{bmatrix}$

With a little bit of algebra, you can achieve the answer:

$\hspace{5.em}$ $_{0}\vec{r}_{OA}$ = $\begin{bmatrix} l_{0} + l_{1}cos(q_{1}) + l_{2}cos(q_{1}+q_{2}) \\ 0 + l_{1}sin(q_{1}) + l_{2}sin(q_{1}+q_{2}) \\ 0 \end{bmatrix}$

That equation make possible to map all your system at any time relative to the origin.