Specific Language for Robot Trajectory Generation

In this paper, a programming language for describing trajectories of the Mover 4 educational robot is discussed. The goal is to overcome the limitations of the programming tools provided by the manufacturer. Object-oriented structures of trajectories in the joint space and three-dimensional space are formulated. The model of the trajectory in the joint space is represented by the value of the joint, its velocity and acceleration, and the inertial tensor of the configuration from the respective joint to the end-effector. The inertia tensor is necessary to calculate joint forces and moments. A point from the trajectory in three-dimensional space is defined by the Cartesian coordinates of the end-effector, its orientation with the Euler angles and its velocity. Language offers spatial primitives to describe trajectories formed by segments, circle arcs, and cubic splines. Each primitive has a method of generating intermediate points. The language will allow the study of kinematic and dynamic capabilities in tracking trajectories. Keywords— kinematics, robotics, robot language, robot path, simulation


I. INTRODUCTION
The Robot Mover4, Commonplace Robotics GmbH, is a four joint robot for use in education, entertainment and research environments [1].The accompanying CPRog software allows to control the robot.CPRog offers two editors to program the movement of the robot.The first is a graphical editor 'GraphEdit'.It is a puzzle-oriented programming language.It is addressed at users who have no basic knowledge of programming languages and programming.The second option is 'TextEdit'.This is a tabular programming language.Regardless which of the two programming tools is selected, the options for motion generation are equivalent.The advantage of TextEdit is that it allows the creation of larger programs than GraphEdit.The programs are saved on an HDD in an XML file.With the language the following operators can be set: ✓ Straight line motion from the current position to the target point.The end-effector velocity is defined in mm/sec.✓ Interpolation of the axes from the current position to the target position in joint coordinates.Velocity is defined as percentage of the maximum joint rotational velocity.
These options are too poor because they restrict the robotics training process -it is not possible to demonstrate fully kinematic and dynamic robot control.Control functions can be enhanced by creating a language for describing the robot trajectories.Many programming languages are known -specifically designed for this purpose or as an upgrade of an existing procedural language [2], [3].The use of a universal programming language and its upgrading with appropriate data structures and methods is the easiest way, as there is no need to create a language compiler.
The purpose of this work is to formulate a language for describing a limited class of trajectory paths on a MOVER 4 robot.The object-oriented graphics upgrade of Borland Delphi will be used [4], and the graphics primitives expanded with data structures specific of the robot trajectory.The generated motion program will be interpreted in an XML file to be read and executed by the robot.Such language will allow the study of kinematic and dynamic capabilities in tracking trajectories.

A. Kinematic Model of Robot
The robot manipulation system (MS) consists of successively connected kinematic pairs which form an open kinematics chain (OKC) with a first link fixed to a selected coordinate system and the last link, end-effector (EE) that performs the desired target movements.Kinematic pairs are the independent parameters that uniquely define the position of MS in space.They are called generalized coordinates and define the vector: q is called configuration of the MS.The vector of the generalized velocities is: The generalized accelerations are:


(3) The change of generalized coordinates q in accordance with the constructive limits determines the range of permissible configurations: Equations ( 1) -( 4) define the kinematic model of the robot.The kinematic model is necessary to solve the inverse kinematics and to control the movement of the manipulation system to perform the technological operation.The coordinates M of the end-effector characteristic point are given by the equation: The inverse kinematics problem consists of determining the generalized coordinates qG where the end-effector will reach goal coordinates G in D: In general, the inverse kinematics has no unique solution.For MOVER 4, the solution is implemented in the accompanying CProg software and therefore will not be the subject of the current work.

B. Trajectory Model in Joint Space
The instantaneous configuration of the MS is determined by the following data: -Values q of joint coordinates -determine the Cartesian position of the EE in D -Eq.( 5); -Joint velocities and accelerations ) , ( q q    -determine the velocity and acceleration of the EE.They are necessary element in the planning and execution of technological operations. -Inertial tensor -needed to calculate joint forces and moments [5], [6].For the current configuration, a class is defined with data for the four joints of MOVER 4: This structure defines a limitation for modeling OKC up to 4 degrees of freedom.It can be overcome by increasing the size of the arrays used or by applying dynamic structures to represent the kinematic chain [7], [8].Consecutive configurations representing the trajectory of motion in joint coordinates are organized into a dynamic list ConfigurationList.When generating a robot motion control program, the configuration list is created either off-line or during on-line task execution.In both cases, EE should follow strictly defined points in the space in order to realize the purpose of the movement.Therefore, it is necessary to formulate tools for describing trajectories in the threedimensional space (3D).

C. Trajectory Model in Cartesian Space
Trajectory Г in the workspace D, consisting of r base points numbered in sequential order is: where: Г0 is a starting point; Г1, Г2,...Гr-1 are intermediate points; Гr -target endpoint.Each point Гi of the trajectory is described by the vector: where: is the orientation in Euler angles of the EE; vi -the velocity in the i-th trajectory point.
The new class that define the trajectory points is descendants of geometric classes of object oriented twodimensional graphics Pascal_2D [4].Base point Гi of the trajectory is defined as the successor of the KPoint class: The Cartesian coordinates of the point are inherited element of the ancestor class KPoint.The class TPathPoint is complemented by the Euler angles and joint velocities.A pointer to the next element allows the creation of the dynamic path list.GET-methods, according to the requirements of the object-oriented programming realize the class interface, providing data encapsulation.They ensure access to the parameters of the concrete object in the program.SET methods give a possibility to modify the properties of generated point exemplars.

D. Cartesian Trajectory Primitives
The sequence (8) can contain a significant number of points.If each one of them must be explicitly entered, this would create problems for the operator.A possible approach is the choice of several trajectory primitives, each of which is defined by the required minimum number of base points.With appropriate interpolation, intermediate points can be generated.Complex trajectories can be created by combining primitives.
The formulation of the classes is determined by the number of points defining a segment of the trajectory.

1)
Two base points (ГS, ГЕ): They define a straightline segment in 3D.The new class is the successor to TPathPoint : Where r is the number of intermediate intervals.

2)
Three base points (ГS, ГМ, ГЕ): They define a circle in 3D.The generated trajectory starts from the first point and ends with the third set point.The class definition is: The task of generating circle points lying in a plane xOy is trivial.If these points are in three-dimensional space, the problem becomes much more complex.In literature, the task of circular interpolation is encountered in CNC programming [9], [10].One of the possible ways is to use the Newton-Raphson method to solve the nonlinear equations describing the trajectory [11].Other algorithms are based on the assumption that the rotation axis is known for the three base points using the Rodrigues' rotation formula [12].In the present work, an algorithm based on the matrix data presentation and using the already developed matrix operations in the CINDY system will be proposed [8].The basic idea of the algorithm is to find spatial transformations with which the three points are translated in the xOy plane, and the center of the circle they define coincides with the origin.So intermediate points can be generated, which in the plane is trivial.By applying the inverse transformations the spatial coordinates of the points from the desired trajectory will be obtained.
The algorithm for generating the intermediate points consists of the next steps.
Step 1. Calculation the normal vector to the plane μ, determined by points (pS,pM,pE) -Figure 1:  The angle α is defined by the trigonometric functions: The transformation matrix RX describing the rotation is: The substitution of trigonometric functions with their equivalent expressions from (13) gives: After this rotation, the normal vector will lie in the xOz plane and its coordinates are   Step 4. With ТXY -translation the center is placed in the coordinate origin: Step 5. Thus, after the transformations the coordinates of the point p(x,y,z) in 3D will be obtained in the xOy plane and the new coordinates p * (x * ,y * ,0) are calculated with: Where : Step 6.The start point and the end point define the angle of rotation φ around the origin: In a selected number of r intermediate points, the sampling angle is r /

  
, and the transformation matrix is: Step 7. The coordinates of the intermediate points in the space will be obtained by the inverse transformation: r 1,2,..., i , Where: Due to the uniform curvature of the circular trajectory, it is appropriate to apply a proportional law to set the velocity and orientation of EE and Eq.10 is applied.
The language will be used to explore the kinematics and dynamics of a MOVER 4 robot following different paths.
in Euler angles of the end-effector.Equation (5) defines the forward kinematics of the robot.The solution of forward kinematics for the continuous set Q0 defines workspace D of the EE:

Fig. 1 .
Fig. 1.Normal vector to the plane μ(pS,pM,pE) Step 2. Two rotations are determined using the normal vector  n  .The first rotation is around axis Ox at angle α to parallelism of μ and Oy (Figure 2).The projection d of the normal vector  n  on a plane yOz is:

Fig. 2 .
Fig. 2. Rotations necessary to coincide the vector  n  with the Z axis

R
is about the axis Oy at the angle β:We again replace trigonometric functions with their equivalent metric expressions from (15): transformations, plane μ becomes parallel with xOy.As shown, the values of the trigonometric functions are not calculated, but the projections of the normal vector are used, which leads to the acceleration of the computational algorithm.The projections complex form of the three base points on xOy are already known.Step 3. The center C(cx,cy,0) of the circle passing through them is calculated by the formula[13]: complexconjugated forms of the coordinates.

Fig. 6 .
Fig.6.Robot program in XML format Fig. 7 shows the simulation of the movement of MOVER 4 with program CINDY, tracking part of the trajectory.CINDY allows visualizing the graphs of change of the kinematic parameters of MS both in Cartesian coordinates and in the space of the generalized coordinates.On Fig. 8 are the graphs of the change of the coordinate of the end-effector respectively in X, Y and Z axis.

Fig. 9
Fig. 9 represents the variation of the velocity of EE decomposed on the coordinate axes.

TABLE I .
BASE POINTS FOR SPLINE GENERATION