ieee robotics and automation magazine ... robotics and automation magazine, submitted for...

IEEE ROBOTICS AND AUTOMATION MAGAZINE, SUBMITTED FOR PUBLICATION AS A REGULAR PAPER, SEPTEMBER 20, 2010 1

The KUKA Control Toolbox : motion control ofKUKA robot manipulators with MATLAB

Francesco Chinello, Stefano Scheggi, Fabio Morbidi, Domenico Prattichizzo

Abstract—The KUKA Control Toolbox (KCT) is a collectionof MATLAB functions developed at the University of Siena,for motion control of KUKA robot manipulators. The toolbox,which is compatible with all 6 DOF small and low-payloadKUKA robots that use the Eth.RSIXML, runs on a remotecomputer connected with the KUKA controller via TCP/IP. KCTincludes more than 40 functions, spanning operations such asforward and inverse kinematics computation, point-to-point jointand Cartesian control, trajectory generation, graphical display,3-D animation and diagnostics. Applicative examples show theflexibility of KCT and its easy interfacing with other toolboxesand external devices.

I. INTRODUCTION

A. Motivation and related work

MATLAB [1] is a powerful and widely used commer-cial software environment for numerical computation, statisticanalysis and graphical presentation, available for a large num-ber of platforms. Specific toolboxes (i.e., collections of dedica-ted MATLAB functions) have been developed in the past fewyears as supports for research and teaching in almost everybranch of engineering, such as, telecommunications, electron-ics, aerospace, mechanics and control. As far as robotics isconcerned, several toolboxes have been recently presentedfor the modeling of robot systems [2]–[7]. These simulationtools have been inspired by various applicative scenarios, suchas, e.g., robot vision [5], [6] and space robotics [3], andhave addressed different targets ranging from industrial [4]to academic/educational [2], [5]–[7].A more challenging problem is to design MATLAB toolk-

its, offering intuitive programming environments, for motioncontrol of real robots. Some work has been done in this fieldfor the Puma 560 [8], [9]: however this robot is known to haveintrinsic software limitations, especially in real-time applica-tions, which have been overcome by more recent manipulators.In this paper we will focus on the manipulators produced

by KUKA [10], one of the world’s leading manufacturers ofindustrial robots. KUKA manipulators are designed to covera large variety of applications in industrial settings, such as,for example, assembly, machine loading, dispensing, palletiz-ing and deburring tasks. A specific Pascal-like programminglanguage, called KRL (KUKA Robot Language), has beendeveloped by KUKA for robot motion control. This languageis simple and allows comfortable programming [11]. However,it does not support graphical interfaces and advanced math-ematical tools (such as, matrix operations, optimization and

The authors are with the Department of Information Engineering,University of Siena, 53100 Siena, Italy.Email of the corresponding author: [email protected]

filtering tasks, etc.), and it does not allow an easy integrationof external modules and hardware (e.g., cameras or embeddeddevices that use standard protocols: USB, Firewire, PCI, etc.).A possible way to overcome these drawbacks is to builda MATLAB abstraction layer upon the KRL. A first steptowards this direction has been taken by a MATLAB toolbox,called Kuka-KRL-Tbx, recently developed at the Universityof Wismar [12]. The authors use a serial interface to connectthe KUKA Robot Controller (KRC) with a remote computerwhere MATLAB is installed. A KRL interpreter running onthe KRC, realizes a bi-directional communication betweenthe robot and the remote computer and it is responsible forthe identification and execution of all the instructions thatare transmitted via the serial interface. Kuka-KRL-Tbx offersa homogeneous environment from the early design to theoperation phase, and an easy integration of external hardwarecomponents. In addition, it preserves the security standardsguaranteed by the KRL (workspace supervision, check ofthe final position switches of every robot’s axis, etc.), andit benefits from the efficient mathematical tools of MATLAB.However, Kuka-KRL-Tbx suffers from some limitations:

• The MATLAB commands of the toolbox are in one-to-one correspondence with the KRL functions: this resultsin an undesirable lack of abstraction that may hinder theuser from designing advanced control applications.

• The serial interface does not allow high transmissionspeeds, and this may represent a serious limitation in real-world tasks.

• The toolbox does not include specific routines for graph-ical display.

Quanser is currently developing a seamless real-timeopen-architecture interface to KUKA small robots based onQuaRC [13]. A control prototyping tool generates code froma Simulink diagram and runs it in real-time in Windows,QNX, or Linux. Full Simulink external mode is supported,which means that the control scheme’s parameters can betuned on the fly and that the feedback data from the robot canbe monitored at run-time. However, the introduction of theadditional QuaRC layer between the robot and the MATLABenvironment appears problematic: in fact, it makes the overallarchitecture more complex and difficult to supervise.As concerns “real-time” motion control of KUKA robots, it

is finally worth mentioning the OROCOS open source softwareproject [14], developed at the University of Leuven. It providesa general-purpose, modular framework for complex sensor-driven robotic tasks. However, even though a toolbox forcreating OROCOS components in Simulink has been recently


released [15], the project is not MATLAB native and itrelies on four C++ libraries for the real-time, kinematics anddynamics, Bayesian filtering and component parts.

B. Original contributions and organization

This paper presents a new MATLAB toolbox, calledKUKA Control Toolbox (KCT), for motion control ofKUKA robot manipulators. The toolbox, designed both foracademic/educational and industrial purposes, includes a broadset of functions divided into 6 categories, spanning operationssuch as, forward and inverse kinematics computation, point-to-point joint and Cartesian control, trajectory generation,graphical display, 3-D animation and diagnostics.KCT shares with Kuka-KRL-Tbx the same advantages and

improves it in several directions:

• The functions of KCT are not a MATLAB counterpartof the corresponding KRL commands. This makes thetoolbox extremely versatile and easy to use.

• KCT runs on a remote computer connected with theKRC via TCP/IP: this protocol guarantees a highertransmission speed than RS-232, and a time determi-nism comparable to that of the Kuka-KRL-Tbx (in fact,although the TCP/IP connection is more sensitive toretransmissions than a serial one, our communicationscheme is not affected by the non-real-time behavior ofa KRL interpreter, as that in [12]). A multi-thread serverruns on the KRC and communicates via Eth.RSIXML(Ethernet Robot Sensor Interface XML) with a clientmanaging the information exchange with the robot. To thebest of our knowledge, KCT is the first software toolpresented in the literature that allows easy access to theEth.RSIXML.

• KCT has several dedicated functions for graphics andanimation (plot of the trajectory of the end-effector, plotof the time history of the joint angles, 3-D display of themanipulator, etc.), and includes a graphical user interface(GUI).

• KCT can be easily interfaced with external toolkits,such as, e.g., MATLAB Image Acquisition Toolbox, theEpipolar Geometry Toolbox [5], the Machine VisionToolbox [6], the Haptik Library [16] or MATLAB rou-tines from the OpenCV Library [17], to perform complexmotion control and robot vision tasks.

KCT is fully compatible with all small and low-payload6 DOF KUKA robot manipulators which use the Eth.RSIXMLwith 5.4, 5.5 or 7.0 KUKA System Software: the controllersKR C2, KRC2 ed05 and KR C3 (equipped with a real-time10/100 card) are currently supported by the toolbox. KCThas been successfully tested on multiple platforms, includingWindows, Mac and Linux. The toolbox is released under theGNU GPL version 3 and it can be freely downloaded from theweb page: http://sirslab.dii.unisi.it/software/kct/This article is the outgrowth of [18], compared to which we

present herein several new functionalities of KCT, as well asa more accurate experimental validation.The rest of the paper is organized as follows. Sect. II

illustrates the main functionalities of KCT. Three applicative

θ1

z0 ≡ z1

θ2z2θ3z3

θ4

θ5z5

z4

θ6

z6

d21

d32 d4

3 d54

d65

xo ≡ x1

yo ≡ y1

Fig. 1. Reference robot: 6 DOF elbow manipulator with spherical wrist.

Small Robots (from 3 to 5 kg)KR3 KR5sixxR650 KR5sixxR850

Low Payloads (from 5 to 16 kg)KR5arc KR5arcHW KR6-2 KR6-2KSKR15SL KR16-2 KR16-2S KR16-2KSKR16L6-2 KR16L6-2KS

TABLE I

6 DOF KUKA ROBOTS CURRENTLY SUPPORTED BY KCT.

examples are reported in Sect. III to show the flexibility ofKCT in real scenarios and its easy integration with othertoolboxes. Finally, in Sect. IV, conclusions are drawn andpossible future research directions are highlighted.

II. THE KUKA CONTROL TOOLBOX

The 6 DOF robot manipulator shown in Fig. 1 will beused as a reference throughout this paper [19], [20]: vectorq = [θ1, θ2, . . . , θ6]T denotes the collection of the joint anglesof the manipulator, and dj

j−1 ∈ R3, j ∈ {1, 2, . . . , 6} the

displacement between the center of the (j − 1)-th and j-thjoint of the robot (note that d1

0 ≡ 0). The homogeneous matrixH6

0 ∈ SE(3) relates the coordinates of a 3-D point written inthe base reference frame 〈x0, y0, z0〉, with the coordinates ofthe same point written in the end-effector frame 〈x6, y6, z6〉.In the next subsections the main functionalities of KCT

will be illustrated. In the interest of clarity, the commands oftoolbox have been subdivided into 6 categories: Initialization,Networking, Kinematics, Motion control, Graphics and Homo-geneous transforms (see Table II). Note that only the Network-ing and Motion control functions, which rely on the TCP/IPand Eth.RSIXML communication protocols, depend on thepeculiar features of the manipulators produced by KUKA(see Fig. 2(a)). The KUKA robot models currently supportedby KCT are listed in Table I: to date, the toolbox has beensuccessfully tested on the KR3, KR16-2 and KR5sixxR850robots.Fig. 2(b) illustrates the communication scheme between

KCT and the robot manipulator. It consists of three parts:


Application Code

Initialization

Networking, Motion control

KinematicsGraphicsHomogeneous transforms

KUKAdependentfunctions

KRC

KUKAindependentfunctions

KUKA RobotController

MATLABEnvironment

(a)

KUKA Control Toolbox

TCP/IP protocol

kctserver.exe

KRCEth.RSIXML

MATLAB

Robot manipulator

kctrsiclient.src

(b)

Fig. 2. (a) Architecture of the KUKA Control Toolbox; (b) Communication scheme between KCT and the manipulator.

1) A remote computer running KCT under MATLAB,2) The KUKA Robot Controller (KRC),3) The robot manipulator.

To establish a connection between the remote computer andthe robot controller, KCT provides kctserver.exe, a C++multi-thread server running on the KRC. kctserver.execommunicates via Eth.RSIXML (a KUKA software pack-age for TCP/IP-robot interface) with kctrsiclient.src,a KRL script which runs the Eth.RSIXML client on theKRC and manages the information exchange with the robot.The server sends the robot’s current state to the remotecomputer and the velocity commands to the manipula-tor via kctrsiclient.src, in a time loop of 12 ms.kctrsiclient.src is also used to define a HOME position(initial position) for the robot arm.Two classes of constraints affect robot’s motion. The hard-

ware constraints depend on manipulator’s physics and can-not be modified by the user. Conversely, the software con-straints (established by the Eth.RSIXML) can be configuredat the beginning of each working session via the functionskctrsiclient.src or kctsetbound (see Sect. III-A formore details). Every time the robot accidentally stops becauseof the hardware bounds, KCT must be re-initialized. This isnot necessary, instead, when the robot halts because of thesoftware constraints.In what follows, all the angles will be in degrees and all

the distances in millimeters.

A. Initialization and networking

The connection between the remote computer and the KRCcan be established in a very intuitive way. The informationrelative to the KUKA robots supported by KCT is stored in theMATLAB file kctrobotdata.mat (see Fig. 3 and Table III)and can be accessed by typing:

>> kctrobot();

The functions kctinsertrobot, kctfindrobot,kctdeleterobot allow to insert, search for and deleterobot data. To initialize a robot (for example the model KR3),it is sufficient to write,

>> kctinit('KR3');

where the argument is a string containing the name of theselected robot, as specified in the file kctrobotdata.mat.The TCP/IP connection can then be established by typing,

>> kctclient('193.155.1.0', 0.012);

where 193.155.1.0 is the IP address of the KRC real-time network card and 0.012 (seconds) is the sampling time.

link1

link2

link3

link4

link5 link6

Fig. 3. Working envelope and links of robot KR5sixxR850 (the image isdrawn from robot’s manual, courtesy of KUKA Robot Group).


Initializationkctrobot Show the list of supported KUKA robotskctfindrobot Search for a robot in the models’ listkctinsertrobot Add a robot to the models’ listkctdeleterobot Remove a robot from the models’ listkctinit Load the parameters of the selected robotkctsetbound Set the workspace’s boundskctgetbound Visualize the workspace’s boundskctchecksystem Check MATLAB version and whether the

Instrument Control Toolbox is installed

Networkingkctsettcpip Set the TCP/IP communication modalitykctgettcpip Return the TCP/IP communication modalitykctclient Initialize the clientkctcloseclient Terminate the clientkctclientmex Initialize the client (MEX-file)kctcloseclientmex Terminate the client (MEX-file)kctsenddatamex Send data to kctserver (MEX-file)kctrecdatamex Receive data from kctserver (MEX-file)

Kinematicskctreadstate Return the current configuration of the robotkctfkine Compute the forward kinematicskctikine Compute the inverse kinematicskctfkinerpy Compute the forward kinematics

(return the pose)kctikinerpy Compute the inverse kinematics

(from the pose)

Motion controlkctsetjoint Set the joint angles to a desired valuekctsetxyz Move the end-effector in a desired positionkctmovejoint Set the joint velocities to a desired valuekctmovexyz Move the end-effector with a desired linear

and angular velocitykctpathjoint Generate a trajectory (joint space)kctpathxyz Generate a trajectory (operational space)kctstop Stop the robot in the current positionkcthome Drive the robot back to the initial positionkctdrivegui GUI for robot motion control

Graphicskctdisprobot Plot the robot in the desired configurationkctdisptraj Plot the 3-D trajectory of the end-effectorkctdispdyn Plot the time history of the joint angleskctanimtraj Create a 3-D animation of the robot

Homogeneous transformskctrotox Hom. transform for rotation about x-axiskctrotoy Hom. transform for rotation about y-axiskctrotoz Hom. transform for rotation about z-axiskcttran Hom. transform for translationkctchframe Change the reference framekctgetframe Return the reference frame

Demoskctdemo General demonstration of the toolboxkctdemohaptik Haptic demokctdemovision Vision demo

TABLE II

LIST OF KCT FUNCTIONS DIVIDED BY CATEGORY

By default, KCT communicates with the server using MEX-files (see Table II). However, it also supports MATLABInstrument Control Toolbox (ICT). To switch between the twomodalities, it is sufficient to write kctsettcpip(’ICT’) orkctsettcpip(’MEX’). Finally, to close the communication,the user should simply type:

’name’ KR3 KR5sixxR650 KR5sixxR850 · · ·’link1’ [mm] 350 335 335

’link2’ [mm] 100 75 75

’link3’ [mm] 265 270 365 · · ·’link4’ [mm] 0 90 90

’link5’ [mm] 270 295 405

’link6’ [mm] 75 80 80

TABLE III

DATA STORED IN THE FILE KCTROBOTDATA.MAT

>> kctcloseclient();

Remark 1: By writing kctclient(’offline’), theTCP/IP connection is not established. However, this enablesthe off-line use of all the KCT functions (except those in theMotion control category, c.f. Fig. 2(a) and Sect. II-C).

B. Kinematics

The state of the manipulator is stored in a 2 × 6 matrix,called robotstate, containing the current position and roll-pitch-yaw orientation of the end-effector (first row), and thecurrent joint angles of the robot (second row). This matrix canbe accessed using the function:

>> robotstate = kctreadstate();

To compute the matrix H60 of the forward kinematics, and the

inverse kinematics solution expressed as a joint angles’ vectorq, KCT provides the following two functions:

>> q = [13, 32, -43, 12, 54, 15];>> H06 = kctfkine(q);>> q' = kctikine(H06);

The function p = kctfkinerpy(q) is analogous tokctfkine but returns the position and roll-pitch-yaworientation of the end-effector of the robot arm as a vectorp = [X, Y, Z, φ, γ, ψ]T . Likewise, the function q =

kctikinerpy(p) computes the inverse kinematics solutionfrom the vector p.

C. Motion control

KCT provides several functions for point-to-point motionand trajectory planning. For these operations, the toolboxdirectly relies on the KUKA robot controller, and the jointor Cartesian information is sent to the KRC in open-loop.Although more sophisticated closed-loop control schemes canbe devised, the open-loop solution offers a good compromisebetween execution time and accuracy of the motion tasks.The simplest operation one could require is to move the

manipulator from an initial to a final configuration definedby robot’s joint angles or by end-effector’s poses. Let qf =[θ1, θ2, . . . , θ6]T be the final desired joint configuration of therobot. The function,

>> qf = [23, 35, 12, -21, 54, 60];>> vp = 20;>> [robotinfo, warn] = kctsetjoint(qf,vp);


(a)

−200

0

200

400

600

−200

0

200

0

100

200

300

400

500

600

X [mm]Y [mm]

Z [m

m]

z

x

y

XY

Z

00

0

6

66

(b)

Fig. 4. (a) The interface loaded by the function kctdrivegui; (b) 3-D animation of the robot arm.

moves the robot from the current to the desired configuration.vp is a parameter that varies between 0 and 100 (percentageof the maximum velocity supported by the Eth.RSIXML),the matrix robotinfo contains the time history of the jointangles and warn is a Boolean variable that is set to 1when an error occurs during robot’s motion. Let now pf =[X,Y, Z, φ, γ, ψ]T be the final desired pose of the end-effector.The function,

>> pf = [412, -2, 350, 20, 12, 15];>> [robotinfo, warn] = kctsetxyz(pf,vp);

moves the robot from the current to the desired pose pf .Note that kctsetjoint and kctsetxyz are user-level

routines relying on two lower-level functions: kctmovejointand kctmovexyz. When kctsetjoint is called, the KRCcomputes the joint velocities necessary to accomplish therequested task using kctmovejoint(qdot). Similarly, whenkctsetxyz is called, the linear and angular velocities of theend-effector necessary to achieve the goal are computed usingkctmovexyz(pdot).It is very frequent in the applications to deal with paths

or trajectories defined by a sequence of Cartesian framesor joint angles. Consider a sequence of n points pi =[Xi, Yi, Zi, φi, γi, ψi]T , i ∈ {1, 2, . . . , n}, stacked into then× 6 matrix,

P =

⎡⎢⎣X1 Y1 Z1 φ1 γ1 ψ1

......

......

......

Xn Yn Zn φn γn ψn

⎤⎥⎦.

The following command,

>> P = [100, 200, 150, 12, -23, 0;>> 10, 0, 50, 24, -15, 11;>> -50, -30, 100, -10, 40, 32];>> vp = 20;>> [robotinfo, warn] = kctpathxyz(P,vp,1);

moves the end-effector of the robot through the three pointsp1, p2 and p3 with velocity vp. The third argument ofkctpathxyz is a Boolean variable enabling (when set to 1)the visualization of the 3-D trajectory of the end-effector andthe time history of the joint angles at the end of the task.The function kctpathjoint is analogous to kctpathxyz:

the only difference is that the trajectory is defined here in thejoint space instead of the operational space. The argument ofkctpathjoint is an n×6 matrix Q, whose rows are vectorsof joint angles:

>> Q = [23, 35, 12, -21, 54, 60;>> 42, -10, 20, 14, -5, 21;>> -15, 31, 10, 12, 20, 80];>> vp = 20;>> [robotinfo, warn] = kctpathjoint(Q,vp,1);

To stop the robot in the current position, the user must firstterminate the execution of the motion control functions usingctrl-c, and then type kctstop(). Finally, to drive therobot back to the HOME position, KCT provides the commandkcthome().A graphical user interface, inspired by Robotics Toolbox’s

drivebot GUI [2] can be loaded by typing (see Fig. 4(a)),

>> kctdrivegui();

It allows the user to easily regulate the joint angles of the robotvia 6 sliders, and visualize the corresponding motion of thelinks through a 3-D animation (see Fig. 4(b)). The trajectorycontrol panel on the right-hand side of the GUI, allows tointuitively plan robot’s point-to-point motion, thanks to thevisual feedback of the 3-D animation.

D. Graphics

Several functions are available in KCT for graphical display.The function,

>> kctdisptraj(robotinfo);


0100

200300

400500

600

−200−100

0100

200

0

100

200

300

400

500

XYZ

X [mm]

Z [m

m]

z

x

y

Y [mm]

00

0

6

66

t =0 sec.

(a)

0 20 40 60 80 100−20

0

20

0 20 40 60 80 100

0

20

40

0 20 40 60 80 100

−80

−60

0 20 40 60 80 100

−20

0

20

0 20 40 60 80 10020

40

60

0 20 40 60 80 100

−20

0

20

θ 1[deg.]

θ 2[deg.]

θ 3[deg.]

θ 4[deg.]

θ 5[deg.]

θ 6[deg.]

samplessamples

samplessamples

samplessamples

(b)

Fig. 5. Example 1: (a) Trajectory of the end-effector; (b) Time history of the reference (dashed) and actual joint angles (solid).

plots the 3-D trajectory of the end-effector, the base referenceframe and the initial and final configuration of the robot.The function,

>> kctdispdyn(robotinfo);

plots the time history of the reference (dashed) and actualrobot joint angles (solid). Finally,

>> kctanimtraj(robotinfo);

creates a 3-D animation of the robot performing the requestedmotion task.

E. Homogeneous transforms

KCT provides a set of transformation functions of frequentuse in robotics. Let d ∈ R

3 be a translation vector and α anangle. The functions,

>> d = [100, -23, 300];>> alpha = 60;>> Htr = kcttran(d);>> Hx = kctrotox(alpha);>> Hy = kctrotoy(alpha);>> Hz = kctrotoz(alpha);

provide the basic homogeneous transformations generatingSE(3) for translation and rotation about the x-, y-, z-axes.Suppose now that we wish to move the robot’s end-effector

with respect to an external reference frame 〈xw, yw, zw〉different from the base 〈x0, y0, z0〉. This could be useful,for instance, in an eye-in-hand framework where robot’smotion should be referred with respect to the camera frame(see Sect. III-C). Let Hw

0 be the homogeneous matrix defin-ing the rigid motion between 〈xw, yw, zw〉 and 〈x0, y0, z0〉.The function,

>> H0w = kctrotoz(alpha)*kcttran(d);>> kctchframe(H0w);

fixes 〈xw, yw, zw〉 as new reference frame. All the operationsspecified by commands executed after kctchframe are thusautomatically referred to 〈xw , yw, zw〉.

III. ILLUSTRATIVE EXAMPLES

This section presents three examples demonstrating theversatility and ease of use of KCT in real scenarios1. In the firstexample, we show an elementary application of the motioncontrol functions (e.g., for painting or welding tasks in anindustrial setting). The second and third example illustratehow to interface KCT with other toolboxes to perform morecomplex tasks. In particular, in the second example we coupleKCT with the Haptik Library [16], in order to control the robotarm with a commercial haptic device (see kctdemohaptik).The third example shows the results of a visual servoingexperiment realized by combining KCT, MATLAB ImageAcquisition Toolbox, the Epipolar Geometry Toolbox [5]and MATLAB routines from the OpenCV library [17] (seekctdemovision). The experiments we will present in the nextsubsections, have been performed using the KUKA KR3 robotwith the KR C3 controller.

A. Drawing a circle

Suppose we wish to draw the circle,⎧⎪⎨⎪⎩

x(k) = 600,y(k) = 150 cos(k), k ∈ [0, 2π],z(k) = 150 sin(k) + 310,

on a whiteboard, with a pen mounted on the flange ofthe KUKA manipulator. To achieve this goal, we must firstinitialize the robot and establish the TCP/IP communication(recall Sect. II-A). It is then opportune to set the softwarebounds of the robot using the command,1The videos of the experiments are available at:

http://sirslab.dii.unisi.it/software/kct/


(a) t = 0 sec. (b) t = 9.5 sec. (c) t = 19 sec.

Fig. 6. Example 1: Snapshots from the experiment.

>> B = [450, 650, -200, 200, 0, 500;>> -90, 90, -90, 90, -90, 90];>> kctsetbound(B);

where the first and second row of the matrix B containthe lower and upper bounds on the position and orientation(limited to the joint angles θ4, θ5 and θ6) of the end-effector,respectively. Note that kctsetbound enables a MATLABwarning message in the Motion control functions when theworkspace’s bounds are violated. To draw the circle with therobot arm, it is sufficient to execute the following lines ofcode:

>> k = [0:pi/50:2*pi];>> x = 600*ones(1,length(k));>> y = 150*cos(k);>> z = 150*sin(k) + 310;>> P = [x',y',z',repmat([0, 90, 0],length(k),1)];>> kctpathxyz(P,20,1);

where P is the matrix of points defined in Sect. II-C. In ourexperiment the circle was drawn in 19 sec. with a maximumposition error less than 1 mm. The sampling time was setto 15 ms, but because of MATLAB’s and TCP/IP’s commu-nication delays the actual value was around 19 ms. Fig. 5(a)reports the trajectory of the end-effector and Fig. 5(b) the timehistory of the joints angles, as returned by kctpathxyz (1sample coresponds to 19 ms). Fig. 6 shows three snapshots ofthe real robot during the experiment.

B. Control of the manipulator via a haptic device

To demonstrate the flexibility and integration capabilitiesof KCT, we established a bidirectional coupling betweena 3-DOF Novint Falcon haptic device (see Fig. 7(a)) andthe KUKA robot. The current position of the haptic deviceis read using the Haptik Library [16], and the velocity ofthe manipulator is controlled with KCT (obviously, sincethe workspace of the haptic device is much smaller thanthat of the robot arm, the position information delivered bythe Falcon needs to be suitably scaled). A force feedbackproportional to the robot displacement is returned by thehaptic interface. Note that the MATLAB environment doesnot support real-time haptic callback-based services becauseof the non-deterministic timers: therefore, the proposed setup

is not suited for real-time remote manipulation tasks, but it isideal for rapid prototyping or teaching purposes.The working frequency of the manipulator, around 83 Hz,

is much lower than that of the haptic device (of the order ofkHz): to couple the two systems, we then lowered the hapticsample time.Since the reference frames of the haptic device and of the

robot are rotated of Rh = Rz(−90◦)Rx(90◦) (see Fig. 7(b)),it is convenient to perform the following change of frame(recall Sect. II-E):

>> kctchframe(kctrotoz(-90)*kctrotox(90));

In this way, all the subsequent robot commands are automat-ically referred to the frame 〈xh, yh, zh〉 of the haptic device.The following lines of code show how the Falcon and therobot manipulator interact:

>> h = haptikdevice;>> for i=1:200>> tic;>> pos = read_position(h);>> write(h,-1*pos*2.5/30);>> while toc < 0.01>> end>> vel = (read_position(h)-pos)/0.01;>> kctmovexyz([vel(1,1), vel(1,2), vel(1,3),...>> 0, 0, 0]*0.015);>> end>> close(h);

The function read_position(h) reads the position of thehaptic device h, write(h,-1*pos*2.5/30) returns theforce feedback proportional to the robot displacement andkctmovexyz sends the velocity commands to the manipulator.Fig. 8 reports the time history of the x-, y-, z-position ofthe haptic device (HD, black) and of the end-effector of therobot arm (red). A maximum position error of about 7 mm isachieved.

C. Visual servoing

When combined with MATLAB Image Acquisition Tool-box, the Epipolar Geometry Toolbox [5] and MATLAB rou-tines from the OpenCV Library [17], KCT offers an intuitiveand versatile environment to test visual servoing algorithms on


Haptik Library

KUKA Control ToolboxTCP/IP

KRC

(a)

x0

y0

z0

xhyh

zh

Rh

(b)

Fig. 7. Example 2: The robot manipulator is controlled via a Novint Falcon haptic device: (a) Communication scheme; (b) Reference frames.

0

1 2 3 4 5 6 7 8 9 10−60

−40

−20

0

20

40

60

time [sec]

x-position[mm]

HDRobot

0 1 2 3 4 5 6 7 8 9 10−60

−50

−40

−30

−20

−10

0

10

20

30

40

time [sec]

y-position[mm]

HDRobot

0 1 2 3 4 5 6 7 8 9 10−50

−40

−30

−20

−10

0

10

20

30

time [sec.]

z-position[mm]

HDRobot

Fig. 8. Example 2: From left to right: x-, y-, z-position of the end-effector of the haptic device (HD, black) and of the robot manipulator (red).

real robots. The classical visual servo control by Rives [21]has been chosen as a tutorial to illustrate the main featuresof KCT in robot vision. The visual control in [21] aims atdriving a robot arm from an initial configuration to a desiredone, using only the image information provided by a cameramounted on the end-effector. The idea behind the controlis that of decoupling the camera/end-effector’s rotation andtranslation by using the hybrid vision-based task functionapproach proposed in [22]. To this end, a suitable errorfunction is minimized in order to first rotate the camera untilthe desired orientation is reached and then translate it towardthe desired position. Fig. 9(a) shows the initial and desiredconfiguration of the robot, and the 3-D observed scene in oursetup. The desired and initial reference frame 〈xd, yd, zd〉 and〈xi, yi, zi〉 of the camera are rotated of R = Rz(10◦) andtranslated of t = [−551.38, −52.35, −200.06]T . The cameracalibration matrix (the image size is 640 × 480 pixels), hasbeen estimated using the Camera Calibration Toolbox [23].In an initialization stage, we first brought the robot to the

desired configuration,

>> qD = [64.4, 45.6, -47.4, 89.1, 64.4, -178];>> kctsetjoint(qD,10);

and took a picture of the 3-D scene using the Image Ac-quisition Toolbox. 13 features have been manually selectedin this picture (see Fig. 9(b)), and collected in the 2 × 13matrix featD. The robot was subsequently brought to theinitial configuration,

>> qI = [-6.6, 73.8, -79.8, -47.6, 9, -52.8];>> kctsetjoint(qI,10);

where a corresponding set of 13 features (featI) has beenchosen (see Fig. 9(c)). Before executing the visual servoingalgorithm, we performed the following change of frame inorder to refer the motion of the robot with respect to thecamera frame,

>> rotfr = kctrotoz(posI(4))*kctrotoy(posI(5))*...kctrotox(posI(6));


0

1000

2000

3000

−500

0

500

0

200

400

600

800

X [mm] Y [mm]

Z[mm]

(R, t)

xd

yd

zd

xi

yi

zi

(a)

(b) Desired image (c) Initial image

Fig. 9. Example 3: (a) Desired and initial configuration of the manipulatorwith respect to the 3-D scene; (b)-(c) Desired and initial image: the corre-sponding features and the epipolar lines are shown in red.

>> kctchframe(kcttran(posI([1:3]))*rotfr);

where posI denotes the pose of the end-effector in the initialconfiguration with respect to the base reference frame. Duringeach of the 200 iterations of the visual servoing algorithm, theoptical flow is calculated with the MATLAB routines from theOpenCV Library, using the pyramidal implementation of theiterative Lucas-Kanade method [24],

>> type = 'opticalFlowPyrLK';>> featAn = cvlib_mex(type,frameP,frameC,featA');

where frameP is the previous image acquired by the camera,frameC is the current image, featA is a matrix containing theprevious features and featAn is a 13 × 2 matrix containingthe current features. The fundamental matrix F [25, Ch.9.2] necessary for the implementation of the visual servoingalgorithm, is estimated using the following function of theEpipolar Geometry Toolbox,

>> F = f_festim(featAn',featD,4);

where the third argument of the function indicates the estima-tion method selected. Once the camera translation vector tAand the rotation angles omegaX, omegaY, omegaZ have beencomputed by the visual servoing algorithm, the KCT function,

>> kctsetxyz([tA', omegaX, omegaY, omegaZ],10);

is called. Fig. 10(a) shows the migration of the features in theimage plane from the initial to the desired configuration (blue:rotation only, red: translation only), and Fig. 10(b) the normof the error function.

IV. CONCLUSIONS AND FUTURE WORK

In this paper we have presented an open-source MATLABtoolbox for motion control of KUKA robot manipulators. TheKUKA control toolbox (KCT) runs on a remote computerconnected with the KUKA controller via TCP/IP. It includesa heterogeneous set of functions, spanning operations suchas forward and inverse kinematics computation, point-to-pointjoint and Cartesian control, trajectory generation, graphicaldisplay, 3-D animation and diagnostics. Special care has beendevoted to keep these functions intuitive and easy to use.The versatility and effectiveness of the toolbox have beendemonstrated through three applicative examples.KCT is an ongoing software project: work is in progress to

extend the compatibility of the toolbox to all KUKA indus-trial robots and to offer new functionalities in the Simulinkenvironment. In order to enhance the prototyping capabilitiesof KCT, we also aim at creating a robot simulator for off-line validation of motion control tasks. The proposed toolboxcurrently relies on the Eth.RSIXML: in future work, we planto revise KCT in order to exploit the superior capabilities ofthe Fast Research Interface (FRI), recently developed for theKUKA lightweight robot [26]. The FRI provides a direct low-level access to the KRC at rates up to 1 kHz (the user canset the flexible cyclic time frame between 1 and 100 ms),while preserving all its industrial-strength features (such as,teaching/touchup, execution of motion primitives, fieldbus I/Oand safety). In addition, the UDP socket communication usedby the FRI ensures easy integration and portability across awide range of operating systems.

ACKNOWLEDGEMENTS

The authors are grateful to Dr. François Touvet and toKUKA Roboter Italy for giving us the opportunity to test thetoolbox on the robots KR16-2 and KR5sixxR850.

REFERENCES

[1] MATLAB and Simulink for Technical Computing. The MathWorks Inc.,USA. [Online]: http://www.mathworks.com/.

[2] P.I. Corke. A Robotics Toolbox for MATLAB. IEEE Rob. Autom. Mag.,3(1):24–32, 1996.

[3] K. Yoshida. The SpaceDyn: a MATLAB Toolbox for Space and MobileRobots. In Proc. IEEE/RSJ Int. Conf. Intel. Robots Syst, pages 1633–1638, 1999.

[4] A. Breijs, B. Klaassens, and R. Babuška. Automated design environmentfor serial industrial manipulators. Industrial. Robot: An InternationalJournal, 32(1):32–34, 2005.

[5] G.L. Mariottini and D. Prattichizzo. EGT for Multiple View Geometryand Visual Servoing: Robotics and Vision with Pinhole and PanoramicCameras. IEEE Robot. Autom. Mag., 12(4):26–39, 2005.

[6] P.I. Corke. The Machine Vision Toolbox: a MATLAB toolbox for visionand vision-based control. IEEE Robot. Autom. Mag., 12(4):16–25, 2005.

[7] R. Falconi and C. Melchiorri. RobotiCad: an Educational Tool forRobotics. In Proc. 17th IFAC World Cong., pages 9111–9116, 2008.

[8] W.E. Dixon, D. Moses, I.D. Walker, and D.M. Dawson. A Simulink-Based Robotic Toolkit for Simulation and Control of the PUMA 560Robot Manipulator. In Proc. IEEE/RSJ Int. Conf. Intel. Robots Syst,pages 2202–2207, 2001.


200 400 6000

100

200

300

400

[pixels]

[pixels]

(a)

0 50 100 150 2000

10

20

30

40

50

60

70

iterations

Normoftheerror[pixels]

(b)

Fig. 10. Example 3: (a) Migration of the features in the image plane from the initial (dot) to the desired configuration (cross); (b) Norm of the error function.

[9] M. Casini, F. Chinello, D. Prattichizzo, and A. Vicino. RACT: a RemoteLab for Robotics Experiments. In Proc. 17th IFAC World Cong., pages8153–8158, 2008.

[10] KUKA Robotics Corporation. [Online]: http://www.kuka-robotics.com/.

[11] G. Biggs and B. MacDonald. A Survey of Robot Programming Systems.In Proc. Australasian Conf. Robot. Automat, 2003. Paper 27.

[12] G. Maletzki, T. Pawletta, S. Pawletta, and B. Lampe. A Model-Based Robot Programming Approach in the MATLAB-Simulink En-vironment. In Int. Conf. Manuf. Res., pages 377–382, 2006. [On-line]. http://www.mb.hs-wismar.de/~gunnar/software/KukaKRLTbx.html.

[13] Quanser’s Rapid Control Prototyping tool. [Online]: http://www.quanser.com/quarc/.

[14] H. Bruyninckx. Open Robot Control Software: the OROCOS project. InProc. IEEE Int. Conf. Robot. Automat, pages 2523–2528, 2001. [Online]:http://www.orocos.org/orocos/whatis/.

[15] Orocos Simulink Toolbox. [Online]: http://www.orocos.org/simulink/.

[16] M. De Pascale and D. Prattichizzo. The Haptik Library: a ComponentArchitecture for Uniform Access to Haptic Devices. IEEE Rob. Autom.Mag., 14(4):64–75, 2007.

[17] MATLAB routines from the OpenCV library. [Online]: http://code.google.com/p/j-ml-contrib/source/browse/.

[18] F. Chinello, S. Scheggi, F. Morbidi, and D. Prattichizzo. KCT: aMATLAB toolbox for motion control of KUKA robot manipulators.In Proc. IEEE Int. Conf. Robot. Automat, pages 4603–4608, 2010.

[19] M.W. Spong, S. Hutchinson, and M. Vidyasagar. Robot Modeling andControl. Wiley, 2005.

[20] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo. Robotics: Mod-elling, Planning and Control. Advanced Textbooks in Control and SignalProcessing. Springer, 2008.

[21] P. Rives. Visual Servoing based on Epipolar Geometry. In Proc.IEEE/RSJ Int. Conf. Intel. Robots Syst, volume 1, pages 602–607, 2000.

[22] B. Espiau, F. Chaumette, and P. Rives. A New Approach to VisualServoing in Robotics. IEEE Trans. Robot. Autom., 8(4):313–326, 1992.

[23] J.-Y. Bouguet. Camera Calibration Toolbox for Matlab. Available on-line at: http://www.vision.caltech.edu/bouguetj.

[24] J.-Y. Bouguet. Pyramidal implementation of the Lucas-Kanade featuretracker. Technical report, Intel Corp., Microprocessor Research Labs,1999.

[25] R. Hartley and A. Zisserman. Multiple View Geometry in ComputerVision. Cambridge University Press, 2nd edition, 2004.

[26] G. Schreiber, A. Stemmer, and R. Bischoff. The Fast Research Interfacefor the KUKA Lightweight Robot. In Proc. IEEE ICRA Workshopon Innovative Robot Control Architectures for Demanding (Research)Applications, pages 15–21, 2010. [Online]: http://www.rob.cs.tu-bs.de/en/news/icra2010.

ieee robotics and automation magazine ... robotics and automation magazine, submitted for...

Documents