# Forward Kinematics with matrices

From the image, I would like to find out how joint 1, 2 and 3 can be computed by using forward kinematics? (Assuming a theta value was given)

I understand that to get point 3 to origin (P3 -> o) you can do the following matrix multiplications:

[P3->o] = [P2->P1][P1->o]


Where each P matrix is:

P = |Scale*Rotation  0|
|Translation     1|


If the first Theta was 50 degrees and the second theta was 100 degrees, what would the [P2->P1] and [P1->o] matrices be?

Since I'm late to the game, see this question

But you want the numerical values.

The transformation matrices you describe are .. strange. here is a great resource on how to set up and use transformation matrices

Which matches my understanding of:

$$^jT_i=\begin{bmatrix} ^jR(\theta_i)&^jp_i\\ 0&1 \end{bmatrix}$$

So, for example,

$$^1T_2=\begin{bmatrix} \cos(\theta_2')&-\sin(\theta_2')&\ell_1\\ \sin(\theta_2')&\cos(\theta_2')&0\\ 0&0&1 \end{bmatrix}$$

Just sub in the values for the angles and lengths to find the matrices.

The position of the point is:

$$x=\ell_1\cos\theta_1 + \ell_2\cos(\pi/2-\theta_2+\theta_1)+x_1$$ $$y=\ell_1\sin\theta_1 + \ell_2\sin(\pi/2-\theta_2+\theta_1)+y_1$$

Where $$\ell$$ is the length of each arm, and $$\theta$$ is the angles you describe.

You can see this by setting the arm to be fully outstretched along the x-axis, and noting that the position of the end point must be $$(\ell_1+\ell_2,0)$$. Also, $$\theta_2'$$ can be temporarily set to $$\pi-\theta_2$$ to make this arrangement correspond to the "laying along the x-axis" one for ease of derivation.

Or:

$$^{o}p_3=^oT_1\cdot ^1T_2\cdot ^2p_3 +p_1$$

read as "point three with respect to origin equals point of joint three with respect to joint 2, transformed from coordinate frame two to coordinate frame 1, then from coordinate frame 1 to original frame".

And then,

$$^2p_3=(\ell_2,0)$$

When solving the full system, remember to use homogeneous coordinates then, so you have to express the end point as

$$^2p_3=(\ell_2,0,1)$$

The general principle of multiplying matrices together is correct, but the matrices themselves are you use are not correct. They appear to be transposed and for kinematics there is no "Scale" parameter.

Another resource you might find useful is this masterclass on robot arm kinematics. There's also a master class on 3D geometry which might help with your understanding of homogeneous transformation matrices.

While the other answers have already provided the analytical solution and numerical approach thereafter, I would mention that you need to have coordinates for point 1 with respect to the origin.

Theta 1 won't suffice to know the transformation to the origin. Always remember that a joint angle only affects the position/orientation of the child links and thereafter. You need to know the relation between the origin and point 1 to obtain transforms with respect to the former.