Inverse kinematics of 6-links manipulator

As promised in the previous article.

Let’s look at direct kinematics.
H = T_z(L_0) \cdot R_z(q_1) \cdot T_z(L_{11}) \cdot T_x(L_{12}) \cdot R_y(q_2) \cdot T_z(L_2) \cdot R_y(-q_3) \cdot T_z(L_{31}) \cdot R_x(-q_4) \cdot T_x(L_4) \cdot R_y(-q_5) \cdot T_x(L_5) \cdot R_x(-q_6)

Where T_a – translation matrix with respect to a axis, and R_a – rotational matrix with respect to a axis.
H – homogeneous transformation matrix, which contains the position and rotation of the endpoint in global coordinates. (L_{32} is already included in L_4 because there is no difference where J_4 is located on this link)

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

6-links manipulator
6-links manipulator

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 H = \begin{bmatrix}   R & T   \\ 0.. & 1   \end{bmatrix}

0. Since J_6 rotates along the X axis, we can easily move the point of rotation to the center of J_5. In addition, remove the initial translation L_0 :
T_{all} = T_z(-L_0) \cdot T \cdot T_x(-L_5)

2. Since rotation around Z is only in the first joint, we can find q_1:
x = T_{all}[0][3]
y = T_{all}[1][3]
z = T_{all}[2][3]
T_{all} is also a homogeneous transformation matrix, so we get coordinates from the desired positions. And by them, if x! = 0, then q_1 = atan2(y, x), otherwise any.

3. Now find q_2 and q_3 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 J5. We need to transfer the axes in J_2 and then in the plane of rotation J_2 and J_3 only q_2 and q_3 will change the coordinates of the center J_5.

Finding q2 and q3
Finding q2 and q3

For that :
x = \frac{x}{\cos{q_1}} - L_{12}
z = z - L_{11}
Also, two links (right angle) L_{31} and L_4 are replaced by one (with an additional angle \delta q_3):
L_{33} = \sqrt{L_{31}^2 + L_4^2}
\delta q_3 = atan2(L_4, L_{31})
Then for a planar two-links manipulator we get:
q_3 = \arccos{ \frac{x^2+z^2 - L_2^2-L_{33}^2}{2\cdot L_2\cdot L_{33}}} - \delta q_3
q_{33} = q_3 + \delta q_3
q_2 = \frac{\pi}{2} - atan2(z,x) - atan2(L_{33}\cdot \sin{q_{33}}, L_2 + L_{33}\cdot\cos{q_{33}})
q_3 = -q_3

4. Calculate T_{456} in symbolic and numerical form. Joints 4,5 and 6 we moved to the center J_5. Therefore, in symbolic form, direct kinematics looks like this:
T_{456s} = R_x(-q_4)\cdot R_y(-q_5) \cdot R_x(-q_6) =
=  \begin{bmatrix}   \cos{q_5} & \sin{q_5}\cdot \sin{q6} & -\sin{q_5}\cdot \cos{q_6} & 0   \\ \sin{q_4}\cdot \sin{q_5} & ..  & .. & 0 \\ \sin{q_5}\cdot \cos{q_4} & ..  & .. & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

To do the same for the numerical form, we find:
T_{123} = R_z(q_1)\cdot T_z(L_{11})\cdot T_x(L_{12}) \cdot R_y(q_2) \cdot T_z(L_2)\cdot R_y(-q_3) \cdot T_z(L_{31})\cdot T_x(L_4)

Knowing, that T_{all} = T_{123} \cdot T_{456}, find:
T_{456} = T_{123}^{-1} \cdot T_{all}

5. Comparing the symbolic and numerical form from the last step. If T_{456}[0][0] == 1, then we get a degenerate case, q_5 = \arccos{T_{456}[0][0]}, q_4 and q_6 – any . Otherwise (one of the solutions):
q_4 = atan2(T_{456}[1][0],T_{456}[2][0])
q_6 = atan2(T_{456}[0][1],T_{456}[0][2])
q_5 = atan2(T_{456}[0][1], T_{456}[0][0] \cdot \sin{q_6}), if T_{456}[0][1] is not zero and q_5 = atan2(T_{456}[0][2], T_{456}[0][0] \cdot \cos{q_6}) in another case.

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

Inverse kinematics of 2 and 3-links manipulator

With direct kinematics for an ordinary manipulator, everything is pretty simple. And what about the inverse? At a minimum, it’s worth to repeat that it is ambiguous. Let’s start with the two-links manipulator.

2-links manipulator
2-links manipulator
Continue reading “Inverse kinematics of 2 and 3-links manipulator”

Direct kinematics of 2-3-6-n-links manipulator

In the last article I have already described which question answers direct kinematics, and now we will look at examples of how to do this.

How to find the end position for 2-links manipulator? Geometry will tell! Obviously:

Continue reading “Direct kinematics of 2-3-6-n-links manipulator”

Ubuntu 18.04, nvidia drivers and second monitor

I installed Ubuntu 18.04 from scratch (I decided to switch to ROS Melodic ). Understandable that ubuntu did not see nvidia drivers by default. But as it works – do not touch. But I decided to connect a second monitor … I checked hdmi cable … well, the second monitor is not exist anyway.

Continue reading “Ubuntu 18.04, nvidia drivers and second monitor”

Control Theory and System Identification

About a year ago I taught the course “System Identification and Simulation”, but at the very beginning with the professor we decided to check how the masters remember Control Theory. As a result, the half of the semester slid to “Recall All”.

Here I want to save some links that I used to prepare for classes and other useful things.
Continue reading “Control Theory and System Identification”

Pupils, code readability and serious robotics

Once I was tinkering with an anthropomorphic robot. I got to the point where I rewrite almost all code from c++ to python. A colleague (more physicist than a programmer) after this trick very quickly (literally in a month) sketched and tested algorithms, and also wrote a graduate work on these algorithms. Readability is an important thing.
Continue reading “Pupils, code readability and serious robotics”