**4.1 Closed-form solution of inverse kinematics**

Let us consider a two-link mechanism moving in the 2D plane, see **Figure 5**. Considering the forward kinematic model, while the angles of joints *ϑ*<sup>1</sup> and *ϑ*<sup>2</sup> are given, the aim is to find the position of end-effector **x***<sup>E</sup>* ¼ ½ � *x y <sup>T</sup>* ∈ *<sup>m</sup>*. The forward kinematic model can be easily determined by the following equations

$$\infty = l\_1 \cos \theta\_1 + l\_2 \cos \left(\theta\_1 + \theta\_2\right) \tag{21}$$

$$y = l\_1 \sin \theta\_1 + l\_2 \sin \left(\theta\_1 + \theta\_2\right) \tag{22}$$

Now, the inverse kinematic problem is to find angles *ϑ*<sup>1</sup> and *ϑ*2, while the endeffector position *x* and y are given by vector **x***<sup>E</sup>* ¼ ½ � *x y <sup>T</sup>* ∈ *<sup>m</sup>*.

$$
\sigma = \mathbf{x}^2 + \mathbf{y}^2 \tag{23}
$$

$$a = \operatorname{atan2}(\mathbf{y}, \mathbf{x})\tag{24}$$

$$\theta\_2 = \pm \arccos\left(\frac{\mathbf{x}^2 + \mathbf{y}^2 - L\_1^2 - L\_2^2}{2L\_1L\_2}\right) \tag{25}$$

$$\beta = \arccos\left(\frac{L\_1^2 - L\_2^2 + c^2}{2L\_1c}\right) \tag{26}$$

$$
\theta\_1 = a - \beta \tag{27}
$$

**Figure 5.** *Inverse kinematics solution for two-link mechanism.*

As can be seen in **Figure 5**, the presented configuration of the mechanism has two solutions in inverse kinematics. These two solutions are based on signum in Eqs. (25) and (27).
