# Three-Dimensional Path-Following with Articulated 6 DoF Robot and ToF Sensors
## 2 Materials and Methods
### Determining the Position of the Detection Tool Based on the Position of the Gripper  
In previous sections, it was assumed that the position and orientation of the effector (detection tool) are known. However, during experimental tests, when the robot controller was queried for actual robot position, the gripper position was returned, because this is how the effector was defined in the robot controller. Of course, it is possible to define a new effector in the robot software, but it was decided to determine the position of the tool on the control computer side, i.e., in the Mathworks Matlab version 2023b (Mathworks, Natick, MA, USA) environment. This solution was selected to facilitate the incorporation of the tool geometry modifications, anticipated during project development, into the algorithm and to discard the need for adjusting the tool definition in the robot controller if any change in the tool is made. Additionally, thanks to this approach, the whole algorithm is  
Figure 7: Example of combining the next position and tool rotation; detection tool seen from the top. Symbols: O—detection tool centre in the current position; O’—detection tool centre in the previous position; S—detected path position for the current detection tool position; \(P_{n}\)—calculated next tool centre position; vectors \(\overrightarrow{w}_{a}\), \(\overrightarrow{w}\), \(\overrightarrow{t}\), \(\overrightarrow{n}\), and \(\overrightarrow{m}\) and \(a\) and \(b\) constants are described in the text. more robot-independent, i.e., there are no configuration changes to be made in the robot controller settings. The tool coordinate system is shifted (in relation to the gripper system) along the \(Z\) axis of the gripper by the distance \(l_{e}\). Its orientation in relation to the gripper system can be described by the following angles: \(\alpha_{e}\)--rotation angle around the \(X\) axis (roll); \(\beta_{e}\)--rotation angle around the \(Y\) axis (pitch); and \(\gamma_{e}\)--rotation angle around the \(Z\) axis (yaw). During the experiments, these angles were as follows: roll \(180^{\circ}(\pi\ rad)\), pitch \(0^{\circ}(0\ rad)\), and yaw \(-45^{\circ}\big{(}\frac{\pi}{4}\ rad\big{)}\). In order to find the position of the tool, the first step is to determine the rotation matrix of the gripper system with respect to the global coordinate system. Using the relationships describing the rotation matrices around individual axes Equations (1)-(3), the rotation matrix of the gripper system has the following form:  
\[R_{c}=R_{z}(\gamma_{c})\cdot R_{y}(\beta_{c})\cdot R_{x}(\alpha_{c}), \tag{23}\]  
where \(\alpha_{c}\), \(\beta_{c}\), \(\gamma_{c}\)--gripper rotation angles (roll, pitch, and yaw); \(R_{c}\)--gripper rotation matrix. The tool coordinate system is shifted along the gripper \(Z\) axis in the positive direction. Knowing the position of the centre of the gripper coordinate system, it is possible to determine the position of the centre of the tool system using Equation (5). The formula for the position of the centre of the tool system takes the following form:  
\[O=O_{c}+R_{c}\cdot \begin{bmatrix}0&0&1\end{bmatrix} ^{T}\cdot l_{e}, \tag{24}\]  
where \(O_{c}\)--gripper coordinate system centre. Next, the orientation of the tool coordinate system relative to the global coordinate system must be determined. First, the rotation matrix of the tool system in relation to the gripper system is determined, again using Equations (1)-(3):  
\[R_{cc}=R_{z}(\gamma_{e})\cdot R_{y}(\beta_{c})\cdot R_{x}(\alpha_{e}), \tag{25}\]  
where \(R_{cc}\)--rotation matrix of the tool's coordinate system relative to the gripper's coordinate system. Using matrices \(R_{c}\) and \(R_{cc}\), it is possible to determine the rotation of the tool coordinate system in relation to the global coordinate system (i.e., to the robot base):  
\[R_{e}=R_{c}\cdot R_{cc}, \tag{26}\]  
where \(R_{c}\)--rotation matrix of the tool coordinate system relative to the coordinate system of the robot base.