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.

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 ?

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?

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