HEMERO: A MATLAB-Simulink toolbox for robotics J. 1vin Maza and Anibal Ollero Systems Engineering and Automatic Dept., University of S ‘Camino de los Descubrimientos, ville, in, 41092 Seville (SPAIN) E-mail: (imaza, aollero) @caruja.us.es Abstract, This paper presents a new MATLAB-Simulink tool for rabotics. The tool can be used for ‘nanipulators and mobile robots. It consists of MATLAB functions and Simulink blocks defined to facilitate the design and simulation of robots including Kinematic and dynamic analysis, and control system design. ‘The tool ts distributed in CD-ROM with a new robotics textbook The examples of the book can be executed with the toolbox. A web site for the use of some functions of the toolbax will be soon available. The paper vimmarizes some general characteristics using as example a Puma 560 manipulator placed on a mobile plasform. 1, INTRODUCTION ‘There are several programs that can be used for the design, and kinematic and dynamic simulation of robotic systems. Some of them ate goneral programs for articulated mechanisms that can also be used for the design of robot manipulators and locomotion systems of mobile robots. There are also programs which emphasize the graphic interfaces leading to realistic simulations of the robot and its environment. In this paper a new MATLAB-Simulink tool for robotics, including manipulators and mobile robots, is presented. The tool is particularly suitable for robotic Courses on kinematics, dynamic and control of ‘manipulators and mobile robots. It is well known that MATLAB is an useful tool for kinematic and dynamic analysis of robots involving extensive matrix computations and symbolic ‘manipulations. The most well known MATLAB toolbox in robotics is probably the one presented in Corke [4]. However, MATLAB functions included in this toolbox ‘only deal with manipulators with a fixed base. Furthermore, only the computed torque method for control is included and the implementation of more sophisticated control strategies such as adaptive control ‘and learning control is relatively complex for the users On the other hand, some Simulink tools for the simulation of manipilator control methods. have been proposed (see for example Valera an others (13). “The HEMERO toolbox presented in this paper combines. fa set of MATLAB functions and Simulink blocks 10 facilitate Kinematic analysis, dynamic analysis, and control system design of manipulators and mobile robots, It provides a number of examples ranging from academic examples with two or three joints to more complex real manipulators such as the Puma 560. Furthermore, the most popular mobile robot locomotion platforms have been implemented, “The toolbox implements most of the methods included in Ollero {9}. The examples of this textbook can be easily executed using HEMERO, Furthermore, it is also possible to generate now examples with any manipulator ‘or mobile robots and simulate the several control strategies included in the toolbox. “The paper is organized as follows. In the next section goneral characteristics on the application of the toolbox fo the kinematic analysis of manipulators and mobile robots are presented, The third section is devoted to the dynamic models. In section 4 the control problem is considered. Section 5 summarizes the trajectory generation methods and section 6 is devoted 10 the ‘graphical interface. The conclusions are in section 7. 2. KINEMATICS HEMERO has a set of MATLAB functions for the kinematic definition of robot manipulators and for the ‘visualization of the manipulators and reference frames involved. The position and orientations can be defined using position vectors, rotation matrices, and transform Imattices. The position of a point in the reference frame {A} is given by the vector PenA=[x yz 1], which three first components are the coordinates of the points in (Al 43“The transform between reference frames is given by 4x4 matrices. Given a mateix T, its possible to verify if tis a transform matrix by means of the function ishomog (1). There is a function T=rotvec(v, theta) to obtain the transform resulting of the rotation of an angle theta with respect 10 a vector v,as well as functions to perform rolaions of an angle theta round the axis X, Fy Z respectively: rotx (theta), roty (theta), rotz (theta). In the same’ way, the funtion Tetranel (x,ys2) retusa transform matrix T, which represents the translation given by the thre selars x, 2 and the function T=transl(v) returns the translation by a vector v. A reference frame with a given position and orientation is represented by means of the function frano(TT,color,tamopt), where TT is the transform matix, color specifies the colo for graphic ragredentation, ta is the ois forthe arrows and opt tives the visibility ofthe reference frames, The inverse transform can be oblained by means of the funtion tri. If the 2-¥-Z Buler angles or the RPY angles are given, the corresponding transform matrix is defined by means of the functions T=eul2tr(alfa,beta, ganna), o T= Ppy2tr(r,p,y), respectively, MATLAB is used for the kinematic models of the robot ‘manipulators using the Craig [6] notation. The MATLAB. functions have as argument a matrix with the Denavit- Hartenberg {8} parameters. Each row / of this matrix has as elements the parameters o)_,,4,_1.8,d, of a joint, land another additional parameter o, indicating if the joint is rotational or prismatic. As an example, the following MATLAB lines define the matrix of a PUMA 560 robot: ‘dhpS60=[0 0 0 0 0; =pis2 0 0 0.2435 0; 0 0.4318 0 -0.0934 0; pi/2 -0.0203 0 0.4331 0; =pi/2 0 0 0 0; pi/2 0.0.0 0)} % Definition of the symbolic joint variables syns U1 t2 t8 t4 t6 t6 real; % Definition of the joint variables vector qe [ti t2 t9 t4 t8 16]; % Manipulator kinematic direct model T = simple( fkine(dhp560,q) ); ‘The function fkine obtains the symbolic expressions of the direct kinematic model. It is also possible to obtain the direct model by multiplying the transforms of each joint. These transforms can be obtained by means of the function Linktrans which first argument is now a vector with the Denavit-Hartenberg parameters of the joint and with the joint variable as the second argument. In the Puma 560 manipulator the following lines can be 44 used: Te=Linketrans (anps60( T2=Linktrans(anp560( Yoslinktrans (anps6o(: Té=Linktrans (dhp560( To=Linktrans (dhps60(: T6= Linktrans(anps60( Tet T2*TSATATS*TE, 11),t1); % Transtora 1-0 12);12)} % Transtora 2-1 13))t3)} 8 Transtora 3-2 14)\44)} © Transform 4-3 15);25)} & Transform 5-4 16),46)} & Transform 6-5 The MATLAB expressions are also used to implement the symbolic expressions of the inverse kinematic mode! and to represent the results. Alternatively, it is also possible to use the function ikine to solve numerically the inverse model. Thus, to solve the Puma 560 manipulator following straight line trajectory with constant orientation, the following MATLAB lines are used: [0.4:0.001:0.45); 0.1*ones(1,ength(x)) ; Yi % Definition of a trajectory x ye end % Threshold and number of iterations. iLimit=1000 qzikine(dhp560, stol, ilimit, 7G) ; ‘The result is a matrix q. The rows of this matrix are the values of the joint variables in each point of the trajectory. The representation of the arm following this trajectory can be obtained by means of the function: plotbot (dhp560,q, ‘fw') “The result for the last point of the trajectory is shown in Fig. 1. “The functions jacobn(ah,q) and jacobo(ah,q) can be used to obtain the Jacobian matrix with respect to (7) and {0} respectively, by means of the technique presented in Paul {10}. Thus, its possible to write: J=simple(jacobn(dh,q)) Alternatively, it is also possible to apply the velocity propagation method (Craig (6}) by means of the function velprop(dh,q,qd,v0,w0) which propagates the linear and rotational velocities from {0} to (1). The function J=tr2jac(T) creates a Jacobian matrix that cean be used to compute the velocity vector in a different frame "y = ",4v D Given differential motion vector[4.4 48,8, 8], the corresponding homogeneous transform can be obtained by means of the function dif#2te(0). Moreover, the function 0 = tr2dift (14,12), computes the difference between wo transforms and creates the corresponding homogeneous transform, Fig. 1 Representation of the reference frames of the Puma 560. The cone represents the manipulator bass. ‘Simulink is used for kinematic analysis of mobile robots. ‘Several Simulink blocks have been implemented for the direct and inverse models of different locomotion systems including synchro-drive, differential configuration, tricycle and bicycle models. Fig. 3 shows the inputs and outputs of some of these blocks. Bach block has the corresponding dialog box to provide the corresponding parameters. 3. DYNAMICS Both MATLAB functions and Simulink blocks are used for the dynamic analysis. The former are applied for the simulation and design of the control system as will be shown in the next section. ‘The matrix dyn has the kinematic and dynamic parameters of the manipulator. This matrix has one row for each joint and 20 columns. The first five columns are the same than in the Denavit-Hartenberg matrix, The following column is for the masses. Columns 7-9 define the position of the centers of mass of the links with respect to the reference frame of the joint. Columns 10- 15 contain the elements of the tensors of inertia with respect to the center of mass of the joints. Column 16 corresponds to the mass moments of inertia in the axis of the joints. Column 17 provides the reductions in the gears of the joint axis. Columa 18 contains the viscous friction coefficients, and columns 19 and 20 have the Coulomb friction coefficients (positive and negative rotations). home (einai Fig 2. Simulink blocks for some direct and inverse kinematic of ‘mobile robots The tool can be used to compute all the terms of the dynamic model: = Maa" + G(q) + Veq.a) + F(a) o where M(q) is the mass matrix, G(g) is the vector of ‘gravity terms, (4,4) is the vector of centrifugal and Coriolis terms, and F(q’) is the veotor with all the friction terms. The function inertia(dyn,q) computes the mass matrix, and the functions gravity (dyn,q), coriolis(dyn,q) and friction(dyn,q) obtain the torques of the corresponding vectors. ‘The function rne(dyn,a,qd,qdd) computes the full dynamic model by means of the iterative Newton-Euler dynamics algorithm, “The function fayn integrates the expression (1) by means of the MATLAB function ode45 returning a time vector and two matrices q and qd coresponding to the joint position and velocities. Alternatively, it is possible to integrate the dynamic model by using Simulink. Thus, the blocks corresponding, to the terms of the model (1) have been implemented as Simulink blocks. These blocks are illustrated in Fig. 3. ‘The last block in this figure can be used to design Simulink simulation diagrams. Each block has the ‘corresponding dialog box. ‘The robot manipulator can be placed on a autonomous mobile platform with a kinematic model as mentioned in asthe previous section, Then, the joint variables q and its derivatives q’ and q”, as well as the rotational velocity w and acceleration «' and the linear acceleration v' of the mobile platform, Konno Tegnaim i fon or evarrenoenop ie PE 218 48 aa mean paar fi © Fig. 3. Simulink blocks for the simulation of robot manipulators, ‘The Simulink diagram in Fig. 4 can be applied to simulate the behavior of the robot arm, Fig, 5. Torque applied to the joints of the Puma 560 ‘manipulator. 2 Cada ayaa cond mode de bt at) 0 0121503 9 bu-9000% 67.0800 ‘ects = Vetta: tinder ooo a (eet | _ H Fig. 6. Dialog box for the “Trayectorias articulares” _ Biot fia [ies Siva Wave - 7 ‘ Fig, 4. Simatink diagram to obtain the joint trajectories of the Puma 560. Assume that the torque a shown in Fig. 5 is applied to the Joints 1-3 of the Puma 560 manipulator and the torque b in the same figure to the joints 4-6, ‘The dialog box of the block “Trayectorias artculares” is shown in Fig. 6. ‘The kinematic and dynamic parameter matrix has been taken from Armstvong and others [1], and the simulation results are in Fig. 7 Fig. 7. Join trajectories obtained in the simulation of the dynamic model of the Puma 560 with the inputs in Fig, 5 ‘The upper trajectory corresponds to the first joint. 464. CONTROL, The control strategies can be implemented by using Simulink. Then, in addition of the blocks corresponding to the dynamic model, several blocks to facilitate the implementation of control methods have been developed, Particularly, several versions of the computed torque method to control robotic manipulators have been implemented. Consider 2 computed torque control system with PD. motion conttollers for the joints of the Puma 560 manipulator. The corresponding Simulink diagram is shown in Fig. 8. L,. The ot ke C+ k— a i }—_J Fig, 8 Simulink diogram for the simulation of the computer torque control system It is intended that the robotic joints follow joint trajectories given by: 940) = sine @ ‘The follo inal the. ‘values are selected for the gains of the PD 10%, ®@ whore J is the 6x6 unity matrix. ‘The study of the effects of unmodelled dynamic terms is very easy with the HEMERO tool. Thus, Fig. 9 compares, the trajectories obtained with a perfect model in the design of the controller and the resulting trajectories ‘when an unmodelled viscous friction of 0.1 Nem sec. appears in the first joint. Notice that when the model is perfect the errors in joints go to 2610 after a short transiont, However, when the friction is present the behavior deteriorates significantly in the First joints. ror 8, (28 Tiempo (609) Fig. 9. Effect ofa viscous friction in the frst joint of the ‘Puma 560 not considered in the computed torque strategy. It is possible the implementation of several control strategies including adaptive control (Slotine y Lee, (12)) and learning control (Craig, [5]) methods. Several Simulink blocks have been developed to facilitate these implementations including the computation of the ‘matrices and vectors required for these control Laws. Fig, 10 shows the Simulink diagram for the application to the Puma 560 of a learning contro! method (Craig, (5). Fig. 10, Simulink diagram for the learning control. It is also assumed a friction of 0.1 Nem secs not considered in the design of the controller. The parameters of this controller are: Ky 125), Ky = 50%, “ 47where %g is the 6x6 unity matrix and the learning parameters of the joints are given by w= 21, Fig. 11 shows the joint position errors in the iterations 1 and 5 of the fearing control method. Observe the improvement of the behavior afer five iterations, Tiempo (seg) Fig, 11. Joint errors in the iterations I and 5 with the Puma 560. ‘The MATLAB-Simulink tool also have several functions and blocks for the implementation of the mobile robot control strategies, including geometric methods such as the simple pure-pursuit technique, and the control theory methods including several methods presented in Canudas and others [3] Assume that it is intended to simulate a Puma 560 mounted on a mobile platform with a tricycle locomotion system controlled using the pure-pursuit technique for path wacking. Fig. 12 shows the diagram to implement this technique and to compute and store the trajectory followed by the platform and the rotational velocity and acceleration and linear acceleration of the platform. le Fig. 12, Simulink diagram for the implementation of the Pure-pursuit 48 I is intended to track a staight line when the vehicle starts from the initial coordinates (0, 0.5) with an initial orientation of O rad. The look-ahead parameter of the pure pursuit method is set to 0.5. The results are in Fig 13, Fig. 13. Pure pursuit results for look-ahead L = 0.5. ‘The dynamic behavior of the Puma 560 mounted on the ‘mobile platform can be studied by means of the Simulink diagram in Fig, 14. It is assumed that there are no errors in the estimation of the manipulator parameters. The PD constants are the same that in the previous example. It is assumed that the joints of the Puma S60 start to move at (05 sec, (when the tricycle is already moving). of] Baek i wet dt yey || Gol > Ca}—_| 2a Fig, 14. Simulink diagram for the simulation of the Puma ‘560 mounted on the tricycle mobile platform with a computed torque control method.Fig. 15, shows the errors of the Puma 560 joints, Itcan be jons due to the motion of the mobile When the manipulator was ina fixed seen the perturb: base that do not exis base (Fig. 9). (as) Errer 8, Tiempe (369) Fig, 15. Joint errors in the Puma 560 with the mobile base. 5. TRAJECTORY GENERATION MATLAB functions for the implementation of alternative trajectory generation methods have been developed including cubic and quintic interpolation polynomials and linear trajectories with parabolic blends (Craig, (6). Ivis also possible to use B-splines, Bezier polynomials, and B-spline curves (Barsky, [2)) ‘The functions can be applied to generate trajectories in the joint space and in the cartesian space, Furthermore, they can be applied for both manipulators and mobile robots providing the trajectories to be followed with the ‘control methods presented in the above section. It has been also planned to incorporate particular functions for manoeuvring such as the generation of ‘trajectories for the automatic parking on nonholonomic mobile platforms as presented in Gémez-Bravo and others (7) 6, GRAPHICAL INTERFACE ‘Some MATLAB functions of the toolbox, such as the ‘above presented plotbot, can be used for simple ‘graphical representations (sce Fig. 1). If a more realistic visualization is requited, itis pos to.use some other programs. Particulaily, the RoboWorks [11] program for the graphical 3D design and visualization has been used. Fig. 16 compares the Puma 560 RoboWorks representation with the results of the function plotbot for the same values of the joint variables. Fig. 16. Puma560 RoboWorks and plotbot representations for the same values of the joint variables. ‘The files with the trajectories generated by MATLAB. and Simulink with the HEMERO tool can be processed by RoboWorks to generate the corresponding animations. 7. CONCLUSIONS, ‘This paper summarizes the HEMERO toolbox for the study of manipulator and mobile robots. The tool combines MATLAB and Simulink for the representation, and the kinematic and dynamic modelling of the robots. It includes several control methods for the manipulators and the mobile robots. ‘The tool can be easily used for the simulation of classical text books examples and particularly the examples presented in Ollero {9]. Furthermore, the user can create its own examples and apply (o them several alternative ‘control methods. ‘The numerical results can be illustrated by means of simple graphics generated with MATLAB or by 49‘generating files that can be processed by RoboWorks for realistic 3D animations. The tool can be executed by means of MATLAB 5.2, 0 later versions, and Simulink. A version of the tool is being implemented in a web site. Furthermore, an internet tool based on HEMERO, which is called HETERO, is being implemented ACKNOWLEDGEMENTS ‘The work described in this paper has been funded by the CICYT TAP 1999-0926-C04-01. REFERENCES Armstrong, B., ©. Khatib, and J. Burdick, The ‘explicit dynamic model and inertial parameters of the Puma $60 arm. Washington. Proc. IEEE Int Conf. Robotics and Automation, 1986, vol. 1, pp. 510-18, 1986. w Barsky, B.A., Computer graphics and geometric ‘modelling using B-Splines. Springer, 1988, Canudas de Wit, C., B. Siciliano and G Bastin, ‘Theory of Robot Control. Springer, 1997. BI Corke, PLL, A Robotics Toolbox for MATLAB. IEEE Robotics and Automation Magazine, 1996, vol. 3, no. 1, pp 24-32. (4) 50 (51 (6 a (8) 19) (10) (uy) fy 03) Craig, 34, Adaptive control of mechanical mani pulators. Addison Wesley, 1988, Craig, LJ., Introduction to roboties. Addison Wes- ley, 1989, second edition, Gomez-Bravo, F, F. Cuesta and A. Ollero, Para Hel and diagonal parking in nonholonomic auto- omous vehicles. Engineering Applications of Artificial Inelligence, Pergamon, No. 4, 2001 Hartenberg, R.S. and J. Denavit, A kinematic notation for lower pair mechanisms based on ‘matrices. Journal of Applied Mechanics, 1955, vol. 77, pp. 215-221. Ollero, A., Robética: Manipuladores y robots mviles. Marcombo-Boixareu edilores. ‘To appear in July 2001 Paul, RP, Robot manipulators: Mathematics, Pro ‘gramming, and Control. Cambridge, Massachu- setts. MIT Press, 1981. RoboWorks, 2001, http://www:newtonium.com/ Slotine J.,Li, W., Adaptive manipulator control: a case study. IEEE Trans. on Automatic Control, 1988, Vol. 33, pp 995-1003. Valera, A., Tornero, J. and Salt, .J., Desarrollo de una Libreria de Control de Robots para MATLAB/ ‘Simulink. Salamanca. Actas de las XX Jornadas de Automatica, 1999, pp 131-136.