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:

2 links manipulator
2-links manipulator

x = L_1 \cdot \cos(q_1) + L_2 \cdot \cos(q_1+q_2)
y = L_1  \cdot \sin(q_1) + L_2 \cdot \sin(q_1+q_2)

All right, but the same for 3-links ?

3 links manipulator
3-links manipulator

x =  \cos(q_1) \cdot \big( L_2\cdot \cos(q_2) + L_3 \cdot \cos(q_2+q_3) big)
y = \sin(q_1) \cdot \big( L_2\cdot \cos(q_2) + L_3 \cdot \cos(q_2+q_3) \big)
z = L_1 + L_2\cdot \sin(q_2) + L_3 \cdot \sin(q_2+q_3)

Also kind of understandable. Even noticeable logic.

Will you do for a 6-links ? – Nonono, David Blaine. Thanks, I’ll see.
And this is a real industrial manipulator. Guess what.

6 links manipulator
6-links manipulator

Lazy people like me come up with such a thing as rotation matricies. You don’t even have to think with them where cosines are, where are sinuses. Look,
for 2-links: H = R_z(q_1) \cdot T_y(L_1) \cdot R_z(q_2) \cdot T_x(L_2) ,
for 3-links: H = R_z(q_1) \cdot T_z(L_1) \cdot R_y(q_2) \cdot T_x(L_2)\cdot R_y(q_3) \cdot T_x(L_3)

The main thing is not to get lost in the coordinate axes. Place the robot in the axial position (as drawn 3 and 6 links manipulators) and write down in turn R_{rotateaxis}(q_i) for joint and T_{translateaxis}(L_i) for link.

And now let’s figure it out for a 6-links: H = 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_{32})\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)

And that’s it!

So, stop, stop. And what is it? Where are cosines, where are x’s?

The matrix R_x(q) looks like (for the convenience of multiplication immediately bring to the size of 4×4): R_x(q) = \begin{bmatrix}  1 & 0 & 0 & 0 \\ 0 & \cos(q) & -\sin(q) & 0 \\ 0 & \sin(q) & \cos(q) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}

R_y(q) = \begin{bmatrix}  \cos(q) & 0 & \sin(q) & 0 \\ 0 & 1 & 0 & 0 \\ -\sin(q) & 0 & \cos(q) & 0  \\ 0 & 0 & 0 & 1  \end{bmatrix} R_z(q) = \begin{bmatrix}  \cos(q) & -\sin(q) & 0  & 0 \\ \sin(q) & \cos(q) & 0  & 0 \\ 0 &  0 & 1  & 0 \\ 0 &  0 & 0  & 1 \end{bmatrix}

And matrices T: T_x(L) = \begin{bmatrix}  1 & 0 & 0 & L \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}

T_y(L) = \begin{bmatrix}  1 & 0 & 0 & 0 \\ 0 & 1 & 0 & L \\ 0 & 0 & 1 & 0  \\ 0 & 0 & 0 & 1 \end{bmatrix} T_x(L) = \begin{bmatrix}  1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & L \\ 0 & 0 & 0 & 1 \end{bmatrix}

With matrices sorted out, but what do we get? What is H?

H – homogeneous transformation matrix. As you might guess it consists of H = \begin{bmatrix}  R & T  \\ 0.. & 1  \end{bmatrix} rotation matrix R(3×3) and translation T(3×1) and added to 4×4.

To get the answer x, y, z, you need to take the corresponding elements:
x = H[1][4]
y = H[2][4]
z = H[3][4]

That’s it. It is not difficult to calculate the direct kinematics for an n-link manipulator.

Add comment

avatar