Generation and Control of Basic Geometric Trajectories for a Robot Manipulator Using CompactRIO D

. The utility of a robot manipulator focuses on the ability to locate its end effector in a position with a determined orientation following a specified trajectory. For this, algorithms were used in order to generate and control the movements joints of robot in a synchronized way. The high-level languages to program robots are based on three types of movement: joint interpolation (MOVEJ), linear interpolation (MOVES), and circular arcs (MOVEC), which are used to develop any type of task. In this work, these three movements are implemented in the industrial controller CompactRIO, as part of the reconditioning process of a robot manipulator of five degrees of freedom (5 DOF) whose controller was obsolete. As a result, it will have an interface in LabVIEW where you can view and modify the basic parameters implemented in the industrial controller. In addition, the results of the validation tests of the joint positions and the end effector of the manipulator will be found.


Introduction
The robot manipulators have been constituted as an essential part of modern industries.Some of the reasons are the low production cost, the optimization in the processes, their flexibility, and the adaptation to several productive situations.Additionally, the robot manipulator with only small modification in the programming allows industries to adopt this technology in their operations instead of company staff.Generally, the algorithms designed for the monitoring and control of trajectories must be accompanied by the mathematical support that guarantees the good performance of the scheduled tasks.There are techniques with different approaches to control robot manipulator movements, such as neural networks [1], uncoupled control [2], and game theory [3].However, some control units that currently accompany these manipulators and incorporate these techniques, due to the passage of time, have presented faults or are very outdated, which limits their use.
In this article, we intend to endow a robot manipulator of five degrees of freedom (5 DOF) a new industrial controller (CompactRIO) (Figure 1), which allows designing a modern interface with open control algorithms.This new control unit offers the possibility of using a generic or different industrial controller to factory design for robot manipulators, which are functional, reliable, and user friendly.
In this new control unit, three types of path-generating algorithms are implemented, which provide the positions that a robot manipulator must follow to execute a specific task [4,5].The first algorithm develops a joint path (MOVEJ), in which the end effector does not have any relevance.The other two will generate Cartesian trajectories that will draw with the end effector the shape of a line or an arc (MOVES and MOVEC).The programming will be done through LabVIEW and will be implemented in the same control unit.It will interact with the user through a computer connected remotely to the Ethernet port, which will make it easier to visualize, create, and modify the tasks that the user would like the manipulator to replicate.The developed interface will contain the reading of the commands, the mathematical modeling found, the algorithms for generating trajectories, the visualization of variables, and the position control (PID) for motors [6].

Hardware Description
2.1.Robot Manipulator.The Scorbot manipulator (Figure 2) has an anthropomorphic configuration, composed of a mechanical structure with five joints, DC motors of 12 Volts, rotary encoders coupled to the axes of the motors, and five microswitches to determine the position of home.
The anthropomorphic design provides a broad region of end effector operation [8,9].It has a rotation at the base of 310 ∘ , shoulder rotation of +130 ∘ /−35 ∘ , rotation of the elbow of ±130 ∘ , tilting of the clamp of ±130 ∘ , and rotation of the mechanically unlimited clamp but originally restricted to ±570 ∘ electrically (configurable software) [5].
The Scorbot-ER 4PC has several types of transmission to generate the movement in the joints of the robot; these transmissions are composed of gears with different configurations of trains, pulley, and toothed belts.It also has a spindle transmission in charge of opening and closing the clamp.a reconfigurable FPGA chassis capable of executing highspeed processes, which is interconnected with the LabVIEW (Laboratory Virtual Instrumentation Engineering Workbench) development platform (Figure 3) [10].

The Algorithms of Trajectories Implemented Description
Three movements (MOVEJ, MOVES, and MOVEC) will be dealt with, which will be implemented in the industrial controller CompactRio, which can be combined so that the robot can develop a specific task [11].
3.1.Joint Movement: MOVEJ.The use of this movement allows the manipulator to reach an end position, with independent joint movements.This does not ensure a linear trajectory between the initial and final points (Figure 4); that is to say the end effector moves from one point to another by a random path [12].The coordinates in the end effector workspace can be obtained through the Direct Kinematic Model (DKM) [13][14][15], using the joint values executed in the MOVEJ command.The pseudo-code can be seen in Algorithm 1.

Linear Motion: MOVES.
If it is desired to arrive at a Cartesian position following a trajectory in a straight line with the end effector, the command MOVES is used.This command requires a vector with six values; the first three are Cartesian variables (, , and ) of the desired position in the workspace of the origin of the coordinate system of the end effector.The following three refers to your orientation; this orientation is entered with the angles Roll(), Pitch(), and Yaw(), also known as angles RPY [16].
In this way, you will have a linear movement as Figure 5 [12], through the path generated from the current point or from start to end point (point entered with the command).
For this command, we will briefly discuss the two transformations that must be performed [17].The treatment is done differently, mainly because the space of translations is Euclidean, while the space of rotations is not.
Comparing the two movements, translation is the simplest of the two interpolations, since it is expected that a point   ∈ R 3 , denoted by three variables (  ,   , and   ) defined in the Cartesian space, is moved through a translation vector (Δ, Δ, and Δ).
The interpolation of orientation and rotation is done by the mathematical method SLERP (Spherical Linear Interpolation) (it is a unitary sphere of R 4 ) [17][18][19].For this, the orientation and rotation must first be represented in four dimensions or quaternions and then interpolated by the method SLERP.This method is to perform the movement at constant speed along a circle, on the surface of a hypersphere (Figure 6) [10], under the assumption that the positions of the trajectories of the end effector are given between the points of   and   .The effect is a uniform rotation with angular velocity around a fixed axis of rotation, which guarantees  a single route [18,20].Equation (1) [21] represents such movement.
where   is quaternion interpolated,   is first quaternion,  1 is second quaternion, Θ is the angle between the vectors   and  1 , and  is the spherical interpolation between vectors   and  1 , with values between 0 and 1.
With this method, implemented in Algorithm 2, any interpolation of quaternions can be drawn to make a smooth movement of the rotation of an object with complex rotations and the orientation of the same ones.Once the interpolated matrices are obtained, we proceed to change Cartesian coordinates to articular coordinates using the Inverse Kinematic Model (IKM) [22,23].
Algorithm 2 shows the description of the execution of the MOVES command whose main part of the implementation can be seen in Figure 7.

MOVEC: Circular
Movement.Three linearly independent points are indispensable to do this movement.The first is the value of , , and  of the Cartesian plane where the end effector is located, the second is the destination point, and the third belongs to the via point.That last one, in addition to helping in defining the path where the end effector should slide, defines the plane where it is going to work, as shown in Figure 8.
To perform the stroke, we define the orientation and direction of a vector associated with the systems  1 ,  1 , and  1 .The representation is made by a vector in a frame that rotates angle  around , an angle  around , and an angle  around  and moves the distances a, b, and c with respect to the axes , , and ; all this transformed to the coordinates of the frame of reference.On the frames  1 ,  1 , and  1 , the three linearly independent points define the plane that will contain the distribution of the points that make up the curve.The circumference in the plane is drawn from the perpendicular bisector of    V and the perpendicular bisector of     , where they will be cut into a single point, which is the center of the circumference, passing through   ,  V , and   , since the three are equidistant from it (Figure 9).From another point of view, it may be said that it is necessary to find a circumference circumscribed in the triangle formed by the three points, the center of this circumference is obtained through the intersection of the two-side bisectors of the triangle.The problem generates as a result a circumference that tends to infinity.
Similarly if you wish to complete the drawn arc to form a complete circle, it is necessary to generate another movement on the same plane, which has, as its starting point, the end point of the first and the end point, as the starting point of the first command.The diameter is defined by the distance between the start and end points and in turn must be equal to the distance between the two-way points.If this proportion is not met, it is also possible to obtain an ellipsoidal trace whose distance between its height and width can vary depending on the distances of the initial and final points with respect to the distances of the way points (or vice versa).The operations that they use to implement the MOVEC command are defined in Algorithm 3.

Scheme Implemented
The implementation of the algorithms of trajectories for the robot manipulator required the implementation of the graphical interface that incorporates the acquisition and control of variables supplied by the industrial controller, the input of parameters for the generation of the movements, and the algorithms generators of trajectories, among others.Next, a brief description will be given on the most relevant components that are part of the work done.

General Scheme of the Interface Implemented.
To integrate the mechanical system of the robot and the industrial controller CompactRIO, to the environment of LabVIEW, the programming and interconnection scheme of the figure was developed in Figure 10, which is programmed in the industrial controller [24].
The main VI of the system is completely designed and intended to interact with the user.To do this, it receives and displays the articulator positions of the manipulator, together with the Cartesian positions of the end effector.In addition, it sends the commands that are to be executed to the two blocks responsible for interpreting them (Commands.viand Home.vi).
The VI Home is just responsible for providing the articular positions to bring the manipulator to the initial configuration (Home).On the other hand, the VI Commands organizes and sends the other orders to the VIs in charge of executing each of the algorithms (MOVEJ, MOVES, and MOVEC), the desired speed with which to execute each robot movement (SPEED), the time delay between commands (DELAY), and the opening and closing of the fingers of the clamp (OPEN and CLOSE).
As the names indicate, the MOVEJ, MOVES, and MOVEC VIs, with the help of the DKM, IKM, and validate blocks, are in charge of creating the algorithms generating articular, linear, and circular trajectories, respectively.These three first blocks give the point to point the route that must be made to comply with the command, the acquisition block,  These six modules, five to control the motors and one for the calibration switches, are connected to the industrial controller CompactRio, which, in turn, communicates with the computer through a network connection that owns the chassis.

Acquisition and Control of Signals.
The purpose of this VI is to provide the program with variable reading and writing, such as the direction of rotation of the motors, number of pulses generated by the encoders increment, currents delivered to each motor, PWMs, stop, reset, and the activation of the push-buttons.The latter is obtained through the configuration of the NI-9403 module and the other variables are done through the NI-9505 modules.
To obtain the PWM signal with which the speed of each of the motors will be controlled, a block is created in a time frame, which is executed at a clock frequency of 40 Mhz (internal frequency of the CompactRio), as the picture shows in Figure 11.
In addition, the NI-9505 module delivers signals A and B of the encoders that are 90 degrees out of phase, so that the diagram in Figure 12 performs the counting of the pulses.Here, the spin direction is detected, based on the relationship phase between the signals, and then add or subtract them, according to the case.
With this count, a small operation is carried out to find the angle that the motor has turned.
The way the manipulator is constructed, with  3 and  4 , presents an angle different from that measured on the axis of the motors that move them.In other words, these angles  are affected by the previous angles; this indicates that, besides taking into account the angles of the motors, the angles that precede the one that needs to be found must be added or subtracted, in such a way that where  motor4 +  motor5 generate the wrist movement that moves the end effector up and down.Additionally, the difference of these angles ( motor4 −  motor5 ) produces the rotary movement of the tool; the latter is not affected by other angles, as is the case of  3 and  4 .
The implemented control scheme was carried out through a simple control, which is based on the movement of the joints independently or decoupled (Figure 13), which together with the path-generating algorithms coordinates the movements articular for Cartesian trajectories.
The first control block is in charge of following the reference position given by the algorithms generators of trajectories; the second control block allows reaching the changes of speed that must make the manipulator guarantee the correct articular and Cartesian positions in different instants of time.
The PID controllers are limited to generate a maximum and minimum PWM of ±2000, which corresponds to the input of the DC motor at ±12 volts.

Enter Commands.
The development of this VI allows entering the commands for the robot manipulator to perform  a task.There are three options: the first is to write directly the command to be used in the command console of the main screen of the program; the second way is to import to the command console the flat file where you have the commands to execute; the last one requires moving the manipulator from grade to grade at each joint until the desired position is reached, then adding the point with the desired operation to the command console (Figure 14).For correct use of the manual mode (option three), it is necessary to select the joint you want to move and then increase or decrease the angle with the plus (+) or minus (−) buttons.Once the robot is in the desired position, you can choose the operation to perform pressing the add button, which is added to the list.For MOVEJ and MOVES commands, only the end point needs to be stored, but for MOVEC it is necessary to store two points; one will be the end point and the other will be the way point.
The commands allowed and the correct way to enter them are mentioned in Table 1.
The angles  1 ,  2 ,  3 ,  4 ,  5 ,   ,   , and   are expressed in degrees, even though the execution of the program is done with radians, and the coordinates  are given in millimeters.

HOME Button.
This command has the function of calibrating the angles of each joint and bringing the robot manipulator to an initial or reference position (home).

SPEED Command.
This order provides the speed change with which the robot manipulator performs the movements, and the SPPED[] command is entered, where  is the integer value between 0 and 100 given in percent, where 100% supplies the maximum joint velocity of 0.218 [rad/s].

DELAY Command.
This command forces the robot to remain in its last position during the seconds that it is commanded.For that, the block time delay of LabVIEW is used.

OPEN and CLOSE Commands. With these commands,
the manipulator can open and close the clamp, which makes it possible to hold and release objects.

MOVEJ, MOVES, and MOVEC
Commands.These commands have the function of generating the interpolations so that the manipulator can trace desired trajectories, as previously exposed.

Workspace.
The effective working space is directly related to the length of the links, the mechanical limitations, and the configuration that the robot has.It can be found mathematically, through the equations describing the position of the end effector along with the mechanical limitations it possesses [25].
Physically, the workspace indicates that it is able to reach these points without exceeding the joint limits.This is essential to be able to propose the movements that are required to develop a task.
Due to the configuration that the Scorbot manipulator has (anthropomorphic), the movement described by the end effector is realized in the three-dimensional space.
The graphs corresponding to the workspace of the robot manipulator Scorbot-ER, with the new restrictions implemented by software in this work, are shown in Figures 15(a) and 15(b).In such graphs, some of the successions of points of the Cartesian coordinates found from the articular positions of the robot are presented.In this way, it gives an idea of the points, in the Cartesian system, that can be reached.For the manipulator to be controlled, most of the positions can be got that are within a radius less than or equal to 604 mm.

Performance of Joint
Movement.An analysis of the result of the kinematic model is performed, based on the MOVEJ command.For that, a known position and a desired joint position are used.For the test, the starting point is chosen as  0 = [0, 0, 0, −90, 0], and the desired joint position is designated,   = [90, −10, −45, 0, 0], as the point of arrival or end, where angles are given in degrees.With these values, an interpolation of the initial angles to the desired angles is generated (Figure 4).
Afterwards, a comparison of the data of the movement made by the manipulator and those obtained in the simulation is made.Figures 16(a), 16(b), 16(c), 16(d), and 16(e) perform such joint movements generated by the robot, where the red stroke represents the expected values and the blue stroke the articular path made by the manipulator.

Performance of Linear Motion in the Cartesian Space.
A straight stroke is the result of applying the MOVES command (Figure 5), as detailed during the development of this document.The example used to analyze the performance of the movement is performed with the initial coordinates, [−28.6,−351.04,64.24, 3.13, 0, −1], to the desired point of [280, −4, 305, 90, 0, −6.3],where distances are given in millimeters and angles in degrees.
The desired movement for the end effector is seen in

Figure 5 :Figure 6 :
Figure 5: Simulation of motion in a straight line.

Figure 9 :
Figure 9: Construction of the circle, using the perpendicular bisectors    V and     .

4. 2 .
Robot, CompactRio, and Computer Communication.NI 9503 FPGA modules will be responsible for the acquisition of the signals generated by the encoders, through the entrances: phase A+ and phase B+.In addition, it will be responsible for energizing the motors that move each joint, through the PWM that each module counts.The signals from the switches, used to calibrate the positions, are read in the inputs 1, 2, 3, 4, and 5 of the NI 9403 digital input module.

Figure 12 :
Figure 12: used to read signals from encoders.
Border of workspace outside area, top view Distance on the x-axis (mm) Distance on the z-axis (mm) Border of workspace, side view
Figures 17(a), 17(b), and 17(c), which describes the path taken in the coordinate axes ,  and , where the red stroke represents the expected stroke and the blue the actual behavior of the manipulator.5.4.Cartesian Movement Performance: Circular.Like the previous move, in this command, a planned figure is described 2  3  4  5 ]; (18) end (19) New position  = [ 1  2  3  4  5 ]; Algorithm 1: Algorithm for joint movement of MOVEJ.