ROBOTLAB: A SOFTWARE FOR ROBOTGRAPHIC SIMULATION

Narpat s. GehlotUniversidade Federal da Paraíba - Departamento de Engenharia Elétrica

58109 - 970 Campina Grande-PB-BrasilE-mail: gehlot@dee.ufPb.br

Pablo J. AlsinaUniversidade Estadual da Paraíba - CCT - Departamento de Física

58100-000 Campina Grande - PB - Brasil

Abstract: The ROBOTLAB is a MATLAB based software package developed for robotsimulation and analysis. MATLAB users can analyze and simulate any arbitrary seriallink manipulator using ROBOTLAB by specifying robot kinematics, geometric anddynamic parameters. The package incorporates an extensive library of functions oftransforrnations, kinematics, dynamics, control, trajectory and 3D animation of robots .IIIustrative examples are presented to show the capability of the ROBOTLAB.

1. INTRODUCTION

The availability of powerful software packages suchas Mathematica, MATLAB and MAPLE, hasmotivated the development of software tools for theanalysis and simulation of robotic systerns (Nethery& Spong, 1994; Corke, 1996). Also, for Unixoperating systems a robot graphic simulator(Marhefka & Orin, 1996) has been developed whichis portable over wide range of platforrns . The use ofsoftware based tools for the simulation and analysisof robotics systems provides a very attractivesolution of design problems & is ideal foreducational purposes, since the cost of experimentalsetups is generally high. Moreover, the flexibility ofthe software tools perrnits convenient training andteaching aids (White, et al., 1989) as well as allowsthe investigation of control strategies without theneed of an experimental prototype (McMillan, et aI.,1995; Agba, 1995). In this paper, the ROBOTLAB:A Graphic Robot Laboratory Simulator (Alsina, J

1996) is described which consists of a versatilerobotics functions library, interactive graphic userinterfaces and forceful 3D animation tools .ROBOTLAB was developed for personal computers,running MATLAB 4.0 in Windows . Given anappropriate description file of any arbitrary N-link

serial robot, the ROBOTLAB functions, likeMATLAB functions, are used in the analysis,dynamics simulation and 3D animation to create aVirtual Prototype. ROBOTLAB user interface isdescribed in Section 2. In Section 3, robot modelingcommands are discussed . Section 4 presents ageneral description of the RüBOTLABenvironment, including details of the differentprogram modules and graphic output interface. AniIIustrative example is given in Section 5 to show theversatility of the package. A comparison with otherstate-of-the-art packages is given in Section 6.

2. ROBOTLAB USER INTERFACE

ROBOTLAB is an interactive environment for robotanalysis , dynamic simulation and graphic 3Danimation. ROBOTLAB front end is a friendlygraphical user Interface, developed in MATLAB,which provides an easy way of executing theROBOTLAB tasks. The ROBOTLAB commandfunctions (see Table I) are norrnally transparent to.the user and are utilized for writing RÇ)BOTLABuser programs. Figure 1 shows an example of theGraphic user interface.

465

file . f-diLWlndows ::· lielp .' t,•..._..- . -'.. . .._."-- - '. .- . ..... - _.._.. _. ..... - .... . .,.I.:

F·OBOTL.A.B

-];:".,r ·1··1

variables : one for the forrn and size and other for thereiative positioning of the part in the link . The firstvariable foIlows the forrnat: P<xx>L<yy>, where<xx> is a two-digit number associated to the partand <yy> is a two-digit number associated to thecorresponding link. This var iabIe must be initializedby means of an appropriate graphic primitivefunction in order to store the forrn and size of thecorresponding part oThe second variable defines therelative positioning of the part with respect to thelink reference frame by means of a homogeneoustransformation. The general format of this variableis TP<xx>L<yy>, where <xx> and <yy> weredefined above. For example, the part 3 of link 4could be created as:

1,

The number 2 in part P03L04 is a code for ahemispheric part and 0.5 is the radius length . Thevariable TP03L04 indicates that the hemisphericpart is aligned with the reference frame ofthe link 4(rotation matrix equal to identity matrix) and itscenter is localized 5 units in the positive direction ofthe x-axis of the frame. Using the geometri cmodeling form given above, user can specify anyrobot of desired structural detai ls. Figure 2 shows anexample ofrobot manipul ator geometric modeling.

0.5

o

X(m)-o. -0.5

Y(m)

0.2

0.4

0.8

N

P03L04 = [2 0.5]

TP03L04 = [ I O O 5O 1 O OO O 1 OO O O 1];

Any serial robotics manipulator can be simulated inROBOTLAB. The manipulator is specified by theuser by a proper parameter file in the formatMATLAB "script-file". This file includes thekinematics, dynamics and geometric parameters of arobot to be simulated. The input data (from files orkeyboard) is written in the MATLAB languageforrnat: matrix between brackets, elements in a rowseparated by blank spaces and each row separated bya semicolon. The kinematics parameters are given ina matrix named KINPAR. Each one ofthe N rows ofmatrix KINPAR consists of tive elements thatdescribe completely the kinematics of one link: thetype of joint and the four Denavit-Hartenbegparameters (ai, ai, di, ai) (Craig, 1986). Forexample, for single-link system with a rotationaljoint, the cinematic parameters.are specified as:

Similarly, the dynamics parameters are given in anN row matrix named DYNPAR. Each row includesthe mass (m.), vector of center of mass (S = [S, S,Sz]T), inertia tensor elements n, = n, Iyy I,z]T andI, = [Iyz .Ixz Ixy]T), Coulomb friction coefficient (Ac)and viscous friction coefficient (Av) of.one link. Forexample, for single-link ' system with a rotationaljoint, the dynamic parameters are specified as:

Fig. 1 - ROBOTLAB Menu .

3. ROBOT MODELING

The geometric model of the robot is composed by\

links, which are numbered sequentially, startingfrom the base of the manipulator. Each link isCõmposed by several numbered parts (graphicobjects), which are created using some specialgraphic primitive functions (prism, cone, cylinder,sphere, hemisphere). Each part is specified by two

Fig. 2 - Example ofrobot geometric modelin g.

4. THE PROGRAM

4.J General CharacteristicsROBOTLAB is structured in several module s, whichcan be expanded accordin g to the user needs .ROBOTLAB is divided: in . six main modules :

466

Table I ::.List ofROBOTLAB Functions

/

Homogeneous Transfonns, Trajectory Generation,Kinematics, Dynamics, Graphics and DynamicsSimulation. Each module consists of a set ofMATLAB functions, developed for each specificproblêrn, and, due to the open structure, they can beutilized by the user in his own programs. A list ofthe available ROBOTLAB functions is given inTable I. <,

4.4 Kinematics

This module includes a set of functions related to thekinematics of manipulators: Home Position, LinkTransfonnations, Direct Kinematics, InverseKinematics, Euler Angles, etc. Due to thecomplexity of the inverse kinematics problem, a"script-file", for computing the inverse kinematics ofits own robot , must be fumished by the user . Thisfile must update the vector of joint variables from agiven position and orientation of the tool. The userhas the option of 3D view of the manipulator in thechosen spatial configuration.

4.5 DynamicsThis module includes several functions, based on theNewton-Euler fonnulation (Featherstone, 1987), forcomputation of the tenns of the dynamics equation:Direct Dynamics, Inverse Dynamics, lnertia Matrix,Coriolis/ CentrifugaI Torques, GravitationalTorques, etc .

4.3 Trajectory Generation

This ROBOTLAB module consists of a set offunction for trajectory generation (position, velocityand acceleration) in joint space. The trajectorygeneration is realized through a 5º order polynomialinterpolation between the initial and final point ofthe path. This module includes functions to updatethe polynomial coefficients and the state vector atany instant ofthe trajectory. The generated trajectoryis stored in a file named trajdata.mat (Trajectorygeneration results).

4.2 Homogeneous Transforms

This module includes severaI functions to deal withthe homogeneous transfonns, which are used todescribe link translation and orientation.

4.6 Dynamics SimulationThis module consists of a dynamics simulator forserial manipulators based on numerical integrationthrough a fourth-order Runge-Kutta method. Acontroller script-file must be fumished by the user toimplement his own robot control law, however, astandard Computed Torque Controller is provided inthe ROBOTLAB. "Measured" values of position,velocity and acceleration can be used for this file toupdate the driving torque vector in a variable namedTORQ. The initial state and the simulation time areinteractively requested by the programo Themanipulator state is updated in the matrices THIST(simulation time), QHIST (joint var iables), DQHIST(joint velocities) , D2QHIST (joint accelerations) eTORQHIST (driving torques). Beside thesevar iables , the following variables are associated to

and

5!h ordertrajectory

offor

Cone-type graphic object.Cylinder-type graphic object.Hemisphere-type graphic object.Prism-type graphic object.Sphere-type graphic object.Graphic display ofa link .Graphic display of a part ofa link .Graphic display ofa robot.Graphic display of unifilardiagramoTransforms positionorientation of graphic object.

rotmaty

rotmatz

trajecttrajpntKinematicstransfortransfb

HomogeneousTransformsrotmatx Homogeneous transfonn about x-

axis.Homogeneous transfonn about y-axis.Homogeneous transfonn about z-axis.

Trajectory Generationcoefiraj Coefficients

polynomialinterpolation.Trajectory generation.Specific point of a trajectory.

Link homogeneous transfonns.Link homogeneous transfonns inbase reference frame.

eulerang Euler angles.dirkin Direct kinematics.invkin Inverse kinematics.homepos Home position.Dynamicsdirdyn Direct dynam ics.invdyn Inverse dynamics.inermat Inertia matrix.inertorq . Inertialload.reactorq Non-inertial load.coritorq Coriolis/Centrifugalload.gravtorq Gravitational load.visctorq Viscous Friction.coultorq Coulomb friction .Dynamics Simulationrgkrobot Runge-Kutta solution of robot

dynamics.State derivatives ofrobot dynamics.robotdyn

Graphicspconepcylindphemispprismpsphershowlinkshowpartshowrobshowunif

transfpa

467

the reference trajectory: QRHIST, DQRHIST andD2QRHIST. The simulation resuits are storedin afile named simudata.mat (Dynamics simulationresponse).

4.7 Graphic ResultsAs explained in Section 3, by using a set of graphic.primitive functions, the user can construct a 3Dgraphic model of its own manipulator, including asmany details as one wants. By means of this model ,RüBüTLAB can be utilized to execute several kindof graphic simulations, resuiting in a powerful 3Danimation tool for robotics. The RüBüTLABgraphic output module includes time responsecurves, 3D animation, 3D tool trajectory, unifilardiagram, etc. Figures 3, 4 and 5 show the graphiccapabilities of the RüBüTLAB.

11I

N II

0.4II

0.201I,

Y(m) X(m)

Fig, 5 - 3D Animation with multiple poses .

t(s) t(s)2

5. APPLICATION EXAMPLE

RüBOTLAB can be used as an educational aid inRobotics , as well as a design tool for off-line testingof control strategies. Suppose, for example, that the

arm , shown in Fig. 2, is required to track acircular trajectory in Cartesian space. üne way toaccomplish this is updating, through inversekinematics, the corresponding joint references frornthe desired Cartesian trajectory, and then, computingthe driving torques by a computed torque controllaw. Figure 6 shows the user file code needed forthis task. This file updates vector TORQ of drivingtorques, utilized by RüBOTLAB for thesimulation .% COMPUTED TORQUE CONTROLLER FILE

1,

O.

In.NO.

o.

Fig. 3 - Time Response graphic output.% Circular Trajectory Generation in Cartesian Space

t=2*pi* simtime/maxtime; % Time IntervalPOSVEC=[R*cos(t); pycenter; di +R*sin(t));ORIENTMAT=eye(3);

% Inverse Kinematics & Reference Trajectory

Qr=invkin(ORIENTMAT,POSVEC);DQr=(Qr-QrO)/samplingtime; QrO=Qr ;D2Qr=(DQr-DQrO)/samplingtime; DQrO=DQr ;

oCID

<, . <'" //' 0.5

Y(m) X(m)

Fig. 4 - Unifilar Diagram

% Tracking Error & Command Acceleration

Err=Qr-Q; DError=DQr-DQ;D2Qc=D2Qr+Kd*DError+Kp*Error;

% Computed Torque Controller

TORQ=invdyn(Q,DQ,D2Qc);Fig. 6 - Computed Torque Controller - User file

468

1 .

1 .

6. COMPARISON WITH OTHER PACKAGES

\

educational purposes. Other simulators with thesame overall goal are Xanimate (Marhefka & Orin,1996), Robotica (Nethery & Spong, 1994) andRobotics Toolbox (Corke, 1996). Xanimate is apowerful tool for graphical simulation of roboticssystems of any topological structure and is portableon a wide range . of platforms. However, theXanimate does not include dynamics simulation,whereas ROBOTLAB petforms both dynamics andgraphics simulations. Robotica is a computer aideddesign package for serial manipulator, includingdynamics simulation, based on the Mathematicapackage, Simnon and a C language front end .Robotica is capable of generating symbolic solutionfor robot kinematics and dynamics, whereas,ROBOTLAB provides numeric solutions. Both .Robotics Toolbox as well as the ROBOTLAB are aoextension of MATLAB language, although , .'ROBOTLAB also runs with a Windows front end .The Robotics toolbox has a more complete set ofrobotic functions than ROBOTLAB, butROBOTLAB is more versatile and realistic ingraphic animation capabilities. A MATLABcompiled version will further improve this salientfeatures of the ROBOTLAB.

7. CONCLUSION

APPENDIX

The graphic robot simulator ROBOTLAB waspresented. This program, based on MATLABlanguage, includes several open modules and can beexpanded and improved by the user. TheROBOTLAB provides user friendly graphic interfaceand graphic output with 3D animation. Thesefeatures make the ROBOTLAB a suitable tool forresearch and teaching. The suggestions for futurework to enhance the functionality of ROBOTLABare : Simulation of any fixed topological roboticstructure, Inclusion of collision detect ion, Expansionof GUI & CAD based generation of parameters.

Dynamics Simulation by Newton-Euler Method

The robot dynamics simulation problem can bestated as follows : given the vector of driving torquesand the initial state, update the time response of thearm position. This can be attained by the numericalintegration of the direct dynamics of the Nvlinkmanipulator, which is represented by the followingnon-linear second order differential equation,

2

X(m)

0.6 0.8(b) Driving Torques

-0 .5

0.4

-0 .'5Y(rn)

0.2

o

-1f

O

Fig. 8 - Circular Trajectory Tracking.

1J

o.Ji

N IIIOAiI

Ioi0.5

Fig. 7 - Time response for tracking a circular path.a) Joint Variables. b) Driv ing Torques.

..

.....3

..__.-O 0.2 0.4 0.6 0.8 1

t ts)

(a) JointAngles

Note that vectors Q and DQ (joint trajectories andjoint velocities) are available "measured values", thatthe user can utilize for controller feedback. Figure 7shows the simulated time response andcorresponding driving torques for this example. Itcan be noted the high torque required by joint 2, dueto the high gravitational load over this joint. Figure8 shows the corresponding Cartesian trajectorytracking. .

were, t is the Nx 1 vector of driving torques, q is theNx 1 vector of joint positions, M(q) is the NxN

Although powerful commercial robot simulationpackages such as SILMA are available, most ofthem were developed for industrial needs. TheROBOTLAB was developed for research and

q = (1)

469

utilized for the inertia matrix computation by anindirect way (Walker & Orin, 1982). First, the vectorof coriolis, centrifugai and gravitational torques, 'tdis updated from:

where '[(O) is the solution of the Newton-Euleralgorithm for the vector of joint accelerations eequal to zero. Then , each column M, of the inertiamatrix is obtained from:

inertia matrix and '[ d (q, q) \is the Nx I vector ofcoriolis, centrifugaI, gravitational and disturbancetorques. The computation of the driving torques(inverse dynamics) can be realized in an efficientrecursive way by means ofthe Newton-Euler method(Featherstone, 1987). This formulation method isrealized in two stages: the Forward and BackwardRecursions. In the Forward Recursion, the cinematicvariables are transformed recursively from the baseto the robot tool. In the Backward Recursion, forcesand torques are updated recursively from the tool tothe base of the robot, thus allowing the computationof the joint driving torques:

(8)

(9)

- Forward Recursion:

= (2)

where vector r (O) is the solution of the Newton-Euleralgorithm obtained by imposing ej = 1 for j = ·i e

ej = ofor j:t= i .

(6)(7)

A jT( . j . .( j ))= . I ro.X p. , + V · + ffi· x ffi·x p. I +1+ 1 1+ I 1 t 1+

(4)

- Backward Recursion:

f; = A;+1f;+1 XlTI;Si +ffi; X(ffij xmsj-rmv,(5)

A;+l n j+l + j'pj+l x(A:+,fj+l)+I jcO j ++ffi j xl jffij -i m.s, XV.Y\ZTn.+zTf

I t I I

where ffii , cOi are respectively the 3xl vectors ofangular velocity and acceleration the link i referenceframe; vj is the 3xl vector of linear acceleration oflink i reference frame; f; and n, are the 3x 1 vectors offorce and torque applied on link-i-l)y link i-I; m, isthe mass of link i; Ij is the inertia tensor of link iexpressed in its own reference frame; Si is the 3x Ivector of position of the center of mass of link i. ipi+)and À:+I are respectively the 3x 1 position vectorand 3x3 rotation matrix of frame i+ I in relation toframe i. Zj = [O Ol]T, (Zj = [O OO]T) if joint i

is rotational and Zi =[O OO]T, (Zj = [O Olf) ifjoint .i is prismatic.

The robot dynamics simulation probleminvolves the numerical integration of Eq. (I), thus ,the computation of the inverse of the inertia matrixis necessary. The Newton-Euler formulation cannotbe used directly for the direct dynamics calculation,because it only updates the vector of driving torques.In spite of this , the Newton-Euler method can be

470

REFERENCES

Agba , E. I. (1995). "SeaMaster: An ROV-Manipulator System Simulator". IEEE ComputerGraphics and App lications, pp. 24-31 .

AIsina, P. J. (1996) . "Modular AdaptiveNeurocontrol of Robotic Manipulators" : DoctoralThesis (in portuguese), Federal Univ. of Paraíba,Brasil.

Corke, P. I. (1996) . "A Robotics Toolbox forMATLAB". IEEE Robotics and AutomationMagazine, Vol. 3, No. 1, pp. 24-32 .

Craig, 1. 1. (1986). "Introduction to Robotics -Mechanics & Control". Addison-Wesley .

Featherstone, R. (1987). "Robot Dynam icsAlgorithms". Boston Kluwer AcademicPublishers.

Marhefka, D. W. & D. E. Orin (1996) "Xanimate:An Educational TooI for Robot GraphicalSimulation". IEEE Robotics & AutomationMagazine. Vol. 3, No. 2, pp. 6-14.

McMil1an, S., D. E. Orin & R. 8. McGhee (1995) ."Object Oriented Design of a DynamicSimulation for Underwater Robotic Vehicles".Proc. of IEEE Int. Conference of Robotics andAutomation, Nagoya , Japan , pp. 1886-1893 .

Nethery,1. F. & M. W. Spong (1994) "Robotica: AMathematica Package for Robot Analysis" . IEEERobotics and Automation Magazine, Vol. 1, No.1, pp. 13-20.

Walker, M. W. & D. E. Orin (1982). "EfficientDynamic Computer Simulation of RabotMechanisms" . ASME Joumal of DvnamicSystems , Measure. & Control 104, pp .205-21 I.

White, R. 8., R. K. Read, M. W. Koch & R. J.Schil1ing (1989) "A Graphics Simulator for aRobotic Arm". IEEE Trans. on Education, Vol.32, No. 4, pp. 417-429.