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.

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

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.

Add comment