Design and Implementation of a Graphic Simulator for Calculating the Inverse Kinematics of a Redundant Planar Manipulator Robot

In this paper, a graphics simulator that allows for characterizing the kinematic and dynamic behavior of redundant planar manipulator robots is presented. This graphics simulator is implemented using the Solidworks software and the SimMechanics Toolbox of MATLAB/Simulink. To calculate the inverse kinematics of this type of robot, an algorithm based on the probabilistic method called Simulated Annealing is proposed. By means of this method, it is possible to obtain, among many possibilities, the best solution for inverse kinematics. Without losing generality, the performance of metaheuristic algorithm is tested in a 6-DoF (Degrees of Freedom) virtual robot. The Cartesian coordinates (x,y) of the end effector of the robot under study can be accessed through a graphic interface, thereby automatically calculating its inverse kinematics, and yielding the solution set with the position adopted by each joint for each coordinate entered. Dynamic equations are obtained from the Lagrange–Euler formulation. To generate the joint trajectories, an interpolation method with a third order polynomial is used. The effectiveness of the developed methodologies is verified through computational simulations of a virtual robot.


Introduction
Nowadays, the use of manipulator robots is widely extended in the industry because these robots carry out tasks that pose risks to humans, improve the quality of the products manufactured, and increase productivity. Nevertheless, robotics is not limited to the industry, but has also expanded to other fields such as medicine (robotic implants or remote surgical intervention systems), agriculture (sheep shearing or pork cutting), space (planetary surface exploration), and geological and military research, among others [1][2][3].
Manipulator robots can be classified into serial and parallel based on their structure. The links of a serial manipulator are normally assembled together by means of rotational or prismatic joints that form an open kinematic chain whose structure is similar to a human arm. This configuration allows for assembling a clamp or any other tools that can be moved to any position within the robot's workspace [4].
The industrial robots available in the global markets are not redundant, but they do have the degrees of freedom necessary for performing the tasks for which they were designed [5]. However, if a failure occurs in the actuator of an industrial robot, without a safety backup, its respective joint blocks, and the robot losses the mobility of that joint. This may lead to situations that imply risks to the operators, difficulties for the users, interruption of the mining or industrial process, and economic losses, among other consequences [6]. Therefore, interest has arisen in the study of the behavior of consumed during the iteration process. In this regard, Zhou et al. [26] propose a smart algorithm based on extreme machine learning (ELM) and sequential mutation genetic algorithm (SGA) to improve the efficiency of the Standford MT-ARM manipulator robot of 6-DoF. Nevertheless, this last study sacrificed the accuracy of the preliminary solution of inverse kinematics.
Currently, several researchers have implemented simulation systems them in robots [24,26,27] because they allow for manipulating and testing the control system of automated robot within a virtual environment. For example, Kereluk and Emani [28] present an integrated design and simulation environment that offers control over the hardware of a reconfigurable manipulator robot with 18-DoF denominated MARS. In this environment, the user can select or design the configuration of the manipulator robot, simulate its operation in a virtual reality environment and operate the physical manipulator robot. The simulation results have proved to be very similar to experimental results. López et al. [24] developed a simulator on MATLAB software to test the performance of different meta-heuristic algorithms for solving the inverse kinematics of manipulator robots. Zhou et al. [26] also used it to analyze, specifically, time efficiency during the training stage of NNs and ELM, as well as for the optimization and computational process.
The contribution of this study is the development (design and implementation) of a graphic simulator using the Solidworks software and the SimMechanics Toolbox from MATLAB/Simulink. This program is commanded from a graphic interface that is created with the MATLAB tool GUIDE. GUIDE allows for characterizing kinematic and dynamic behavior; additionally, tests can be conducted in the control system of a planar manipulator robot with 6-DoF within a virtual environment. An innovative aspect of this work is that the special configuration of the 6-DoF manipulator robot, which includes rotational and prismatic (linear) joints. Most recent research employs robot structures with rotational joints and omit linear ones. Exploring new types of configurations allow for improving the skills of the robot, broadening its workspace, increasing its accuracy, or improving its energy consumption [28]. The SA algorithm will be applied to the manipulator robot without adding any other meta-heuristic algorithm. Additionally, before using SA, an algorithm to help search for the initial solution (end effector initial position) will be implemented to enable the end effector to be as close as possible to the set point and will contribute to a faster convergence of the initial solution.

Description of the 6-Dof Redundant Planar Manipulator Robot
This work considers a novel 6-DoF redundant planar manipulator robot with three rotary (R) and three prismatic (P) joints arranged in a RPRPRP configuration. If we draw an analogy with a human arm, this robot is composed of three parts: an arm, a first forearm and a second forearm. Each part is made up of two links and two types of joints (rotary and prismatic).
This redundant planar manipulator robot constitutes an open kinematic chain that originates from the assembling of the rotary joint, specifically between its fixed base and the arm; the arm and the first forearm, and, finally, the first and second forearm. Additionally, the prismatic joints connect the links as follows: first and secondary link, third and fourth link, and fifth and sixth link.

Kinematic Analysis
This section is concerned with the development of the mathematical relationships that analytically describe the spatial motion of the redundant planar manipulator robot under study, without considering the causes of the same. The equations relate the position and orientation of the end part of the robot with the coordinates of the robot joints. This is carried out using a reference system arbitrarily chosen, but that is usually placed on the base of the redundant planar manipulator robot.

Forward Kinematics of the 6-Dof Redundant Planar Manipulator Robot
The D-H method is used to characterize the kinematic behavior of the redundant planar manipulator robot. Taking the axes assigned in the geometrical configuration shown in Figure 1, it is possible to obtain the robot joint parameters, which are presented in Table 1.  The homogeneous transformation matrix of Equation (1) is completely defined based on joint parameters: where: i = 1, . . . ., 6. According to the D-H convention, the product of each transformation matrix that link the coordinate system 0 i−1 is calculated with the immediately after 0 i , and the 4x4 T matrix is obtained, which represents the pose (position and orientation) of the last link from the base system.

Direct Dynamics of the 6-Dof Redundant Planar Manipulator Robot
To obtain the dynamic equations of the 6-DoF redundant planar manipulator robot, the L-E Method is applied to the robot shown in Figure 2. Taking into account the prismatic and rotational motions, the generalized coordinate vector is where: l ci is the length of the mass center of the link i, i = 1, 2, 3, 4, 5, 6. The dynamic model of the manipulator of 6-DoF, composed of fixed links interconnected by means of joints, can be described in its compact form (matrix), as shown in Equation (5) [29]: where: τ is the (6 × 1) vector of the generalized forces applied by the actuators on the joints; τ 0 is the (6x1) vector of the input unknown disturbances; M is the (6×6) inertia matrix; C is the (6 × 6) centrifugal and Coriolis forces matrix; G is the (6 × 1) gravitational forces vector; F is the (6 × 1) vector of the frictions, and q,q,q are the vectors of the joint displacement/angle, speed and acceleration joint vectors, respectively. Since the manipulator works in a horizontal plane, then G(q) is zero (gravity acts perpendicular to the motion plane). Vectors τ 0 and F are not considered in the construction of the model. The elements of the matrix M(q) are presented in Appendix A.
The centrifugal and Coriolis forces matrix C(q,q) can be obtained through the Christoffel coefficients C ijk (q) defined in Equation (6) [30]: where: M ij (q) denotes the ij-th element of the inertia matrix M(q). Thus, the kj-th element of the C(q,q) matrix can be obtained as: Then, the resulting centrifugal and Coriolis forces vector is: c(q,q) = C(q,q)q Using Equations (6) and (7), the centrifugal and Coriolis forces vector c(q,q) shown in Equation (8) is obtained. The components of this vector are presented in Appendix A.

Simulated Annealing Algorithm
These days, several methods exist to solve the inverse kinematics of the manipulator robots, but, unfortunately, some of them have long calculation times and only provide one restricted solution [15,31]. In this section, a new method is presented for calculating the inverse kinematics of (standard and redundant) manipulator robots of any type and configuration, based on the probabilistic technique denominated Simulated Annealing (SA).

Description of the Annealing Process
The physical process called annealing is a technique that consists in heating and then slowly cooling a material to alter its physical and chemical properties. The heat allows atoms to increase their energy, thus making them capable of moving from their initial positions (from a local minimum value of energy to a global one). This occurs because a slow cooling gives atoms higher probabilities of recrystallizing into configurations with lower energy than the initial one. Based on this idea, the SA algorithm was created, which allows conducting a meta-heuristic search for global optimization problems [23].

Application of the SA Algorithm to the 6-DoF Redundant Planar Manipulator Robot
The SA basic algorithm starts with an iteration number C, an initial state vector x i of six components (initial solution), that represents the initial inverse kinematics of the robot, and maintains a calculation of a parameter T, commonly called temperature, which is initially high and then is progressively reduced to a value T f close to zero during the algorithm execution [23]. The configuration of the state and temperature is typically a solution for the function to be optimized. This paper considers an initial temperature T =90, C = 450 iterations, and the cooling factor α =0.94.
The solution is perturbed in each iteration of the algorithm to produce a new neighbor solution x j from its neighborhood [32]. The first heuristic perturbation of the initial state x i occurs according to Equation (9).
where: n 1 and n 2 are the environment variables and c 1 is a random number evenly distributed over the (0,1) interval. Subsequently, it is verified that the value of each component of the x j vector calculated in Equation (9), which represents the joint position (current inverse kinematics), is within the mechanical limits of the redundant planar manipulator robot. Otherwise, a new calculation of the values for x j is made using Equation (10).
where: n 3 is an environment variable and c 2 is a random scale evenly distributed. This second disturbance of x i , governed by Equation (10) does not stop until each value of the joint variables of x j is within the allowed limits.
In addition, the quality of both solutions is assessed using a D target function, which is shown in Equation (11). Then, a new state x j could be eventually selected from the two solutions, as long as x j is better quality than x i .
where: d(x i ) is the performance of the initial state, and d(x j ) the performance of the current state, as shown in Figure 3. The calculation of each performance corresponds to the root mean square function shown in Equation (12): where: d k is a cost function, p(x i ) is the initial position of the end effector due to the initial joint configuration x i , p(x j ) is the current position of the redundant planar manipulator robot, x j is the current state vector of the joints position, p(x) is the set point, and x the target vector that contains the joint variables (inverse kinematics).
To assess the quality of the new solution x j , one acceptance criterion is assessed in each execution step of the SA algorithm. This criterion is associated with a change of energy E (in this case, E = D), where: if D ≤ 0 , the shift is immediately accepted, while if D > 0, the possibility of accepting the disturbing solution depends on the temperature and the solution is finally accepted, but with a probability of [33]: where: K is a random value ∈ (0,1). Once the shift is approved, the initial state vector x i is updated to x j (x i = x j ) and the algorithm described iterates 449 times (as a maximum) based on Equation (9) until finding a better x j . Once iterations end, the starting temperature T is reduced following Equation (15), and the procedure is repeated to search for a better x j until the temperature T is lower than the minimum temperature established, T f (T < T f ) [34]. The latter is the stop criterion of the SA algorithm.
where: α is a cooling factor, which determines the speed at which the system cools down [35]. Finally, considering the real dimensions of the redundant planar manipulator robot, it is established that the inverse kinematics solution be acceptable when the function fitness d j or the position error between p(x) and p(x j ) is below 1 mm. In that case, x j is accepted as the final inverse kinematic solution x, knowing that the maximum radius measured from the base to the end of the redundant manipulator robot is 600 mm.
Based on the difference between D performances and the minimization of the cost function, the set of inverse kinematic solutions for manipulator robots is found, as well as whether the current inverse kinematic solution generates values for the positions of the links), which allows manipulator robots to reach specified positions. The flowchart in Figure 4 summarizes the above described.

Graphic Simulator
This section addresses the development of the redundant planar manipulator robot graphic simulator. Such a simulator is created using the MATLAB/Simulink (including its toolbox SimMechanics TM ) and Solidworks software, which were chosen due to their 3D modelling and simulation of mechanical system features.

Simmechanics TM Model
Using the SimMechanics Link utility, the CAD assembly developed in SolidWorks is exported to the toolbox SimMechanics TM of MATLAB/Simulink. Then, the SimMechanics toolbox is used to complement and potentiate the basic simulator previously elaborated in SolidWorks. By means of the configuration of blocks representing the components of the redundant planar manipulator robot it is possible to make the robot perform tasks defined by the user, such as controlling its joints and position of its end, among others.
The configuration of some parameters of the blocks is set considering that motion takes place only in the XY plane, and the rest position of the redundant planar manipulator robot occurs when all the links are placed on the X axis, as seen in Figure 2. In home position, it is observed that when each prismatic joint is retracted, the arm and its two forearms are 160 mm long. However, the maximum extension of 200 mm is reached when each prismatic joint is stretched to their limit values, i.e., when each p-joint is extended up to 40 mm the maximum reach of the manipulator is 600 mm long. Figure 5 shows the circuit in a block diagram imported from the SolidWorks environment, which allows for the dynamic simulation of the redundant manipulator robot. This scheme represents the basic skeleton of the simulator, and was created based on the robot's real dimensions. The construction of the manipulator robot considered a structure based on aluminum material. Table 2 presents the main parameters extracted from the blocks modelling each link. These parameters characterize the redundant planar manipulator robot dynamics, with CG as the center of gravity (centroid) measured from the origin of the coordinate system of the respective link. Among the complementary blocks used in the development of the graphic simulator, those that model actuators and sensors are of great importance, since they generate and record, respectively, all the movements of the redundant planar manipulator robot. In the specific case of actuators, the necessary power signals come from the MATLAB/Simulink environment. Blocks from workspace are used to control each link. These blocks are in charge of reading and storing the diverse joint trajectories programed by the user. Figure 6 shows the block diagram and connections necessary for simulating and controlling the motion of the first link. Particularly, control is conducted over the first rotary joint of the redundant planar manipulator robot using a method for interpolation through polynomial. Similar connections and control strategies are used to simulate the movement of the remaining links.

Graphic Interface
Below is presented the virtual environment designed to input the parameter values in the blocks that represent the six links that compose the redundant planar manipulator robot. Figure 7 shows, as an example, the configuration window of all the links. Access to this GUIDE in MATLAB is granted through a main window that allows for opening other GUIDE sub-windows. The parameters of the link that can be modified are mass, inertia tensor, position of the center of gravity (CG), and the coordinates of the endpoints of the joints (CS1 and CS2). With this last adjustment, the starting position of each link can be defined and consequently the end effector's. If any box associated with the link is not configured, the manipulator robot will used the link parameters exhibited by the real manipulator robot in Table 2 by default. An interpolation method based on polynomial equations is used for controlling the joints of the redundant planar manipulator robot. This is achieved using the pchip instruction of the MATLAB, which allows for constructing third-order polynomials that do not cause overshoot during the movement of the joints, thus making it possible to route and move the joints from their initial positions to the desired positions with few oscillations. Figure 8 shows the GUIDE used to program the following joint trajectories. This GUIDE allows for entering desired values for the prismatic and rotary joints, indicating also the time of execution (TI), end (TF) and rest (TR) of the movement. The programming limit is five movements per joint. Once the boxes have been filled in using the button "CHARGE", the values entered are automatically stored in .mat files and in the "from workspace" blocks. Additionally, the trajectories of the programmed joint movements can be visualized through the button "PLOT", as shown in Figure 9. The main window of GUIDE also makes it possible to see all the curves resulting from the dynamic performance of the 6-DoF redundant planar manipulator robot, i.e., position, speed, and acceleration of each joint. Specifically, Figure 10 presents the position, speed, and acceleration of the first rotary joint.  Figure 11 shows the development of a GUIDE window, which allows for solving the inverse kinematic problem of the redundant planar manipulator robot based on a SA algorithm. Considering the desired location of the end of the redundant planar manipulator robot, it is possible to know the set of values of the joint variables necessary for reaching this location. The joint variables are six and represent the physical location adopted by the three rotary joints and the three prismatic joints. In addition, the position variables are two, because the end of the robot moves only on the XY plane. When the coordinates (x, y) are input in the GUIDE window, the workspace allowed to the robot, shown in Figure 12, should be considered, since it depends directly on the mechanical limitations of the same, which are exhibited in Table 3. The base of the redundant planar manipulator robot is located in the (x, y) = (0, 0) coordinate, and its workspace is presented highlighted in blue, with its maximum and minimum scopes being 600 and 160 mm, accordingly, as shown in Figure 12.  Table 3. Mechanical limitations of the 6-DoF redundant manipulator planar robot joints.

Joint
Range By means of the graphic interface, it is possible to enter the coordinates (x, y) of the end effector of the redundant planar manipulator robot; after which, the SA algorithm automatically calculates the inverse kinematics of the same. Figure 11 shows the menu to enter the position. In this example, the coordinate (350,150) yields the solution set with the position adopted by each joint for the entered coordinate. This result corresponds to an inverse kinematics solution selected from multiple options, after 450 iterations of the SA algorithm. Nevertheless, if the coordinates entered are outside its workspace, the SA algorithm automatically adjusts the end of the redundant planar manipulator robot to the closest position within the allowed region, and then delivers the solution set for inverse kinematics.
After continuous simulations for different set points, it is concluded that the SA algorithm requires on average 200 iterations to reach the desired position with an error of 1 mm or less. In addition, with the same number of iterations, the average convergence speed of the algorithm, to make the end effector of the redundant planar manipulator robot move from the initial position to the set point, is 0.5 (s). The scenario changes when the set point is within an unfeasible workspace; in this case, the algorithm needs 2 (s) to take the end effector to an allowed position. The convergence performance of the algorithm strongly depends on the selection of the environmental and cooling parameters defined in Section 6.

Conclusions
To characterize the kinematic and dynamic behavior of redundant planar manipulator robots, a graphic simulator was designed and implemented, which allows for entering the Cartesian coordinates (x, y) of the end effector of these robots through a graphic interface, with the purpose of automatically obtaining its inverse kinematics. The basic structure of this basic simulator was built using the software Solidworks, which was subsequently potentiated through the SimMechanics tools of MATLAB/Simulink.
An algorithm based on the probabilistic technique called Simulated Annealing was used for the automatic calculation of the optimal solution, among several options, of the inverse kinematics of the 6-DoF redundant planar manipulator robot. By means of the SA algorithm, it was possible to minimize significantly the distance between any initial position and a target position of the robot's end effector, reaching values close to zero. In this way, the enhanced algorithm is able to distinguish and correct joints out of the mechanical limits or, also, out of the workspace. In all tests for different targets, the optimization criteria outlined was satisfied (fitness function below or equal 1 mm) and the inverse kinematic solution of the manipulator robot was obtained in a time shorter than one second. This technique can be easily reproduced in other type of manipulator robot configurations with more (or less) redundancy, given that their main component is the information about the direct kinematics, which is easy to obtain in most cases.