Forward kinematic computing the transformation matrix

Question

I am the moment trying to compute the transformation matrix for robot arm, that is made of 2 joints (serial robot arm), with which I am having some issues. L = 3, L1 = L2 = 2, and q = ($q_1$,$q_2$,$q_3$) = $(0 , \frac{-\Pi}{6},\frac{\Pi}{6})$

Based on this information I have to compute the forward kinematic, and calculate the position of each joint.

Problem here is though, how do I compute the angle around x,y,z.. for the transformation matrix. Using sin,cos,tan is of course possible, but what do their angle corresponds? which axis do they correspond to?

I tried using @SteveO answer to compute the $P_0^{tool}$ using the method he provided in his answer, but I somehow mess up something, as the value doesn't resemble the answer given in the example..

Answer 1

Start with coordinate systems. I've drawn one example.

In my analysis, if all $q_i = 0$ then the manipulator would point straight up. You can choose other coordinate frames to get the same result. Build your rotation matrices from the coordinate systems you set up.

The rotation matrix from coordinate system $0$ to coordinate system $1$ is $$_0^1R = \begin{bmatrix} \cos q_1&-\sin q_1&0\\ \sin q_1&\cos q_1&0\\ 0&0&1 \end{bmatrix} $$

The mapping from coordinate system $1$ to coordinate system $2$ is

$$_1^2R = \begin{bmatrix} -\sin q_2&-\cos q_2&0\\ \cos q_2&-\sin q_2&0\\ 0&0&1 \end{bmatrix} $$

And the mapping from system $2$ to $3$ is

$$_2^3R = \begin{bmatrix} \cos q_3&-\sin q_3&0\\ \sin q_3&\cos q_3&0\\ 0&0&1 \end{bmatrix} $$

Notice that all of the joint angles are taken as positive rotations around the $z$ axis. I'm not sure from your problem statement or drawing if that is your intention.

So to compute the position of point $p$ with respect to the global $0$ coordinate system, apply the vector loop

$$ \vec p = L \vec y_1 + L_1 \vec x_2 + L_3 \vec x_3$$

$\vec y_1$ is the second column from $_0^1R$. $\vec x_2$ is the first column from $_0^2R$, and $\vec x_3$ is the first column from $_0^3R$.

You compute $_0^2R$ by multiplying $_0^1R _1^2R$, and $_0^3R$ is $_0^2R _2^3R$.

Answer 2

In your forward kinematics transformation matrix (4x4, incl. also translation, of just 3x3) the orientation of the end-effector is expressed relative to the base (or world) coordinate system.

SteveO described very well how to obtain the 3x3 rotation matrix, similarly you can obtain also the 4x4 transformation matrix

If you have the matrix you have to choose how do you want to express the coordinates. Commonly in robotics euler angles are used, with successive rotations about the axes of a coordinate system. After choosing the Euler angles, you also have to choose which is the succession of the rotation. In robotics in many cases the rotations are successive (intrinsic) rotations.

After choosing hwo do you want to express the orientation you can build up the orientation matrix from your rotations(e.g. X-Y-Z, then you multiply $R_{TCP} = R(\phi_x) * R(\phi_y) * R(\phi_z) $).

The orientation of your TCP expressed in function of your joints is $R_{joints}=R_{joints}(q1, q2, q3)$ or $^3_0R$ in the notation used by SteveO. Both of the matrices express the same orientation but in a different way, so $R_{TCP}=R_{joints}$ the joint angles are known in forward kinematics, so you can calculate the orientation angles. Please note that if you are using Matlab the dcm2angle() function does exactly this.

Answer 3

Try the approach used in here

The rotation matrix, using generalized coordinates, it's straightforward! Do the math and check it by trigonometry identities.

Good luck!