As promised in the previous article.

Let’s look at direct kinematics.

Where – translation matrix with respect to a axis, and – rotational matrix with respect to a axis.

H – homogeneous transformation matrix, which contains the position and rotation of the endpoint in global coordinates. ( is already included in because there is no difference where is located on this link)

Minus signs near angles show that in reality, the joint moves in the another direction.

For inverse kinematics, there were not invent such a simple thing as a simple multiplication of the rotation and translate matrices. Therefore, we take steps.

At the entrance we have the matrix T – the position of the endpoint, its format is the same (4×4) as

0. Since rotates along the X axis, we can easily move the point of rotation to the center of . In addition, remove the initial translation :

2. Since rotation around Z is only in the first joint, we can find :

is also a homogeneous transformation matrix, so we get coordinates from the desired positions. And by them, if x! = 0, then , otherwise any.

3. Now find and according to standard formulas for a planar two-link. Only need to do a little preparation. Now the axes are located at the point P – and we know the coordinates of center of . We need to transfer the axes in and then in the plane of rotation and only and will change the coordinates of the center .

For that :

Also, two links (right angle) and are replaced by one (with an additional angle ):

Then for a planar two-links manipulator we get:

4. Calculate in symbolic and numerical form. Joints 4,5 and 6 we moved to the center . Therefore, in symbolic form, direct kinematics looks like this:

To do the same for the numerical form, we find:

Knowing, that , find:

5. Comparing the symbolic and numerical form from the last step. If , then we get a degenerate case, , and – any . Otherwise (one of the solutions):

, if is not zero and in another case.

Thus, we found all the positions of the joints according to the position of the endpoint.