Digital Twins of Robotic Systems: Increasing Capability for Industrial Applications

Abstract

Digital twin is one of the emerging areas of research and technology development and the enabling technologies of Smart Manufacturing and Industry 4.0. This study aims to develop and demonstrate a proof-of-concept prototype with a case study of the digital twin of a robotic system. The system has two main elements: the virtual element and the physical or the real element. The virtual element of system has been built based on the Unity platform, which is a cross-platform game engine developed by Unity Software Inc., and the physical element was built with the use of two servomotors and the NVIDIA^® Jetson Nano™ Developer Kit. The virtual and the physical elements are connected and communicated via using the TCP socket protocol suite. A digital twin model of the ABB IRB 120 robot was successfully developed and demonstrated. The collected data include the joint angle position values of the physical and virtual models, and they are stored both locally and in the cloud for the future system development, which can be used as for minimizing the errors between the physical and virtual models of digital twins of robotic systems. The successfully developed digital twin model can be considered as the cost-effective solutions for demonstrating and evaluating potential applications of digital twins in industrial practices as well as in higher educations and research.

1 Introduction

A digital twin [1, 2] is a digital duplicate of a physical entity whose base is infrastructure and data and at its core are algorithms and models and applications software and services. This technology along with robotics has been growing rapidly in recent years and is considered by research and industry to be the main driving factors for Industry 4.0. Under the development of artificial intelligence (AI) [3,4,5,6,7], robotics [8,9,10,11] has made breakthrough developments in the capabilities of robots such as performing complex jobs with high danger, taking care of health, cooperation with people, and more. The combination of digital twin and robot applying new technologies and algorithms will be the foundation for building and developing smart manufacturing. The trends of using AI to analyze data and make prediction are being special care.

The Robot Operating System (ROS) platform is a collection of tool software libraries that help build robotic applications with source code [12]. The control algorithms tend to apply ML to increase performance and optimization process. In this study, ROS is used as middleware to connect the virtual and real parts and makes it possible to apply platforms like motion planning [13,14,15,16], autonomous mobile robotics [17, 18].

A digital twin provides various features such as visualization, simulation with real data, and performance monitoring. To do that, the amount of data collected must be stored. Firebase is a cloud-based database service, accompanied by an extremely powerful server system of Google. Firebase provides capabilities such as analytics, databases, activity reports, and error reporting for easy development. It has versatile services and trusted security. Therefore, the data in this project, namely the joint values and the execution time, are all stored using Firebase.

The main contribution of this paper includes data storage cloud which are the limitations of the recent research papers [19,20,21,22]. In addition, we build virtual environment, calculated the kinematics, assure real time two-way data transmission between virtual and analyze and evaluate the obtained data.

In this paper, we divide into five main sections. We first build a virtual model and a virtual environment for the robot. Second, we build the real part system for the robot. Third, we connect and transfer data between the real and the virtual models. Fourth, we calculate the kinematics for the robot. Finally, the data will be transmitted in both directions to control, evaluate errors, and store the data on both local storage and the cloud.

2 Related Works

2.1 Proposed Framework

We have built a digital duplicate for a robot architecture. It is divided into two components: virtual and real parts. The virtual element is a software built on the Unity platform. For the real part, we use two servomotors with built-in encoder to represent the two joints of the robot. These two motors are controlled via a PID controller. In addition, we also use Jetson Nano with integrated ROS as a central processor. The virtual element and the real part are connected to each other through the TCP socket protocol suite.

Jetson Nano is a small but very powerful computer that allows to run multiple neural networks in parallel for applications such as image classification, object detection, segmentation, and speech processing. Being in one platform, it is easy to use and consumes less than 5 W. The Jetson Nano also delivers 472 GFLOPS to run modern AI algorithms quickly, with a 64-bit ARM quad-core CPU, an onboard 128-core NVIDIA GPU, as well as 4 GB of LPDDR4 memory. It is possible to run multiple neural networks in parallel and handle several high-resolution sensors simultaneously. Here, users can control from the virtual model to real model and vice versa. First, we will present the control process from virtual to real model. When the user changes the robot’s joint position via the Unity controller, the matching angle data will be sent through the central processor. The central processor will calculate the forward kinematics to give the position of the end of the manipulator. Next, the matching angles will be controlled by the Virtual robot model, and the data is also given to the user interface to display the joint angle values and the coordinates of the robot’s manipulator end point. After that, the data will be sent to the Physical robot to control the real model through the TCP socket protocol suite.

In this study, we take two servomotors representing two joints of the robot. The data will be sent to the central processor and then sent to the controller to control the servomotor. Then the controller will take the position value of the servomotor and send it to the central processor. The central processor will process the data through the aforementioned formula and calculate the error between the matching angles of the real and imaginary model. The data will then be stored on Cloud and sent back to Unity which calculates the delay and then saves the data including the error, the position of the real and virtual model’s joints, and the delay to local storage. For the control process from real to virtual object, it will reverse the above process and start from the controller at ROS. In this process, the Unity side controller will be ignored and the robot’s matching angle values will be fed directly into the Virtual robot model through Unity’s central processor. Figure 1 is the architecture of the system.

Fig. 1

System overview. The system consists of two parts: real and virtual models. On the left is a virtual element system built on the Unity platform. On the right is the real part system

2.2 Virtual Environment

We use the Unity platform to build and develop virtual environments. Unity is software that specializes in games, so building a virtual environment is almost easy. In addition, Unity also supports virtual reality and augmented reality for future development of the system. To build a virtual environment on Unity, we used available tools and some other graphics software like Blender. After building the objects of the virtual environment, we started to set up features such as data communication, kinematics, and rotation controller for the robot. In order to build a robot model and put it into a virtual environment, we first downloaded the details of the ABB IRB 120 robot model from the ABB homepage. Second, we edit and stitch the parts together into a robot ABB IRB 120. Third, we set the coordinate axes for each joint of the robot model. Fourth, we set the parameters of the model. Fifth, we exported the model as a URDF file and stored it in the directory where the project is located. Finally, we bring that model into the Unity virtual environment.

In the virtual environment with the Virtual robot model, the central processor will process the data, store the data in local memory, and transmit the data to the real model, meanwhile the controller helps the user to change the angles. This virtual environment allows the data visualization, monitoring, and control. Figure 2 shows the virtual environment built.

Fig. 2

Virtual environment in Unity includes robot model, robot status panel, and control panel

3 Approach: Digital Twin of Joint Angle of Robot Arm

3.1 Compare Inverse Kinematics Between the Construction Algorithm and Robot Studio

After building the inverse kinematics algorithm, we compare the inverse kinematics with Unity, MATLAB, and Robot Studio. We found that the coordinates of Unity, the algorithm we built, and Robot Studio were interchanged. The joints values have very low errors. These errors are due to the rounding process in the algorithm's calculation. The results showed that our inverse kinematic calculation is valid within very acceptable limits of error. Table 1 is the test value obtained after testing.

Table 1 Evaluation of inverse kinematics

3.2 Build a PID Controller

A PID controller is built for the purpose of controlling the motor rotation angle. Planetary GP36 Planetary Reducer DC Servomotor with 1:27 deceleration ratio with 145 rpm integrated Optical Encoder 500 CPR (count per round) and two channels A–B has been used. From here, we get the formula to convert between pulse count to angle with angle being the current angle and p being the count of pulses, c is the number of pulses of the rising or falling edge when the motor is turn 1 revolution, and r is rate of motor reducer:

$$ \alpha \left( x \right) = \frac{p}{{{\text{c}} \times {\text{r}}}} \times 360 $$

(1)

$$ = \frac{p}{500 \times 27} \times 360 = \frac{p}{37.5} $$

(2)

The PID controller for the motor includes three parameters: K_p; K_i; and K_d. The above parameters are determined by the method of manual adjustment through experiment, respectively, K_p = 0.01; K_i = 0.002; and K_d = 0.001. The built-in PID controller is used to control the rotation position of the motor with the feedback signal being the number of pulses of the encoder. Through the PID algorithm, it will determine the voltage to be supplied to the motor (the desired voltage value). U(V) is the value of voltage to be supplied to the motor is shown by the formula:

$$ U = K_{p} \times e + K_{i} \times \left( {e_{{{\text{pre}}}} + e \times \left( {t - t_{{{\text{pre}}}} } \right)} \right) + \frac{{K_{d} \times \left( {e - e_{{{\text{pre}}}} } \right)}}{{t - t_{{{\text{pre}}}} }} $$

(3)

In which, K_p, K_i, K_d, respectively, are the PID coefficients declared above. t is the current sampling time, and t_pre is the immediate previous sampling time. t and t_pre have units of seconds (s). e is the error between the desired motor angle (p) and the current motor angle (p_cur). e_pre is the number of times before.

$$ e = p - p_{{{\text{cur}}}} $$

(4)

Since the power supply for the motor driver (U_Pow) is 12 V source and the microcontroller is 8-bit, the formula for converting voltage to control pulse (Pwm) is

$$ {\text{Pwm}} = \frac{U \times 255}{{U_{{{\text{Pow}}}} }} = \frac{U \times 255}{{12}} = U \times 21.25 $$

(5)

3.3 Two-Way Data Transfer Between Real and Virtual Models

Here, we install ROS on a Jetson Nano and set up nodes and topics to transmit and receive data between Arduino and ROS and between ROS and Unity. ROS is an intermediary that supports sending and receiving data between Arduino and Unity. We use TCP socket protocol for two-way data transmission between ROS and Unity and UART for two-way data transmission between ROS and Arduino.

A TCP end point running as a ROS node facilitates message passing to and from Unity and ROS. The message being passed between Unity and ROS is expected to be serialized as ROS would internally serialize them.

4 Experiences and Results

4.1 Compare the Forward Kinematics Between the Construction Algorithm and Robot Studio

After building the forward kinematics algorithm, we compare the forward kinematics with Unity and Robot Studio. We found that the coordinates of Unity, the algorithm we built, and Robot Studio were interchanged. Here, Unity’s X-axis is the Y-axis of our algorithm and Robot Studio’s X-axis. Unity's Y-axis is the Z-axis of the algorithm we built and Robot Studio's Z-axis. Unity’s Z-axis is the X-axis of the algorithm we built and the Y-axis of Robot Studio. The obtained values have very low errors. These errors are due to the rounding process in the algorithm’s calculation. That proved our algorithm can accept. Table 2 is the test value obtained after testing.

Table 2 Evaluation of forward kinematics

4.2 Error and Latency

The error between the virtual model and the real model is calculated by the central processing unit of Physical robot. We take the value of the virtual model matching angles minus the value of the real model matching angles. Here, E_m is the value of the joint virtual model and E_n is the value of the joint physical model with m and n are from 1 to the last retrieved data stored at the system. From there, we calculate the ΔE(t) of the system using the formula:

$$ {\Delta }{\mathbb{E}}\left( t \right) = {\mathbb{E}}_{m} - {\mathbb{E}}_{n} $$

(6)

To calculate the delay, we took the time of data sent from Virtual robot to Physical robot set as t₁ and time of data sent from Physical robot to Virtual robot set as t₂. From there, we calculate the Δt(s) (latency) of the system using the formula:

$$ {\Delta }t\left( s \right) = t_{2} - t_{1} $$

(7)

4.3 Data Storage

The data is stored locally, in the Firebase Cloud and Google sheet. Cloud Firestore’s data transmission and reception rate is 10 Hz, and a limit of 50,000 data updates and edits in one day for the free version. We also tried storing data on Google Sheets, and it also receives and sends data at 10 Hz and is limited to 100 fetches and updates per minute. The data is divided into three parts including the collection, the directory and the file. The collection we named data contains folders inside. The name of each folder is the time that the data inside that folder is stored in the Cloud. And the file is inside the directory. The data including the time, the matching position of the virtual model, the rotational position of the servomotor and the error between the matching position of the virtual model and the position of the virtual model, and rotation position of the servomotor at that time was collected.

We also used one more local data storage. With this local data storage, the frequency is around 165 Hz and is limited to memory on local storage only. This is not a concern because local storage can expand memory and it also takes a long time to fill this local storage. Where the STT column is the ordinal number of the data sorted in ascending order based on the time that the data was stored, the Time column is the column that stores the data about the time that the data was stored. The Error column is the column used to represent the error between the matching position of the virtual model and the real one. The Joints Unity column is the column representing the joint position of the virtual model, and the last column is the Joints Servo column, which represents the rotation position of the servomotor.

4.4 Results

The analyzed data showed that the system takes about 2 s for the system to stabilize after booting. Here, the sampling frequency is about 165 Hz. They are shown in Fig. 3. Here, the vertical axis is the value of the number of samples taken in one second and the horizontal axis is the time the sample is taken. The system latency is about 30 ms with an error of ± 2 degrees. They are shown in Fig. 4. Figure 4 has the vertical axis being the error between the matching angle of the virtual and real model (degrees) and the horizontal axis being the time (ms).

Fig. 3

Number of samples received in 1 s

Fig. 4

Error between real and virtual model

Here, we have simulated servomotor power failure. Figure 5 shows the difference of the data when the fault occurs and when the failure occurs. When there are 3000 data stored, we have disconnected the servomotor and powered up again when 3150 data is stored, and we have also disconnected the servomotor when there is 4665 data until it stops completely system. Error is error of joint between real and virtual when the system is operating normally.

Fig. 5

Error between real and virtual model when there is a problem

5 Conclusion

In this paper, we have successfully built a digital twin model of ABB IRB 120 robot. The digital twin model has developed communication between real and virtual through the TCP socket protocol suite. We also calculated the forward kinematics of the robot and compared it with the forward kinematics of Robot Studio. In addition, we also build a PID controller for the engine and store the data on Cloud and local storage for later machine learning development.

The system operates stably with a maximum frequency of 165 Hz when not storing data and 10 Hz when putting data in the cloud. Calculation of kinematics gives results with very small error due to rounding of values in the calculation. The PID controller is stable and has been able to control the position of the motor as desired. Data is transmitted bidirectionally with very small errors due to rounding during conversion in calculations.

In the future, we will increase the frequency of sending data to the cloud and use machine learning to analyze the collected data to help monitor and evaluate the system. We will then make digital copies of typical mobile robots like manipulator platform and rehabilitation robots that we plan to do in the near future.

Appendices

Appendix

Kinematics DH

We use ABB IRB 120 robot model as a model to calculate forward and experimental kinematics. We have set representation of the coordinate positions and orientation based on the joints of the robot model as shown in Fig. 6. Figure 6 is a coordinate axis location diagram with the red axis being the X-axis, the orange axis being the Y-axis, and the blue axis is the Z-axis.

The DH parameters of the ABB IRB 120 for the specified joint frames in Fig. 6 are presented in Table 3. Here, $\theta_{i}$ is the relative rotation of the stitches, $a_{i}$ is the length, $\alpha_{i}$ is the twist angle of the stitch, and $d_{i}$ is the distance between the stitches.

Table 3 DH parameters of robot ABB IRB 120

Fig. 6

Schematic and frames assignment of ABB IRB 120. ⁰P = [⁰R₁.¹R₂.²R₃.³R₄.⁴R₅.⁵R₆].⁶P = ⁰R₆.⁶P is the direction and coordinate location

According to the manufacturer’s specifications, the limits of joints are shown in Table 4.

Table 4 Table ranges of motion in every axes

Each link can bethink as a coordinate transformation from the previous coordinate system to the next coordinate system. That transformation is described as a product of translations along and rotations about X- and Z-axes. They are expressed in formula 33:

$$ ^{n - 1} T_{n} = \left[ {\begin{array}{*{20}l} {\cos \theta_{n} } \hfill & { - \sin \theta_{n} \cos \alpha_{n} } \hfill & {\sin \theta_{n} \sin \alpha_{n} } \hfill & {a_{n} \cos \theta_{n} } \hfill \\ {\sin \theta_{n} } \hfill & {\cos \theta_{n} \cos \alpha_{n} } \hfill & { - \cos \theta_{n} \sin \alpha_{n} } \hfill & {a_{n} \sin \theta_{n} } \hfill \\ 0 \hfill & {\sin \alpha_{n} } \hfill & {\cos \alpha_{n} } \hfill & {d_{n} } \hfill \\ 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] $$

(8)

Forward Kinematics

The purpose of this part is to find the absolute position and orientation of each frame (which is attached to each joint/servo) in reference to the global coordinate system, which is the very first frame attached to the shoulder. Apply Table 1 attributes to Eq. 33, we get transformation for each link.

Now that we have each transformation, we can calculate the position and orientation of each frame in reference to the “global coordinate system.”

$$ { }^{0} T_{6} =^{0} T_{1}^{1} T_{2}^{2} T_{3}^{3} T_{4}^{4} T_{5}^{5} T_{6} $$

(9)

$$ = \left[ {\begin{array}{*{20}l} {r_{11} } \hfill & {r_{12} } \hfill & {r_{13} } \hfill & {p_{x} } \hfill \\ {r_{21} } \hfill & {r_{22} } \hfill & {r_{23} } \hfill & {p_{y} } \hfill \\ {r_{31} } \hfill & {r_{32} } \hfill & {r_{33} } \hfill & {p_{z} } \hfill \\ 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] $$

(10)

In matrix 35, the parameters r form the rotation matrix of the robot and the parameter P is the coordinate position of the end point of the manipulator. From that, we can derive the formula for determining the position of the manipulator's end point coordinates as follows:

$$ p_{x} = d_{5} c_{1} s_{234} + d_{4} s_{1} - d_{6} c_{1} c_{234} + a_{2} c_{1} c_{2} + d_{6} c_{5} s_{1} + a_{3} c_{1} c_{2} c_{3} - a_{3} c_{1} s_{2} s_{3} $$

$$ p_{y} = d_{5} s_{1} s_{234} - d_{4} c_{1} - d_{6} s_{1} c_{234} + a_{2} c_{2} s_{1} - d_{6} c_{5} c_{1} + a_{3} c_{2} c_{3} s_{1} - a_{3} s_{1} s_{2} s_{3} $$

$$ p_{z} = d_{1} - d_{6} s_{234} s_{5} + a_{3} s_{23} + a_{2} s_{2} - d_{5} c_{234} $$

Inverse Kinematics

A very large number of joints make it difficult to solve the trigonometric equation, we can divide 6 joints into two groups of joints: Group one is from joint 1 to joint 3, and group two is from joint 4 to joint 6. For joint 1 to joint 3: $P_{c}$ point coordinates are shown in Fig. 7. Parameters d₁, a₂, a₃, and d₄ are taken in Table 3. Change to new end point $P_{c} = \left[ {P_{cx} ;P_{cy} ;P_{cz} } \right]$ with the matrix a being the cosine of the three Euler angles:

$$ P_{c} = P\left[ {x;y;z} \right] - d_{6} a\left[ {a_{x} ;a_{y} ;a_{z} } \right] $$

(11)

Fig. 7

Point coordinates $P_{c}$ ($Pc_{x}$; $Pc_{y}$; $Pc_{z}$)

Perform inverse matrix multiplication:

$$ (^{0} T_{1} )^{ - 1} \times \left[ {\begin{array}{*{20}l} {r_{11} } \hfill & {r_{12} } \hfill & {r_{13} } \hfill & {p_{x} } \hfill \\ {r_{21} } \hfill & {r_{22} } \hfill & {r_{23} } \hfill & {p_{y} } \hfill \\ {r_{31} } \hfill & {r_{32} } \hfill & {r_{33} } \hfill & {p_{z} } \hfill \\ 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] = (^{1} T_{2} )(^{2} T_{3} )(^{3} T_{4} ) $$

(12)

From 2, we have the formula

$$ P_{cy} \cos \left( {\theta_{1} } \right) - P_{cx} \sin \left( {\theta_{1} } \right) = 0 $$

(13)

$$ = > \theta_{1} = a\tan 2\left( {P_{cy} ,P_{cx} } \right) $$

(14)

$$ 2a_{2} a_{3} \cos \left( {\theta_{3} } \right) - 2a_{2} d_{4} \sin \left( {\theta_{3} } \right) = n_{x}^{2} + n_{y}^{2} - \left( {a_{2}^{2} + a_{3}^{2} + d_{4}^{2} } \right) $$

(15)

After simplifying, we get

$$ \theta_{3} = a\tan 2\left( { \pm \sqrt {1 - (\frac{D}{\rho })^{2} } ,\frac{D}{\rho }} \right) - a\tan 2\left( {d_{4} ,a_{3} } \right) $$

(16)

with

$$ D = n_{x}^{2} + n_{y}^{2} - \left( {a_{2}^{2} + a_{3}^{2} + d_{4}^{2} } \right) $$

(17)

$$ \rho = 2\sqrt {(a_{2} a_{3} )^{2} (a_{2} d_{4} )^{2} } $$

(18)

$$ n_{x} = \frac{{P_{cx} }}{{cos\left( {\theta_{1} } \right)}} $$

(19)

$$ n_{y} = P_{cz} - d_{1} $$

(20)

Continuing to multiply the inverse matrix, we have

$$ (^{1} T_{2} )^{ - 1} (^{0} T_{1} )^{ - 1} \times \left[ {\begin{array}{*{20}l} {r_{11} } \hfill & {r_{12} } \hfill & {r_{13} } \hfill & {p_{x} } \hfill \\ {r_{21} } \hfill & {r_{22} } \hfill & {r_{23} } \hfill & {p_{y} } \hfill \\ {r_{31} } \hfill & {r_{32} } \hfill & {r_{33} } \hfill & {p_{z} } \hfill \\ 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] = (^{2} T_{3} )(^{3} T_{4} ) $$

(21)

We have the equation

$$ d_{4} \cos \left( {\theta_{3} } \right) + a_{3} \sin \left( {\theta_{3} } \right) = \cos \left( {\theta_{2} } \right)\left( {d_{1} - P_{cz} } \right) + P_{cy} \sin \left( {\theta_{1} } \right) + \sin \left( {\theta_{2} } \right)P_{cx} \cos \left( {\theta_{1} } \right) $$

After simplifying, we get

$$ \theta_{2} = \varphi - a\tan 2\left( { \pm \sqrt {1 - (\frac{M}{\sigma })^{2} } ,\frac{M}{\sigma }} \right) $$

(22)

with

$$ M = d_{4} \cos \left( {\theta_{3} } \right) + a_{3} \sin \left( {\theta_{3} } \right) $$

(23)

$$ \sigma = \sqrt {p_{x}^{2} + p_{y}^{2} } $$

(24)

$$ \varphi = a\tan 2\left( {p_{y} ,p_{x} } \right) $$

(25)

$$ p_{x} = P_{cx} \cos \left( {\theta_{1} } \right) + P_{cy} \sin \left( {\theta_{1} } \right) $$

(26)

$$ p_{y} = d_{1} - P_{cz} $$

(27)

For joint 4 to joint 6: We can calculate the transformation matrix from joint 4 to joint 6

$$ { }^{3} T_{6} =^{3} T_{4}^{4} T_{5}^{5} T_{6} = \left[ {\begin{array}{*{20}l} {c_{4} c_{5} c_{6} - s_{4} s_{6} } \hfill & { - c_{4} c_{5} c_{6} - s_{4} c_{6} } \hfill & {c_{4} s_{5} } \hfill & {c_{4} s_{5} d_{6} } \hfill \\ {s_{4} s_{5} c_{6} + c_{4} s_{6} } \hfill & { - s_{4} s_{5} s_{6} + c_{4} c_{6} } \hfill & {s_{4} s_{5} } \hfill & {s_{4} s_{5} d_{6} } \hfill \\ { - s_{5} c_{6} } \hfill & {s_{5} s_{6} } \hfill & {c_{5} } \hfill & {c_{5} d_{6} } \hfill \\ 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] $$

We get rotated matrix

$$ { }^{3} R_{6} = \left[ {\begin{array}{*{20}l} {c_{4} c_{5} c_{6} - s_{4} s_{6} } \hfill & { - c_{4} c_{5} c_{6} - s_{4} c_{6} } \hfill & {c_{4} s_{5} } \hfill \\ {s_{4} s_{5} c_{6} + c_{4} s_{6} } \hfill & { - s_{4} s_{5} s_{6} + c_{4} c_{6} } \hfill & {s_{4} s_{5} } \hfill \\ { - s_{5} c_{6} } \hfill & {s_{5} s_{6} } \hfill & {c_{5} } \hfill \\ \end{array} } \right] $$

(28)

but

$$ { }^{0} R_{6} =^{0} R_{3}^{3} R_{6} $$

(29)

or

$$ { }^{3} R_{6} = (^{0} R_{3} )^{T} (^{0} R_{6} ) = \left[ {\begin{array}{*{20}l} {r_{11} } \hfill & {r_{12} } \hfill & {r_{13} } \hfill \\ {r_{21} } \hfill & {r_{22} } \hfill & {r_{23} } \hfill \\ {r_{31} } \hfill & {r_{32} } \hfill & {r_{33} } \hfill \\ \end{array} } \right] $$

(30)

From matrixes 18 and 20, we have:

$$ \cos \left( {\theta_{5} } \right) = r_{33} $$

(31)

$$ \sin \left( {\theta_{5} } \right) = \pm \sqrt {1 - r_{33}^{2} } $$

(32)

$$ = > \theta_{5} = a\tan 2\left( { \pm \sqrt {1 - r_{33}^{2} } ,r_{33} } \right) $$

(33)

$$ \frac{{\sin \left( {\theta_{5} } \right)\sin \left( {\theta_{6} } \right)}}{{ - \sin \left( {\theta_{5} } \right)\cos \left( {\theta_{6} } \right)}} = \frac{{r_{32} }}{{r_{31} }} = > \theta_{6} = a\tan \left( {\frac{{ - r_{32} }}{{r_{31} }}} \right) $$

(34)

$$ \frac{{\sin \left( {\theta_{5} } \right)\sin \left( {\theta_{4} } \right)}}{{\sin \left( {\theta_{5} } \right)\cos \left( {\theta_{4} } \right)}} = \frac{{r_{23} }}{{r_{13} }} = > \theta_{4} = a\tan \left( {\frac{{r_{23} }}{{r_{13} }}} \right) $$

(35)

From Eqs. (14), (16), (22), (33), (34), (35), we have obtained the formula to calculate the joint angle values to complete the inverse kinematics problem for the ABB IRB 120 robot. From here, we will build a robot controller based on this algorithm.