**5. Differential kinematics**

#### **5.1 Analytical Jacobian**

In order to describe the relation between joint angles and end-effector configuration, often the relation between the joint and end-effector velocities is used. Let us consider a set of coordinates **<sup>x</sup>**<sup>∈</sup> *<sup>m</sup>*, their velocity is **<sup>x</sup>**\_ <sup>¼</sup> *<sup>d</sup>***x***=dt*<sup>∈</sup> *<sup>m</sup>*. Then, we can apply Eq. (19). Then, one obtains

$$\dot{\mathbf{x}} = \frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}} \frac{d\mathbf{q}}{dt} = \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}} \tag{28}$$

$$\mathbf{J}(\mathbf{q}) = \begin{bmatrix} \frac{\partial \mathbf{x}\_1}{\partial q\_1} & \cdots & \frac{\partial \mathbf{x}\_1}{\partial q\_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial \mathbf{x}\_m}{\partial q\_1} & \cdots & \frac{\partial \mathbf{x}\_m}{\partial q\_n} \end{bmatrix} \tag{29}$$

where **J q**ð Þ<sup>∈</sup> *<sup>m</sup>*�*<sup>n</sup>* is the analytical Jacobian matrix, which is very often used in kinematics and dynamics of robotic systems. Jacobian reflects differences between joint and configurations space of the investigated mechanism. In robotics, Jacobian is often used for several purposes such as for the definition of the relation between joint and configuration space, definition of the relation between forces/torques between spaces, the study of kinematic singularities, the definition of numerical solution for inverse kinematic problem, and the study of manipulability properties. We can look at Jacobian from a different perspective. Particular Jacobian columns represent the influence of *i*-th joint on the end-effector velocity.

The following example will demonstrate the derivation of analytical Jacobian for a three-link mechanism (**Figure 6**).

**Figure 6.** *Three-link mechanism.*

In order to utilize Eq. (29), we need to define position of point **E** ¼ *xEyE* � �*<sup>T</sup>* ∈ <sup>2</sup> .

$$\mathcal{X}\_{E} = L\_1 \cos q\_1 + L\_2 \cos \left(q\_1 + q\_2\right) + L\_3 \cos \left(q\_1 + q\_2 + q\_3\right) \tag{30}$$

$$y\_E = L\_1 \sin q\_1 + L\_2 \sin \left(q\_1 + q\_2\right) + L\_3 \sin \left(q\_1 + q\_2 + q\_3\right) \tag{31}$$

Assuming Eqs. (30) and (31), Jacobian according to Eq. (29) will be matrix **J q**ð Þ<sup>∈</sup> <sup>2</sup>�<sup>3</sup>

$$\mathbf{J(q)} = \begin{bmatrix} \frac{\partial \mathbf{x}\_E}{\partial q\_1} & \frac{\partial \mathbf{x}\_E}{\partial q\_2} & \frac{\partial \mathbf{x}\_E}{\partial q\_3} \\ \frac{\partial \mathbf{y}\_E}{\partial q\_1} & \frac{\partial \mathbf{y}\_E}{\partial q\_2} & \frac{\partial \mathbf{y}\_E}{\partial q\_3} \end{bmatrix} \tag{32}$$

where the elements of the Jacobian matrix are

$$\begin{aligned} \frac{\partial \mathcal{E}\_E}{\partial q\_1} &= -L\_1 \sin \left(q\_1 \right) - L\_2 \sin \left(q\_1 + q\_2 \right) - L\_3 \sin \left(q\_1 + q\_2 + q\_3 \right) \\ \frac{\partial \mathcal{E}\_E}{\partial q\_2} &= -L\_2 \sin \left(q\_1 + q\_2 \right) - L\_3 \sin \left(q\_1 + q\_2 + q\_3 \right) \\ \frac{\partial \mathcal{E}\_E}{\partial q\_3} &= -L\_3 \sin \left(q\_1 + q\_2 + q\_3 \right) \\ \frac{\partial \mathcal{V}\_E}{\partial q\_1} &= L\_1 \cos \left(q\_1 \right) + L\_2 \cos \left(q\_1 + q\_2 \right) + L\_3 \cos \left(q\_1 + q\_2 + q\_3 \right) \\ \frac{\partial \mathcal{V}\_E}{\partial q\_2} &= L\_2 \cos \left(q\_1 + q\_2 \right) + L\_3 \cos \left(q\_1 + q\_2 + q\_3 \right) \\ \frac{\partial \mathcal{V}\_E}{\partial q\_3} &= L\_3 \cos \left(q\_1 + q\_2 + q\_3 \right) \end{aligned}$$

#### **5.2 Geometric Jacobian**

Besides analytically expressing the Jacobian, we can express it by a geometric approach. To establish function **f q**ð Þ in closed-form, a symbolic formalism is necessary, which could be difficult from the view of implementation. For this reason, a different way of Jacobian expression, the so-called geometric Jacobian, was developed. The geometric Jacobian can be obtained by consideration of rotational velocity vector **ω**. Let us consider link according to **Figure 6**. The Jacobian can be expressed as

$$\mathbf{J} = \begin{bmatrix} \mathbf{J}\_v \\ \mathbf{J}\_o \end{bmatrix} = \begin{bmatrix} \mathbf{J}\_{v1} & \dots & \mathbf{J}\_{vn} \\ \mathbf{J}\_{o1} & \dots & \mathbf{J}\_{am} \end{bmatrix} \tag{33}$$

The first term expresses the effect of *q*\_ <sup>1</sup> on linear velocity **v** and the second term expresses the effect on the rotational velocity **ω**. Thus,

$$\begin{aligned} \mathbf{v} &= \mathbf{J}\_{v1}\dot{q}\_1 + \dots + \mathbf{J}\_{vn}\dot{q}\_n\\ \mathbf{oo} &= \mathbf{J}\_{o1}\dot{q}\_1 + \dots + \mathbf{J}\_{om}\dot{q}\_n \end{aligned} \tag{34}$$

That is, the analytical Jacobian differs from the geometrical Jacobian for the rotational part. Considering the revolute joint, the *i*-th column of Jacobian can be computed as

$$
\begin{bmatrix}
\mathbf{J}\_{vi} \\
\mathbf{J}\_{ai}
\end{bmatrix} = \begin{bmatrix}
\mathbf{^0z\_{i-1}} \times \begin{pmatrix}
\mathbf{^0p\_n} - \mathbf{^0p\_{i-1}} \\
\mathbf{^0z\_{i-1}}
\end{pmatrix} \\
\end{bmatrix} \tag{35}
$$

For prismatic joint, the *i*-th column of Jacobian can be computed as

$$
\begin{bmatrix}
\mathbf{J}\_{vi} \\
\mathbf{J}\_{ai}
\end{bmatrix} = \begin{bmatrix}
^0 \mathbf{z}\_{i-1} \\
\mathbf{0}
\end{bmatrix} \tag{36}
$$

where <sup>0</sup>**p***<sup>n</sup>* is the end-effector position defined in transformation matrix <sup>0</sup>**T***<sup>n</sup>* defined in the previous section. Next, <sup>0</sup>**p***<sup>i</sup>*�<sup>1</sup> is the position of frame *Ii*�<sup>1</sup> <sup>¼</sup> *Oi*�1, *xi*�1, *yi*�1, *zi*�<sup>1</sup> � �, defined in transformation matrix <sup>0</sup>**T***<sup>i</sup>*�1. Finally, <sup>0</sup>**z***<sup>i</sup>*�<sup>1</sup> is the third column of rotation matrix <sup>0</sup>**R***<sup>i</sup>*�1, while <sup>0</sup>**R***<sup>i</sup>*�<sup>1</sup> <sup>¼</sup> <sup>0</sup>**R**<sup>1</sup> *<sup>q</sup>*<sup>1</sup> � �1 **R**<sup>2</sup> *q*<sup>2</sup> � � … *<sup>i</sup>*�<sup>2</sup> **<sup>R</sup>***<sup>i</sup>*�<sup>1</sup> *qi*�<sup>1</sup> � �.

The following example will demonstrate the derivation of geometric Jacobian for a two-link mechanism (**Figure 7**).

$$\mathbf{J(q)} = \begin{bmatrix} \mathbf{z}\_0 \times (\mathbf{p}\_2 - \mathbf{p}\_0) & \mathbf{z}\_1 \times (\mathbf{p}\_2 - \mathbf{p}\_1) \\ \mathbf{z}\_0 & \mathbf{z}\_1 \end{bmatrix} \tag{37}$$

$$\begin{aligned} \text{where } \mathbf{p}\_0 = \begin{bmatrix} \mathbf{0} \\ \mathbf{0} \\ \mathbf{0} \end{bmatrix}, \mathbf{p}\_1 = \begin{bmatrix} L\_1 \cos q\_1 \\ L\_1 \sin q\_1 \\ \mathbf{0} \end{bmatrix}, \mathbf{p}\_2 = \begin{bmatrix} L\_1 \cos q\_1 + L\_2 \cos \left( q\_1 + q\_2 \right) \\ L\_1 \sin q\_1 + L\_2 \sin \left( q\_1 + q\_2 \right) \\ \mathbf{0} \end{bmatrix} \text{ and } \mathbf{p}\_3 = \begin{bmatrix} \mathbf{0} \\ \mathbf{0} \end{bmatrix} \end{aligned}$$

rotational axes are **z**<sup>0</sup> ¼ **z**<sup>1</sup> ¼ 0 1 6 4 7 <sup>5</sup>. By solving Eq. (37), we get

$$\mathbf{J(q)} = \begin{bmatrix} -L\_1 \sin q\_1 - L\_2 \sin \left( q\_1 + q\_2 \right) & -L\_2 \sin \left( q\_1 + q\_2 \right) \\ L\_1 \cos q\_1 + L\_2 \cos \left( q\_1 + q\_2 \right) & L\_2 \cos \left( q\_1 + q\_2 \right) \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{bmatrix} \tag{38}$$

**Figure 7.** *Two-link mechanism.*

#### *Kinematics of Serial Manipulators DOI: http://dx.doi.org/10.5772/intechopen.93138*

Now, the Jacobian is a 6 � 2 matrix and its maximum rank is 2. That is, at most two components of angular/linear end-effector velocity can be independently assigned. In this case, when orientation is not required, only first two rows are considered.

### **5.3 Kinematically redundant manipulators**

The next approach to inverse kinematic solution we want to focus on is numerical solutions. Nevertheless, many times, it is hard to find a closed-form solution for inverse kinematics; the basic kinematics of industrial robots have developed an approach to solve it. The problems arise with nonconventional kinematic structures, especially with kinematically redundant manipulators. A kinematically redundant manipulator has more number of DOFs than is absolutely necessary to perform the desired task. For example, conventional industrial robot has usually six DOFs, by which it is able to reach any point in its workspace. By adding an additional DOF, this robot becomes kinematically redundant due to this additional DOF.

A numerical solution is usually used when a closed-form solution for **q** does not exist or is difficult to find. In this section, we will focus on kinematically redundant mechanisms. Considering the dimension of joint space *n* and dimension of task space *m*, for kinematically redundant mechanisms *n* > *m*. The level of redundancy can be expressed by *r* ¼ *n* � *m*. Kinematic redundancy is used for many tasks such as kinematic singularities avoidance, obstacle avoidance, joint limits avoidance, increasing the manipulability in specified directions, minimizing the energy consumption, minimum of motion torques, optimizing execution time, etc. As can be seen, kinematic redundancy allows many optimization tasks to be solved. On the other hand, kinematic redundancy brings some disadvantages as well; for example, a greater structural complexity of construction caused by many of DOFs (mechanical, actuators, sensors), which have an influence on final cost of this kind of mechanism. Next field of potential disadvantages is the field of control, due to complicated algorithms for inverse kinematic computation or motion control. From this reason redundant manipulators could be difficult in real-time control.

There are many approaches within numerical solution of inverse kinematics, which are still in focus of research. Most approaches deal with Jacobian matrix in order to find a linear approximation to the inverse kinematic problem. Among the most used of them are damped least squares (DLSs), Jacobian transpose, and damped least squares with singular value decomposition (SVD) [8, 9].

Another kind of approach is the approach based on Newton methods [6]. The aim of these algorithms is to find the final configuration of joints with focus on minimization problem. For this reason, the final motion of robot is smooth. This family of methods includes methods such as Powell's method [10] or Broyden, Fletcher, Goldfarb and Shanno (BFGS).

A very well-known and used method for inverse kinematics of kinematically redundant mechanisms is the so-called cyclic coordinate descent (CCD) algorithm [11]. The CCD method is a very simple and at the same time a very strong method. It is a heuristic iterative method with low computational cost per iteration. Next very know heuristic iterative method is FABRIK (Forward And Backward Reaching Inverse Kinematics) [12, 13]. The FABRIK method minimizes the system error by adjusting each joint angle one at a time.

Most of inverse kinematics numerical methods could be divided into two classes: linearization algorithms and minimization algorithms. Concerning the linearization algorithms, the idea is piecewise linearization of nonlinear inverse kinematic problem, which is based on the Jacobian matrix. An example of this kind of method is the Jacobian transpose method. In minimization algorithms, the idea is to formulate some cost function, which will be minimized, for example cyclic coordinate descent algorithm. Besides the mentioned methods, there are many other such as pseudoinverse methods, such as the Levenberg–Marquardt damped least squares methods, quasi-Newton and conjugate gradient methods, neural network and artificial intelligence methods.

The basic technique is based on Eq. (39)

$$
\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}\tag{39}
$$

The above relation can be inverted to so-called Jacobian control method

$$
\dot{\mathbf{q}} = \mathbf{J}^{\dagger}(\mathbf{q}) \dot{\mathbf{x}} \tag{40}
$$

which leads to joint velocity vector **q** with minimum norm. The term **J** † represents the Moore-Penrose pseudoinverse given by **J** † <sup>¼</sup> **<sup>J</sup>** *<sup>T</sup>* **JJ***<sup>T</sup>* �<sup>1</sup> for kinematically redundant mechanisms where *<sup>m</sup>* <sup>&</sup>lt;*<sup>n</sup>* **JJ**† <sup>¼</sup> **<sup>I</sup>** or by **<sup>J</sup>** † <sup>¼</sup> **<sup>J</sup>** *<sup>T</sup>***J** �<sup>1</sup> **J** *<sup>T</sup>* for *m* > *n* **J** † **<sup>J</sup>** <sup>¼</sup> **<sup>I</sup>** .

A very common method on which the solution is based is the Newton-Rhaphson method. The Newton-Rhaphson method is a root-finding algorithm that produces approximations of the roots of a real-valued function. The method starts with differentiable function *f* defined for a real variable *x*, derivative of function *f* 0 , and initial guess *x*<sup>0</sup> for a root of function *f*. If these assumptions are satisfied and initial guess is close, then

$$\mathbf{x}\_1 = \mathbf{x}\_0 - \frac{f(\mathbf{x}\_0)}{f'(\mathbf{x}\_0)} \tag{41}$$

Talking about numerical motion optimization of kinematically redundant mechanisms, there are two approaches. The first approach deals with local methods, which are solved typically online, and the second one with global methods, which require quantity of computation. For this reason, the global methods are computed usually offline.

One of the commonly used local methods is the family of null-space methods. This method uses the extension of Eq. (40) and gives

$$
\dot{\mathbf{q}} = \mathbf{J}^{\dagger}(\mathbf{q})\dot{\mathbf{x}} + \left(\mathbf{I} - \mathbf{J}^{\dagger}\mathbf{J}\right)\dot{\mathbf{q}}\_{0} \tag{42}
$$

where **q**\_ <sup>0</sup> ∈ *<sup>n</sup>* is an arbitrary joint space velocity vector, chosen according to desired behavior; so it is chosen for optimization purposes. Next, **I**∈ *<sup>n</sup>*�*<sup>n</sup>* is the identity matrix. The term **I** � **J** †**J** represents the orthogonal projection matrix in the null space of **J**, **J** † ð Þ **q x**\_ is orthogonal to **I** � **J** †**<sup>J</sup> <sup>q</sup>**\_ <sup>0</sup>. For this reason, **<sup>q</sup>** <sup>¼</sup> 0. Physically, this term corresponds to self-motion, where the combined joints motion generates no motion in the task space (no motion of end-effector). So, the term **I** � **J** † **<sup>J</sup>** is symmetric and idempotent ( **<sup>I</sup>** � **<sup>J</sup>** † **<sup>J</sup>** <sup>2</sup> <sup>¼</sup> **<sup>I</sup>** � **<sup>J</sup>** † **J** ). Also, it ensures **I** � **J** † **<sup>J</sup>** † <sup>¼</sup> **<sup>I</sup>** � **<sup>J</sup>** †**J** . The inverse kinematic solution expressed by Eq. (42) is equivalent to solving a quadratic programming (QP) problem based on **<sup>H</sup>**ð Þ¼ **<sup>q</sup>**\_ min **<sup>q</sup>**\_ <sup>1</sup> <sup>2</sup> **q**\_ � **q**\_ <sup>0</sup> *<sup>T</sup>* **W q**\_ � **q**\_ <sup>0</sup> subjected to **<sup>x</sup>**\_ <sup>¼</sup> **J q**ð Þ**q**\_ .

Now the question is, how the vector **q**\_ <sup>0</sup> can be chosen. One of the basic ways to choose it is the so-called projected gradient method **q**\_ <sup>0</sup> ¼ ∇*q***H q**ð Þ. Supposing that **x**\_ ¼ 0, that is, the mechanism performs only self-motion, it can be written **q**\_ ¼ **I** � **J** † **<sup>J</sup>** <sup>∇</sup>*q***H**; so, **<sup>I</sup>** � **<sup>J</sup>** †**<sup>J</sup>** <sup>∇</sup>*q***<sup>H</sup>** <sup>¼</sup> 0 is a necessary condition of constrained optimality. Based on these facts, an objective function can be chosen for some optimization of motion:

• Manipulability—maximize the distance from kinematic singularities

$$\mathbf{H(q)} = \sqrt{\det\left[\mathbf{J(q)} \mathbf{J^T(q)}\right]} \tag{43}$$

• Joint limit avoidance—minimize the distance from the middle of joints range

$$\mathbf{H(q)} = \frac{1}{2} \sum\_{i=1}^{n} \left( \frac{q\_i - q\_i^-}{q\_{M,i} - q\_{m,i}} \right)^2 \tag{44}$$

where *qi* ∈ *qm*,*<sup>i</sup>* , *qM*,*<sup>i</sup>* h i and *<sup>q</sup>*� *<sup>i</sup>* <sup>¼</sup> *qM*,*<sup>i</sup>* �*qm*,*<sup>i</sup>* <sup>2</sup> .

• Obstacle avoidance—maximize the minimum distance to obstacle

$$\mathbf{H}(\mathbf{q}) = \min\_{\mathbf{a} \in robot} \quad \left\| \mathbf{a}(\mathbf{q}) - \mathbf{b} \right\|^2 \tag{45}$$

$$\mathbf{b} \in obstacle$$

where **a q**ð Þ represents points on investigated mechanism and **b** represents points on the obstacle.

Example: Let us consider a planar six-link robot connected by six revolute joints. All links have the same length *L* ¼ 100 *mm*. The purpose of the simulation is path tracking (circle form) described by matrix **<sup>X</sup>***path* <sup>¼</sup> **<sup>x</sup>***<sup>d</sup>* **y***d* � �<sup>∈</sup> *<sup>m</sup>*�*<sup>p</sup>*, where *<sup>p</sup>* is the number of geometric points of the desired path, while **x***<sup>d</sup>* ¼ �2*:*5*L* þ *L* cos *φ*, **y***<sup>d</sup>* ¼ *L* sin *φ*, *φ*∈f g 0, … , 2*π* assuming the step of *φ* increase to be 0.2. That is, **x***<sup>d</sup>* ¼ *x*1, … , *xp* � �<sup>∈</sup> *<sup>p</sup>* and **<sup>y</sup>***<sup>d</sup>* <sup>¼</sup> *<sup>y</sup>*1, … , *yp* h i<sup>∈</sup> *<sup>p</sup>*. From the matrix **<sup>X</sup>***path* will be in each path point determined the desired point **e***<sup>p</sup>* ¼ *xpyp* h i*<sup>T</sup>* ∈ *<sup>m</sup>*. Since, there is consideration of planar task with focus on end-effector position, the task space is *m* ¼ 2. The expected solution assumes only primary solution without any secondary tasks. For inverse kinematic solution, damped least squares method, which avoids many of pseudoinverse method's problems, will be used. This method is also known as the Levenberg–Marquardt method, which arises from cost function k k **J***Δ***q** � *Δ***x** <sup>2</sup> <sup>þ</sup> *<sup>λ</sup>*<sup>2</sup> k k *Δ***q** <sup>2</sup> where *λ*∈ is a non-zero scalar constant. By minimizing this term, one obtains

$$
\Delta \mathbf{q} = \mathbf{J}^T \left( \mathbf{J} \mathbf{J}^T + \lambda^2 \mathbf{I} \right)^{-1} \Delta \mathbf{x} \tag{46}
$$

where **I** *<sup>m</sup>*�*<sup>m</sup>* is the identity matrix. The simulation will work according to the following algorithm.

**Algorithm**: Inverse kinematic model for 6-link manipulator.

**1:** Set desired path for end-effector by matrix **X***path* and parameters such as *L*, *λ*. **2: FOR** step = 1 ! *p:*

**3:** Set the desired position of end-effector from **X***path*¼)**e***<sup>p</sup>* ¼ *xpyp* h i*<sup>T</sup>* ∈ *<sup>m</sup>*



The results of the simulation can be seen in **Figures 8** and **9**. **Figure 8** presents the motion of planar robot. The aim is to tracking of path with circle shape (green color) by end-effector of the robot. The robot has a fixed frame in point [0,0]. The initial position of all joints is **<sup>q</sup>** <sup>¼</sup> ½ � <sup>000000</sup> *<sup>T</sup>*.

During the simulation, no restriction such as joint limit was considered. The simulation was done with a tolerance �5 mm. **Figure 9** presents the variation of individual robot joints during the path tracking.

Example: Let us consider a planar 20-link robot connected with revolute joints. The links have the same length *L* ¼ 16*:*75 *mm*. The aim is to move the end-effector from its initial position to the end position by tracking the desired path. We will consider two cases. The first one considers free robot environment, the second one considers obstacles in the robot environment. The second solution will also consider kinematic singularities avoidance task and joint limit avoidance task.

Now, let us consider the cost function dealing with all mentioned secondary tasks [10].

$$\mathbf{H} = \left\| \mathbf{J}\dot{\mathbf{q}} - \dot{\mathbf{x}} \right\|^2 + \left\| \mathbf{J}\_c \dot{\mathbf{q}} - \mathbf{x}\_c \right\|^2 + \left\| \mathbf{J}\_L \dot{\mathbf{q}} - \mathbf{x}\_L \right\|^2 + \left\| \rho \dot{\mathbf{q}} \right\|^2 \tag{47}$$

After mathematical adjustment, we get the final formula for kinematic control

$$\dot{\mathbf{q}} = \left(\mathbf{J}^T \mathbf{W} \mathbf{J} + \mathbf{J}\_c^T \mathbf{W}\_c \mathbf{J}\_c + \mathbf{J}\_L^T \mathbf{W}\_L \mathbf{J}\_L + \mathbf{W}\_s\right)^{-1} \left(\mathbf{J}^T \mathbf{W} \dot{\mathbf{x}}\right) \tag{48}$$

**Figure 8.** *Simulation of inverse kinematics for six-link manipulator.*

**Figure 9.** *Variations of joint angles.*

where **W**, **W***c*, **W***L*, and **W***<sup>s</sup>* are in our case 20 � 20 weight matrices of primary task, obstacle avoidance task, joint limit avoidance task, and kinematic singularities avoidance task, respectively. The setting of weight matrices is subjective. For example, if obstacle avoidance task should have higher priority above primary task by 100 times, the matrix **W***<sup>c</sup>* would consist of values 100 while **W** consist of values 1.

Let us consider individual secondary tasks. The joint limit avoidance task deals with the range of motion of individual manipulator links. How many joints will have their limit range is up to us. Of course, in real robots, usually all joints are limited in their motion. There are several ways on how to model the range of joint motion. In this case, an approach with changing of value of weight variable *Wli* based on joint position will be used. If the joint is in admissible range, the value of the weight variable is set to be zero. When the joint reaches the boundary of its range motion, the value of the weight variable increases. When the joint reaches a value outside its admissible range, the value of the weight variable increases to its maximum. This approach can be expressed by Eq. (49)

$$\mathbf{W}\_{\mathrm{li}} = \begin{cases} \mathbf{W}\_{\mathrm{w}} \leftarrow \mathbf{q}\_{i} < \mathbf{q}\_{\mathrm{lim}} \\\\ \frac{\mathbf{W}\_{\mathrm{w}}}{2} \left\{ 1 + \cos \left[ \pi \left( \frac{\mathbf{q}\_{i} - \mathbf{q}\_{\mathrm{lim}}}{\rho\_{i}} \right) \right] \right\} \leftarrow \mathbf{q}\_{\mathrm{lim}} \leq \mathbf{q}\_{i} \leq \mathbf{q}\_{\mathrm{lim}} + \rho\_{i} \\\\ 0 \leftarrow \mathbf{q}\_{\mathrm{lim}} + \rho\_{i} < \mathbf{q}\_{i} < \mathbf{q}\_{\mathrm{lim}} - \rho\_{i} \\\\ \frac{\mathbf{W}\_{\mathrm{w}}}{2} \left\{ 1 + \cos \left[ \pi \left( \frac{\mathbf{q}\_{\mathrm{lim}} - \mathbf{q}\_{\mathrm{i}}}{\rho\_{i}} \right) \right] \right\} \leftarrow \mathbf{q}\_{\mathrm{lim}} - \rho\_{i} \leq \mathbf{q}\_{\mathrm{lim}} \\\\ \mathbf{W}\_{\mathrm{w}} \leftarrow \mathbf{q}\_{\mathrm{i}} > \mathbf{q}\_{\mathrm{lim}} \end{cases} \tag{49}$$

The value of the weight variable has to be set for every joint of the manipulator that needs to be limited in the range of motion. Individual weight variables *Wli* where *i* ∈f g 0, … , *N* are parts of the final weight matrix of the joint limit avoidance task *Wl* ∈ *<sup>n</sup>*�*<sup>n</sup>*. The final weight matrix **W**<sup>l</sup> is the diagonal matrix [14]:

$$\mathbf{W}\_{\rm l} = \begin{bmatrix} \mathbf{W}\_{\rm l1} \\ & \mathbf{W}\_{\rm l2} \\ & & \mathbf{W}\_{\rm l3} \\ & & & \cdots \\ & & & \mathbf{W}\_{\rm ln} \end{bmatrix} \tag{50}$$

The weight matrix **W**<sup>l</sup> is used with the corresponding Jacobian matrix **J***<sup>L</sup>* ∈ *<sup>n</sup>*�*n*. The Jacobian matrix for the joint limit avoidance task is **<sup>J</sup>***<sup>L</sup>* <sup>¼</sup> *<sup>∂</sup>***e***=∂***q**. If a particular joint does not consider the joint limit avoidance task, the value of **J***<sup>L</sup>* is set to be zero; otherwise it is set to be one. The limit of all joints in motion of the manipulator investigated in this study is set to be �100°. Different way of joint limit control is according to above mentioned Eq. (44).

During the obstacle avoidance task, the control system investigates the relation between manipulator links and obstacles in their environment. In general, this task can be solved from two views. At first, one group of obstacles can represent static obstacles or other robots in an investigated environment. The second group of obstacles can be represented by dynamic obstacles, which means that these obstacles change their position relating to global reference system in the time. Of course, the second group of obstacles is more difficult from the view of control in comparison with static obstacles. It is more difficult especially in the cases of requirements for real-time control [15].

The aim of obstacle avoidance is to prevent the collision between any part of the manipulator and potential obstacles, other robots, or collision with itself. Again, there are many methods on how to control robot motion at a safe distance from other objects, regardless of whether the obstacles have regular or irregular shape. For simplification, the irregular shapes are usually replaced by appropriate regular shape. This simplification can also significantly simplify the mathematical model and obtaining the numerical solution can be faster and the solution more stable. One of the methods on how to simplify irregular shapes is to replace all irregular shapes by a cylinder, with the obstacle being situated in the center of the cylinder, see **Figure 10**. The diameter of the cylinder determines the distance of influence of this obstacle.

At first, we set the coordinate of an obstacle in the task space as **s***o*. The projection of the line from the *i*-th joint of the manipulator link to the center of a cylinder (obstacle) on the *i*-th link is:

$$\mathbf{p}\_{\mathrm{i}} = \mathbf{e}\_{\mathrm{i}}^{\mathrm{T}}(\mathbf{s}\_{0} - \mathbf{s}\_{\mathrm{i}}) \tag{51}$$

The coordinate of the link point with potential to get into collision is:

$$\mathbf{s}\_{\rm ci} = \mathbf{s}\_{\rm i} + \mathbf{p}\_{\rm i} \mathbf{e}\_{\rm i} \tag{52}$$

The distance between the potential point of collision and the center of the cylinder is:

$$\mathbf{d}\_{\rm ci} = \|\mathbf{s}\_{\rm ci} - \mathbf{s}\_0\|\tag{53}$$

Subsequently, the unit vector of the potential point of collision can be given by:

$$\mathbf{u}\_{i} = \frac{\mathbf{s}\_{\text{ai}} - \mathbf{s}\_{0}}{\mathbf{d}\_{\text{ci}}} \tag{54}$$

Now, the Jacobian matrix for the obstacle avoidance task can be given. The *i*-th row of the Jacobian matrix can be expressed as

**Figure 10.** *Relation between manipulator link and obstacle.*

$$\mathbf{J}\_{\dot{\mathbf{c}}} = -\mathbf{u}\_{\dot{\mathbf{i}}}^{\mathrm{T}} \mathbf{J}\_{\mathbf{s}\_{\dot{\mathbf{c}}}} \tag{55}$$

where matrix Jsci is:

$$\mathbf{J}\_{\mathbf{s}\_{\mathrm{ci}}} = \frac{\partial \mathbf{s}\_{\mathrm{ci}}}{\partial \mathbf{q}} \tag{56}$$

The Jacobian matrix Jc consists of submatrices Jci. The dimension of the Jacobian matrix is Jc ∈Rc�<sup>c</sup> , where *c* represents the number of manipulator links that could collide with the obstacles.

For numerical simulation, we will use following algorithm: Inverse kinematic model for 20-link manipulator [14].

**Algorithm**: Inverse kinematic model for 20-link manipulator
