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

From the direct we know:
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)

We squared both and add. Find q_2:
q_2 = \acos( \frac{x^2 + y^2 - L_1^2- L_2^2}{2\cdot L_1 \cdot L_2})
q_1 find as: q_1 = q_a \pm q_b . Bottom sign if shoulder down in 4th quarter or q_2>0

Now is easy to find q_a = \arctan(\frac{y}{x}) , and q_b = - \arctan (\frac{ L_2 \cdot \cos(q_2 )}{ L_1 + L_2 \cdot \sin(q_2)})

Get q_1 = \mp \arctan (\frac{ L_2 \cdot \cos(q_2 )}{ L_1 + L_2 \cdot \sin(q_2)}) + \arctan (\frac{y}{x})

Not so beautiful. And what for 3-links ?

3 links manipulator
3-links manipulator

Let’s start with a familiar manipulator. Obvivious, q_1 = \arctan2 (\frac{y}{x}). And the rest is above discussed 2-links manipulator.
q_3 = \arccos(\frac{ x_a^2 + z_a^2 - L_2^2 - L_3^2}{2\cdot L_2 \cdot L_3})
q_2 = \mp \arctan (\frac{ L_3 \cdot \cos(q_3 )}{ L_2 + L_3 \cdot \sin(q_3)}) + \arctan(\frac{z_a}{x_a}), where
x_a = \sqrt{x^2 + y^2}
z_a = z-L_1
It means we moved the origin to q_2 and rotated the axes to q_1 along z.

Great, but what if we have a planar 3-links manipulator?

3-links planar manipulator
3-links planar manipulator

Then we can not cope without additional knowledge, namely, without knowledge of orientation the tip q_z. The rotation will only be around z, so remembering what is H you can understand, that q_z = \arccos(H[0][0]).
And then by default:
x_2 = x - L_3\cdot \cos(q_z)
y_2 = y - L_3\cdot \sin(q_z)
q_2 = \acos( \frac{x_2^2 + y_2^2 - L_1^2- L_2^2}{2\cdot L_1 \cdot L_2})
q_1 = \mp \arctan (\frac{ L_2 \cdot \cos(q_2 )}{ L_1 + L_2 \cdot \sin(q_2)}) + \arctan(\frac{y_2}{x_2})

As you understand, it was all told in order to be the basis for calculating the kinematics of 6-links manipulator day in the next article.

Add comment

avatar