## 3 Methods and Materials
### Application Logic Unit Layer
#### 3.4.4 Image Base Visual Servoing  
To control the end-effector movements, the \(IBVS\) algorithm is used. The latter is a technique to realize a closed-loop feedback control by comparing the center of gravity of the chosen object with a point (_Camera features_) in the camera frame [29], whose coordinates are called as Equation (4).  
\[K_{x},K_{y} \tag{4}\]  
\(IBVS\) approaches may use more reference points to avoid rotation around the axis passing trough the camera plane. Depending on the number of camera features used, an _Interaction Matrix_ (\(J_{IM}\)) is defined. For a single camera features, it is made as shown in Equation (5):  
\[J_{IM}=\begin{bmatrix}\frac{-f_{x}}{depth}&0&\frac{u}{depth}&\frac{u\cdot p}{ f_{x}}&-(f_{x}+\frac{u\cdot u}{f_{x}})&v\\ 0&\frac{-f_{y}}{depth}&\frac{v}{depth}&(f_{y}+\frac{v\cdot u}{f_{y}})&\frac{-u \cdot p}{f_{y}}&-u\end{bmatrix} \tag{5}\]  
where \(f_{x}\) is the focal length for x axis of the camera; \(f_{y}\) is the focal length for y axis of the camera; \(u\) is the coordinate of the center of gravity of the object on x axis; \(v\) is the coordinate of the center of gravity of the object on y axis; and \(depth\) is the distance taken from the depth sensor.  
Thus, the coordinates expressed as pixels and the distance from the target are taken by the \(IBVS\) to calculate the difference between them and the camera features position, computing the error estimation (\(\epsilon_{x,y}\)). The last one is multiplied by a pseudo-inverse of the _Interaction Matrix_ (\(J_{IM}^{-1}\)) to calculate the effective translational and rotational velocity. It is worth noting how the algorithm also takes in to account the depth between the end-effector and the target to compute the difference of prospective, which permits to control a robot using a 2D camera.  
The procedure to compute such error estimation, the pseudo-inverse of the _Interaction Matrix_ and the end-effector velocity are described in Equation (6).  
\[\begin{array}{c}\epsilon_{x,y}=\begin{bmatrix}u\\ v\end{bmatrix}-\begin{bmatrix}K_{x}\\ K_{y}\end{bmatrix}\\ \\ J_{IM}^{-1}=J_{IM}^{T}\cdot(J_{IM}\cdot J_{IM}^{T})^{-1}\\ \\ \vec{V}=J_{IM}^{-1}\cdot e_{x,y}\end{array} \tag{6}\]  
Thanks to them, it is possible to use the standard kinematic equation to compute the next step as:  
\[\dot{q}=J^{T}\cdot(J\cdot J^{T})^{-1} \tag{7}\]  
\(\dot{q}\) is proportional to the increment that the rotative joints shall perform to reach the new position. Thus, once \(\dot{q}\) is calculated, it is used for deriving the value of the new joint position \(q[n+1]\), as expressed in Equation (8). \(q[n+1]\) is provided to the _Phydget_, included in the _Hardware Layer_, which directly drives the motors.  
\[q[n+1]=q[n]+\dot{q}\cdot\Delta T \tag{8}\]  
\(\Delta T\) is the time necessary to the _Application Logic Unit Layer_ to process the relevant image frame from its acquisition to the end of the loop. This time is used to perform all the derivation needed to compute the path.  
The approaching phase is handled by the \(IBVS\) by moving the end-effector toward the target. This phase is guided by the proximity sensor that avoid the collision of the end-effector with thetarget. Using the depth sensor within the above algorithm allows you to drive a real manipulator that performs movements in three dimensions, using the algorithms KCF and AI working on two dimensions. Obviously, it is necessary that the field of action of the robot is free of obstacles, otherwise the robot enters in error mode, and waits until a hard restart.