Find orientation through Transformation matrix

Question

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.

Answer 1

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 e₀ + e Here e₀ is the cos(angle/2) and e is the unit vector of axis of rotation multiplied by sin(angle/2).

Answer 2

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$.

Answer 3

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.