# A 6 DOF Virtual Environment Space Docking Operation with Human Supervision
## 2 Real and Virtual Setup Description
The inverse kinematic model is described as  
\[\varphi=\pm\arccos\left(\frac{M}{\sqrt{K^{2}+N^{2}}}\right)+\arctan\left( \frac{N}{K}\right)+k\pi,\ \forall k\in\mathbb{Z}, \tag{1}\]  
Figure 2: (**a**) Real world six degree of freedom (6 DOF) platform for co-simulation, including a frame and Kinect IR-camera; (**b**) Relation between coordinate systems of the real world and the virtual world. where  
\[ \begin{array}{ll}M=\ell^{2}-\ell_{1}^{2}-r^{2}\\ K=2(x_{a}-x_{c})r\cos\theta+x_{a}+2(y_{a}-y_{c})r\sin\theta+y_{a}\\ N=2(z_{a}-z_{c})r\end{array} . \tag{2}\]  
The inverse kinematic model from (1) has four solutions:  
\[ \begin{array}{l}\varphi_{1}=\arccos\left(\frac{M}{\sqrt{K^{2}+N^{2}}}\right) +\arctan\left(\frac{N}{K}\right)+\pi\\ \varphi_{2}=-\arccos\left(\frac{M}{\sqrt{K^{2}+N^{2}}}\right)+\arctan\left( \frac{N}{K}\right)+\pi\\ \varphi_{3}=\arccos\left(\frac{M}{\sqrt{K^{2}+N^{2}}}\right)+\arctan\left( \frac{N}{K}\right)\\ \varphi_{4}=-\arccos\left(\frac{M}{\sqrt{K^{2}+N^{2}}}\right)+\arctan\left( \frac{N}{K}\right)\end{array} . \tag{3}\]  
which are illustrated in Figure 3b. Due to the employed mathematical method, a second point \(C^{\prime}\) is found; thus, solutions \(|varhi_{3}\) and \(\varphi_{4}\) (for points \(B_{3}\) and \(B_{4}\)) are discarded. Solution \(\varphi_{1}\) and \(\varphi_{2}\) (for points \(B_{1}\) and \(B_{2}\)) brings the connection \(C\) to its desired position. As the servo position is restricted between \(\pm 90^{\circ}\), solution \(\varphi_{1}\) (point \(B_{1}\)) is selected as the unique feasible solution. The closed loop control uses classical visual feedback control based on camera vision information, as described in [14]. There are many possibilities for mapping a 3D environment, ranging from very expensive and accurate technologies to low-cost devices which are available for consumers. The Microsoft Kinect is a perfect example of such a low-cost sensor, and this has had a huge impact on recent research in computer vision as well as in robotics. Here, a Kinect v2 sensor is used as it was available in the laboratory. The Kinect v2 sensor holds two cameras (a RGB and an infrared (IR) camera), an IR emitter and a microphone bar, as shown in Figure 4a. The Kinect software maps the depth data to the coordinate system, as shown in Figure 4a, with its origin located at the center of the IR sensor. Note that this is a right-handed coordinate system with the orientation of the y-axis depending on the tilt of the camera and one unit representing one meter. Kinect v2 uses optical time-of-flight (ToF) technology in order to retrieve the depth data with the infrared (IR) camera. The basic principle of ToF is to measure the time difference between an emitted light ray and its collection after reflection from an object (Figure 4b). The RGB-camera within the Kinect sensor has a resolution of 1920 \(\times\) 1080 pixels with a field of view of 70.6 \(\times\) 60 degrees, while the IR-camera resolution is 512 \(\times\) 424 pixels and the field of view is 84.1 \(\times\) 53.8 degrees. The operative measuring range of the Kinect sensor is between 0.5 and 4.5 m and the frame rate is 30 fps. Figure 3: (**a**) Local coordinate frame for a servo control unit; (**b**) mapping of possible solutions for (1). The intrinsic parameters of the depth camera of the Kinect, the focal length and the principal point, can be acquired using the Kinect For Windows SDK while the intrinsic parameters of the RGB camera can be acquired with the Camera Calibration Toolbox for MATLAB. Next, the virtual model was constructed with a Siemens NX12.0 based on the existing 6 DOF model of the platform [10; 11; 12; 13]. Combining the NX12.0 Motion environment and the inverse kinematics calculations programmed in VisualBasic.Net, a simulation of the virtual world model was executed. Finally, a connection between the LabVIEW program and the NXOpen program was created using registry keys in the Windows Registry, as this allowed for the continuous monitoring of the keys without hindering access to the key from other processes.