javakuka

Create  KUKA  Robot  Language  (KRL)  codes  with  Java

http://javakuka.com

H.  Hua
Southeast  University
[email protected]
 First Program
Javakuka is an open-source project for creating KUKA Robot Language (KRL) codes in Java. Currently
the following tools are available:
LA: linear algebra.
Robot: forward and inverse kinematics.
KRLwriter: put geometric data into a .src file.
updates:LA140817, KRLwriter031017.

The first is an example of using the robot to cut polystyrene foam.

Step 1: Create the tool path in Java:

A hot knife will be used to follow the spline to cut the foam.

Step 2: After Java writes the spline into KRL as a .src file, copy the first program.src from the computer
to Kuka (KRC:/R1/Program/) and run it. The correspondence between Java, Kuka, and KRL is as follows:

FirstProgram.java includes header(), PTP(), SPL() and other functions. It produces the runnable .src
program.

The second image shows that the trajectories of Tool Center Point (TCP) during the two cuts are
identical; however, the knife’s orientations are di↵erent in the two cuts.
1
 FirstProgram1 visual (dependencies: Processing and Peasycam) visualizes the foam and the tool path.

Unsatisfied with the industrial separation of “design” - “CAD Model” - “production”, the javakuka
project attempts to join design and production. Our strategies are: 1) directly designing tool paths to avoid
generating tool paths from CAD models; 2) transparent mapping from Java codes to KRL instructions to
avoid using additional software.

2
 XYZABC
1. What is ABC?

A, B, C are the essential elements of KUKA KRL language. This introduction serves to explain and
compare three models of orientation: fixed angles, Euler angles (ABC), and the 3-point method.

A typical KRL program reads:

$TOOL=TOOL DATA[1]
$BASE=BASE DATA[1]
PTP XHOME
PTP {X 280, Y 0, Z -10, A 30, B 90, C 0}
···

The first two lines specify the TOOL Frame (relative to the FLANGE frame) and the BASE frame
(relative to the WORLD frame), after you define them with the teach pendant. The following lines specify
the positions (X, Y, Z) and the orientations (A, B, C) of the tool in the BASE frame.

For instance, {X 0, Y 0, Z 0, A 0, B 0, C 0} means that the tool is aligned with the BASE Frame:

3
 Through this article, “TOOL” refers to the static (unless one explicitly changes it in KRL) frame of the
tool relative to the FLANGE. Conversely, “tool” refers to the variable position of the tool in the BASE.

Tips:
To see the TOOL data, navigate in the pendant to Start-up / Calibrate / Tool / Numeric input.
To see the BASE data, navigate to Start-up / Calibrate / BASE / Numeric input.
To monitor the tool’s position, first set the current TOOL/BASE in the pendant, and then go to Display
/ Actual position.

2. X-Y-Z fixed angles

As KUKA uses ABC representation (Euler angles) instead of fixed angles, you may skip this section.
Nevertheless, understanding fixed angles is critical to grasping the spirit of Euler angles. They are simply
two interpretations of one mathematical object!

It is a convention to represent the rotation in a 2D space with a 2 ⇥ 2 matrix


cos ✓ sin ✓
sin ✓ cos ✓
One can obtain the coordinates (x’, y’) of a point (x, y) after rotation by
 0  
x cos ✓ sin ✓ x
=
y0 sin ✓ cos ✓ y
Extending this formulism into 3D, we get three matrices for rotations about the x, y, and z-axis respec-
tively (here, we replace cos with c and sin with s)
2 3 2 3 2 3
1 0 0 c✓y 0 s✓y c✓z s✓z 0
40 c✓x s✓x 5 4 0 1 0 5 4s✓z c✓z 05
0 s✓x c✓x s✓y 0 c✓y 0 0 1
Therefore, rotating a point (x, y, z) first about the x-axis, then about the y-axis, and finally about the
z-axis leads to
2 03 2 32 32 32 3
x c✓z s✓z 0 c✓y 0 s✓y 1 0 0 x
4y 0 5 = 4s✓z c✓z 05 4 0 1 0 5 40 c✓x s✓x 5 4y 5 (1)
z0 0 0 1 s✓y 0 c✓y 0 s✓x c✓x z
where the x, y, and z axes are fixed during rotations. That is why people term such representations with
X-Y-Z fixed angles.
4
3. Z-Y-X Euler angles

The following transformation {X 0, Y 0, Z 0, A 90, B -90, C -90} for KUKA can be virtually performed
by:
Frist, rotating the tool about the tool’s z-axis through an angle A (90 degree)
Second, rotating the tool about the tool’s y-axis through an angle B (-90 degree)
Third, rotating the tool about the tool’s x-axis through an angle C (-90 degree)

The process is illustrated below

The intermediate steps are only for illustrative purposes, as the robot does NOT follow these steps to
reach the final position. Therefore, the unreachable intermediate state {A 90, B -90, C 0} (third column in
the figure) does not prevent the machine from arriving at its final destination.

Contrary to fixed angles (1), the matrix for a successive rotation is at the right side of the multiplication
in the ABC representation (Euler angles). For instance, rotating about the tool’s z-axis (A) and then about
its y-axis (B) amounts to
2 32 3
cA sA 0 cB 0 sB
4sA cA 05 4 0 1 05
0 0 1 sB 0 cB
The general formula for Euler angle rotations is given by
2 32 32 32 3
cA sA 0 cB 0 sB 1 0 0 x
4sA cA 05 4 0 1 0 5 40 cC sC 5 4y 5 (2)
0 0 1 sB 0 cB 0 sC cC z

It is surprising that formulas (1) and (2) are identical. This suggests that X-Y-Z fixed angles is equivalent
to Z-Y-X Euler angles!

Our LA (linear algebra) class implements the conversion between ABC and rotation matrix by the two
functions:

5
 VisualABC1 (dependencies: LA, Processing, and Peasycam) visualizes the tool in the BASE frame.

4. 3-point method
Given the vectors of the tool’s three axes (normalized)
2 3 2 3 2 3
x0 y0 z0
x = 4x1 5 y = 4 y1 5 z = 4 z1 5
x2 y2 z2
a straightforward representation of orientation is a 3 ⇥ 3 orthogonal matrix (whose columns are orthonor-
mal vectors) 2 03 2 32 3
x x 0 y 0 z0 x
4 y 0 5 = 4 x 1 y 1 z1 5 4 y 5
z0 x 2 y 2 z2 z
We can also incorporate the tool’s position (XYZ) into the matrix to obtain a general formula
2 3
2 03 2 3 x
x x 0 y 0 z0 x 6 7
y7
4 y 0 5 = 4 x 1 y 1 z1 y5 6
4 z 5 (3)
0
z x 2 y 2 z2 z
1
6
 Comparing (3) with (2) and (4), we can determine the correspondences between the 3-point method and
the ABC model. The LA class o↵ers a method of coverting an orientation matrix to ABC angles:
double[] ABC(double[][] m) //Euler angles from 3*3 matrix

Formula (3) complies with Kuka’s “3-point” method: 1) define the origin o; 2) find a point p on the
positive x-axis; 3) find a point q in the XY plane with a positive Y value.

The y-axis can be found by: y = Vxy (Vxy · x) x, where x = p o, Vxy = q o.

The z-axis can be obtained by cross product of x and y (both normalized): z = x ⇥ y. Therefore, the
3-point method is equivalent to specifying the vectors of x, y, z axes.

The LA class also contains a method double[][] matrixBy2Axis(double[] dx, double[] dxy) that returns
the orientation matrix by 3-point method.

5. XYZ & ABC

In linear algebra a point (x, y, z) is often represented by a column vector as in formulas (1) (2) (3).
Translating a point is equivalent to vector addition
2 03 2 3 2 3
x x x
4y 0 5 = 4 y 5 + 4y 5
z0 z z

An alternative is multiplying a 3 ⇥ 4 matrix with the column vector

2 3
2 03 2 3 x
x 1 0 0 x 6 7
y7
4 y 0 5 = 40 1 0 y5 6
4z 5
0
z 0 0 1 z
1

Now, we can integrate the rotation and the translation into one formula
2 3
2 03 2 3 x
x cAcB cAsBsC sAcC cAsBcC + sAsC x 6 7
y7
4y 0 5 = 4sAcB sAsBsC + cAcC sAsBcC cAsC y5 6
4z 5 (4)
z0 sB cBsC cBcC z
1

which is consistent with (2) and (3).

One can perform transformations with the LA class, for example (VisualXYZABC1):
double[][] matrix = LA.matrix(20, 30, 40, -90, -90);
double[] point=50, 0, 0;
double[] point transformed = LA.mul(matrix, point);

6. TOOL & BASE revisited

Under most circumstances, a KUKA user first defines the TOOL and the BASE with the pendant and
then refers to them in KRL. However, one can always define a TOOL directly in KRL, for instance:
$TOOL = {X 280, Y 0, Z -10, A 30, B 90, C 0}
where the XYZABC data are relative to the FLANGE.

Likewise we may directly write:

$BASE = {X 0, Y 0, Z -200, A 90, B 0, C 0}
7
 where the XYZABC data are relative to the WORLD.

The contingence of tool and base coordinates makes the serial manipulator unique among CNC machines.
We will use this feature in another entry to facilitate rotary table use.

References:
1) Kuka manual, on tool calibration and base calibration.
2) J. J. Graig, Introduction to Robotics, chapter 2.
3) P. Schneider and D. Eberly, Geometric Tools for Computer Graphics, chapters 2,3,4.

8
 ROTARY TABLE

1. External axes

What’s the point in installing a rotary table when the robot already has six axes? While the robot arm
is indeed agile, adding an external axis could make planning the tool path simpler.
Once the external axes are correctly installed, KRL can directly drive the external axes. Suppose the
robot’s first external axis is a conveyor and the second is a rotary table, then E1 informs the linear unit’s
displacement and E2 informs the degree of the rotation, for example
{X 27.3, Y 84.2, Z 20, A 201.5, B 0, C -90, E1 0, E2 -162}
Recall that XYZ specify the tool’s position relative to the BASE; and ABC inform the tool’s orientation
(Euler angles) relative to the BASE.

There are many options for using a rotary table:

1. Turn the table using its own controller when it is not wired to KUKA.
2. KUKA and the rotary table are fully coupled so the table’s rotation becomes part of the robot’s
internal kinematics. A position instruction XYZABC may invoke the table rotation, even if there are no
explicit instructions on the external axis.
3. KUKA controls the rotary table but decouples the rotation from the robot’s kinematics (let $ EX
KIN={ET1 #NONE, ET2 #NONE,· · · } in KRC/R1/MADA/$machine.dat). Because the robot is unaware
of the e↵ects of the rotation, the o✏ine programming takes care of the cooperation between the robot and
rotary table.

The following example takes the third approach. It keeps a hot cutter on one side of the workpiece (a
cylindrical foam) during the cutting process. The procedure is as follows:
1. Calibrate the tool.
2. Calibrate the rotary table and work piece simultaneously by our 5-point method.
3. The Java program uses the 5-point data to unify the movements of the robot and the rotary table.
Finally, the program transforms the tool path into a .src file.
4. Copy the .src file to KUKA and run the machine.

9
2. Frame

The BASE (usually aligned with the workpiece) is a frame (coordinate system) relative to the WORLD.
Any frame can be defined by its position (x, y, z) and its orientation (x, y, and z-axis):

The orientation can be written as a 3 ⇥ 3 matrix (see the methods matrixBy2Axis and matrixBy3Axis in
LA class). Combining position and orientation leads to a 4⇥4 matrix (allowing matrix-matrix multiplication
and matrix-vector multiplication) 2 3
· · · x
6 · · · y7
6 7
4 · · · z5
0 0 0 1
where the top left block is the orientation matrix. This matrix and KRL’s XYZABC are interchangeable,
see the methods in LA class:
double[][] matrix(double aDeg, double bDeg, double cDeg) //compute a 3*3 matrix from Euler angles
double[][] matrix(double x, double y, double z, double aDeg, double bDeg, double cDeg) //compute a
full matrix from XYZABC
double[] ABC(double[][] m) //compute Euler angles from a 3*3 matrix
double[] XYZABC(double[][] m) //compute XYZABC from a full matrix

3. 5-point method

This method computes the BASE 0b T from five points measured on the workpiece. 0b T is the matrix
representation of the BASE (frame -1) relative to the WORLD (frame 0), equivalent to the XYZABC model
(extended discussions are in entry Kinematics). With a rotary table, the following relationship holds
0
bT = 0r T rb T

where 0r T denotes the frame of rotary table (r) relative to the WORLD while rb T stands for the BASE
(workpiece) relative to the rotary table.

The following image shows the table’s flange and the workpiece can be defined simultaneously by five
points: p: workpiece’s origin; px : a point on the positive x-axis of the workpiece; py : a point on the XY plane
(with positive y) of the workpiece; O+: the workpiece’s origin when the table is turned 90 degrees; O :
workpiece’s origin when the table is turned -90 degrees. The method does NOT assume the workpiece’s
origin is at the center of rotary table nor that the workpiece’s XY plane is parallel to the table’s flange.

10
 To measure the five points:
1. Use a pen to mark the origin P of the workpiece (it is the top center of the cylinder in our case), and
draw a line on the top as the x-axis.
2. Set the current TOOL/BASE in the pendant (BASE = NULLFRAME, i.e. the WORLD).
3. Manually align the defined tool to a point (e.g., the origin P) on the workpiece, navigate to Display
/ Actual position to read the position. O+ and O need to be measured after rotating the table by 90 and
-90 degrees, respectively.
4. Put the position data (XYZ) into the Java program. For example:
final double[] P = 436.39, -1414.71, 763.37 ;
final double[] Px = 341.55, -1315.84, 762.09 ;
final double[] Pxy = 341.52, -1516.66, 764.59 ;
final double[] Op = 425.80, -1412.27, 763.27 ; // +90
final double[] On = 434.58, -1425.12, 763.49 ; // -90

The Java program Rotary produces a runnable .src file for KUKA, employing the LA (linear algebra)
class and the KRLwriter class.

Now, we explain how to transform the 5-point data into the rotating BASE (workpiece). First, the origin,
the x-axis, and the y-axis of the table can be defined as O = (O+ + O )/2, P O, O+ O, respectively.
Thus, the table’s orientation 0r T (T1 in Java) can be computed in Java by:
O = between(Op, On, 0.5);
T1 = LA.matrixBy2Axis(LA.sub(P, O), LA.sub(Op, O));
where the columns of matrix 0r T represent the x, y, z axis of the orientation of the table
2 3
x 0 y 0 z0
0 4
r T = x1 y 1 z1 5
x 2 y 2 z2
T T
Second, the origin, the x-axis, and xy-axis of the BASE (workpiece) are (d, 0, 0), 0r T (Px P ), and 0r T (Pxy
P ), respectively, relative to the rotary table. Thus, rb T (T2 in Java) can be obtained by:
double[] rela ax = LA.mul(LA.transpose(T1), LA.sub(Px, P));
double[] rela ay = LA.mul(LA.transpose(T1), LA.sub(Pxy, P));
double d = LA.dist(O, P);
T2 = LA.matrixBy2Axis(d, 0, 0, rela ax, rela ay); // relative to T1;

r
bT has a form of 2 3
· · · d
6· · · 07
6 7
4· · · 05
0 0 0 1

11
where the top left 3 ⇥ 3 elements represent the frame’s orientation while the last column specifies the frame’s
origin.
Third, we compute the BASE 0b T rotated by an arbitrary angle ✓. A 3 ⇥ 3 matrix represents the rotation
around the z-axis and, subsequently, the orientation of the rotated table 0b T 0 can be obtained
2 3
cos ✓ sin ✓ 0
0 4
0 0
r T = r T sin ✓ cos ✓ 05 (1)
0 0 1

Combining the origin O leads to a 4 ⇥ 4 matrix (the top left 3 ⇥ 3 elements are taken from matrix 0r T 0
(1)) 2 3
· · · Ox
6· · · Oy 7
0 0
rT
6 7 (2)
4· · · Oz 5
0 0 0 1
Therefore, the rotated BASE (relative to WORLD) is
0
bT = 0r T 0 rb T (3)

The corresponding Java codes read:

4. Tool path planning

The plan is to cut ten S shapes around a cylindrical foam. The first S shape is a parametric curve
(parameter s) that lies on the XZ plane of the BASE
2 3 2 3
x r sin(s 7⇡
4 )
4y 5 = 4 50 R 5
z H0 + sH

Other S shapes are the rotated (around the z-axis of the BASE) copies of the original:
2 03 2 32 3
x cos( ✓) sin( ✓) 0 x
4y 0 5 = 4 sin( ✓) cos( ✓) 05 4y 5 (4)
z0 0 0 1 z

If there is no rotary table, the robot has to turn the tool around the cylinder. In this situation, it is
difficult to avoid a collision between the robot and the workpiece. With the help of a rotary table, the robot
arm can stay on one side of the workpiece with the following process:
1. Cut the first S shape.
2. Rotate the table by ✓, update BASE by (3) and
12
 cut the S shape, which is rotated ✓ from the first one.
3. Rotate the table by 2✓, update BASE by (3) and cut the S shape, which is rotated 2✓ from the first
one.

The signs of ✓ in (1) and (4) are opposite, meaning the BASE’s rotations “compensate” the rotations of
the S shapes.

The corresponding KRL codes look like:

KUKA is blind on workpiece’s rotation; however, the updates of $BASE guarantee the relative position
between the robot and the workpiece is correct.

Each S shape corresponds to a SPLINE of three segments (SPLs, SLIN, SPLs):

13
 The following image shows that the two cuts follow the same path but with di↵erent orientations.

This example indicates the added freedom given by a rotary table can facilitate path planning, especially
when collision and the robot’s workspace are critical. The 5-point method is for general purpose; it computes
the BASE rotated with the rotary table.

5. 5-point method revisited

When the workpieces’s origin is close to the turntable’s center, the aforemetioned 5-point method may
result in inaccurate calibration.

14
 To solve this problem, one can measure the 5 points as in the following image

The measured coordinates are saved in the Java codes, for example:
final double[] P = -558.92, 1400.13, 841.81 ;
final double[] Px = -77.44, 1270.00, 843.67 ;
final double[] Pxy = -440.21, 1840.17, 840.69;
final double[] Pxy90p = -999.72, 1516.83, 842.63; // point xy +90
final double[] Pxy90n = -116.46, 1281.05, 844.92 ; // point xy -90

The origin, the x-axis, and the y-axis of the table can be defined as O = (O+ + O )/2, O O, Pxy O,
respectively. Thus, the table’s orientation 0r T (T1 in Java) can be computed in Java by:
O = M.between(0.5, Pxy90p, Pxy90n);
T1 = LA.matrixby2Axis(LA.sub(Pxy90n, O), LA.sub(Pxy, O));
where the columns of matrix 0r T represent the x, y, z axis of the orientation of the table:
2 3
x 0 y 0 z0
0 4
r T = x1 y 1 z1 5
x 2 y 2 z2
T T
Second, the origin, the x-axis, and xy-axis of the BASE (workpiece) are 0r T (P O), 0r T (Px O), and
0 T
r T (Pxy O), respectively, relative to the rotary table. Thus, rb T (T2 in Java) can be obtained by:
double[] rela ax = LA.mul(T1T, LA.sub(Px, P));
double[] rela ay = LA.mul(T1T, LA.sub(Pxy, P));
double[] d = LA.mul(T1T, LA.sub(P, O));
T2 = LA.matrixby2Axis(d[0], d[1], d[2], rela ax, rela ay);

References:
1) KUKA manual, on the calibration of the base and the external kinematic system.
2) J. J. Graig, Introduction to Robotics, chapter 2.
3) P. Schneider and D. Eberly, Geometric Tools for Computer Graphics, chapters 2,3,4.

15
 KINEMATICS
This chapter explains position and orientation of the manipulator linkages. Kinematics, or the transla-
tions between joint angles (A1-A6) and tool positions (XYZABC) is KUKA’s backbone. This entry is of
more theoretical than practical interest, as KUKA already facilitated us directly planning the tool’s motion
rather than being concerned with the joint angels.

1. Transformation algebra

Translating a point leads to vector addition:

2 03 2 3 2 3
x x x
4y 0 5 = 4 y 5 + 4y 5
z0 z z

Rotating the point about the z-axis of the coordinate system (or frame) corresponds with multiplying a
rotation matrix (here we replace cos with c and sin with s)
2 03 2 32 3
x c✓z s✓z 0 x
4y 0 5 = 4s✓z c✓z 05 4y 5
z0 0 0 1 z

It’s convenient to put translation and rotation (about the z-axis) into one matrix
2 03 2 32 3
x c✓z s✓z 0 x x
6y 0 7 6s✓z c✓z 0 y7 6 7
6 07 = 6 7 6y 7 (1)
4z 5 4 0 0 1 z 5 4z 5
1 0 0 0 1 1
Translation and rotation about the y-axis amounts to
2 03 2 32 3
x c✓y 0 s✓y x x
6y 0 7 6 0 1 0 y7 6y 7
6 07 = 6 76 7 (2)
4 z 5 4 s✓y 0 c✓y z 5 4z 5
1 0 0 0 1 1

Suppose the default frame is {0}, performing transformation (1) relative to {0} leads to a new frame
{1}. Performing transformation (2) relative to {1} leads to a third frame {2}. A point (x,y,z) (coordinates
in 2) has its coordinates (x0 , y 0 , z 0 ) in the original {0}
2 03 2 32 32 3
x c✓z s✓z 0 x c✓y 0 s✓y x x
6y 0 7 6s✓z c✓z 0 y 76 0 1 0 y 7 6y 7
6 07 = 6 76 76 7
4z 5 4 0 0 1 z 5 4 s✓y 0 c✓y z 5 4z 5
1 0 0 0 1 0 0 0 1 1
0
We denote the matrix description of {2} relative to {0} with 2T
2 32 3
c✓z s✓z 0 x c✓y 0 s✓y x
6 s✓ c✓ 0 y 7 6 y7
0 6 z
2T = 4 0
z 76 0 1 0 7
0 1 z 5 4 s✓y 0 c✓y z5
0 0 0 1 0 0 0 1

The principle is that the matrix of a successive transformation is at the right side of the multiplication
0
2T = 01 T 12 T (3)

16
2. Forward kinematics

Following the rule of compound transformation (3), it is straightforward to computer 06 T , namely, the
description of the FLANGE {6} relative to the WORLD {0}:
0
6T = 01 T 12 T 23 T 34 T 45 T 56 T
i
We can obtain the general form of i+1 T
from the DH parameters
2 3
ci si c↵i si s↵i ai c i
6 s ci c↵i ci s↵i ai s i 7
i 6 i
i+1 T = 4 0
7
s↵i c↵i di 5
0 0 0 1
where ci stands for cos(✓i ) and si stands for sin(✓i ); c↵i and s↵i denote cos(↵i ) and sin(↵i ) respectively.
↵ = {⇡/2, 0, ⇡/2, ⇡/2, ⇡/2, 0} for most KUKA robots, so the six transformation matrices are given by
2 3 2 3 2 3
c 0 0 s 0 a0 c0 c1 s 1 0 a1 c1 c2 0 s 2 a2 c2
6s0 0 c0 a0 s 0 7 6s 1 c1 0 a1 s 1 7 6s2 0 c2 a2 s 2 7
0 6
1T = 4 0
7 1 6
2T = 4 0
7 2 6
3T = 4 0
7
1 0 d0 5 0 1 d1 5 1 0 d2 5
0 0 0 1 0 0 0 1 0 0 0 1
2 3 2 3 2 3
c3 0 s3 a3 c3 c4 0 s4 a4 c4 c5 s5 0 a5 c 5
6 s 0 c3 a3 c3 7 6 s 0 c4 a4 s 4 7 6 s c5 0 a5 s 5 7
3 6 3
4T = 4 0
7 4 6 4
5T = 4 0
7 5 6 5
6T = 4 0
7
1 0 d3 5 1 0 d4 5 0 1 d5 5
0 0 0 1 0 0 0 1 0 0 0 1
Where ai and di (i=0,1,.., 5) are the DH (DenavitHartenberg) parameters that describe the dimensions
of the each linkage of the robot.

For instance, the DH parameters of KR6 R900 are:

a = 25, 455, 35, 0, 0, 0 ;
d = 400, 0, 0, 420, 0, 80 ;
Likewise, the DH parameters of KR16 are:
a = 260, 680, -35, 0, 0, 0 ;
d = 675, 0, 0, 670, 0, 158 ;
ABB IRB 4600-45/2.05:
a = 175, 900, 175, 0, 0, 0 ;
d = 495, 0, 0, 960, 0, 135 ;

These data agree with the robot’s dimensions, which can be found in the data sheet.

Now, we use forward kinematics to visualize robot’s body in Java. The pedestal is directly rendered in
frame {0} (WORLD), which is the default coordinate system of Java’s virtual 3D scene. As the first link
resides in frame {1}, we need to transform the geometry of the link before drawing it in the frame {0}
2 03 2 3
x x
6y 0 7 0 6y 7
6 0 7 = 1T 6 7
4z 5 4z 5
1 1

where x, y, z are the coordinates of the vertices of the link’s geometry, relative to itself (the link’s bottom
center is located at (0,0,0)); while x0 , y 0 , z 0 are the coordinates of the link in the frame {0}, so we can render
all the vertices (x0 , y 0 , z 0 ) in {0} correctly.

17
 The vertices of the second link are given by
2 03 2 3
x x
6y 0 7 0 1 6y 7
6 0 7 = 1T 2T 6 7
4z 5 4z 5
1 1

Likewise for other succeeding links. Finally, the position of the flange (the sixth link) would be
2 03 2 3
x x
6y 0 7 0 6y 7
6 0 7 = 6T 6 7
4z 5 4z 5
1 1

As such, one can visualize the links one by one using the transformation matrices 0i T (i=1, 2,· · · ,6):

DisplayKuka (dependencies: Processing and Peasycam) demonstrates the forward kinematics using a 3D
model of KR6 R900. Please download the model (and unzip) and place it in the bin folder (Eclipse).
Our Robot class provides the function
double[][] forward(double[] ts)
whose input is the joint angles in radius, and output is the matrix 06 T .

Another function
double[][][] forwardSequence(double[] ts)
returns the six matrices: 01 T , 02 T , 03 T , 04 T , 05 T , and 06 T .

3. BASE & TOOL

XYZABC and transformation matrix are two exchangeable representations of transformations (methods
for conversion can be found in the LA class). When navigating to Start-up / Calibrate / BASE (or TOOL)
/ Numeric input, we can see the BASE or the TOOL represented by XYZABC, which is equivalent to the
matrix representation.

18
 0
bTdenotes the BASE frame relative to WORLD. Regarding the tool as the 7th link, we denote the
TOOL frame (relative to FLANGE) as 67 T , as a result
0 b
b T 7T = 06 T 6
7T

b 0 10 6
7T = bT 6T 7T

where b7 T is the very data that one mostly cares for, because b7 T tells how the tool is located in the BASE
(usually corresponds to the workpiece). Notice that a typical KRL command
PTP X 280, Y 0, Z -10, A 30, B 90, C 0
specifies the tool’s position in the BASE.

The Robot class provides a method

double[] forward(double[] degs, double[][] base, double[][] tool)
that converts joint angles A1-A6 (with TOOL and BASE) to b7 T (in the form of XYZABC).

Here is a simple example testing forward kinematics:

double[] a = 260, 680, -35, 0, 0, 0 ;
double[] d = 675, 0, 0, 670, 0, 158 ;
Robot kr=new Robot(a,d); //KR 16
double[] degs = 35.55, -54.91, 88.58, 62.39, 39.19, -32.95 ; //A1-A6
double[][] tool = LA.matrix(-54.707, -59.723, 77.7, -11, 22, -33);
double[][] base = LA.matrix(898.094, -1265.699,245.752,161.956,-11,22);
double[] results = kr.forward(Robot.DegToRad(degs, ”KUKA”), base, tool);

4. Inverse Kinematics

Opposite to forward kinematics, inverse kinematics calculates joint angels (A1-A6) from tool positions
(XYZABC). Given a flange position XYZABC (corresponds to 06 T ), there could be multiple solutions of
joint angles. KUKA uses S and T parameters to reduce ambiguity.
The robot arm has a characteristic feature: the center of the fifth joint (p5 ), as well as p1 , p2 , p3 , and
p4 , always lies on the XZ plane of the first joint. Therefore angle A1 is obtained by
1
✓0 (A1) = tan (p5y , p5x )

19
 2 3
0
6 0 7
p5 = 06 T 6 7
4 d5 5
1
Within the XZ plane, one can employ geometric method to find A2 and A3 (multiple values possible).
The following image shows the robot skeleton in the XZ plane (orthogonal to the XY plane of WORLD) of
the first joint.

First we compute ↵
1
↵ = tan (h, g)
h = p5z d0
(
1 if (p5xy p2xy ) · (p1xy p2xy ) < 0
g = s||p5xy p2xy || s=
1 otherwise
where (·)xy denote the projection of a vector onto the XY plane of WORLD. Notice that p2 is available
once A1 is obtained. Then we compute
✓ 2 ◆
1 l1 + l32 l22
= cos
2l1 l3
✓ 2 ◆
1 l1 + l22 l32
= cos
2l1 l2
so A2 and A3 are given by
✓1 (A2) = ↵ +
1 ⇡
✓2 (A3) = tan (a2 , d3 )
2
Notice that the above formula only gives one possible pair of A2-A3 values that lead to a given position
p5 .
Now, we take an algebraic approach to find A4, A5, and A6. First, one can obtain the matrix’s value:
3
6T = (03 T ) 1 0
6T

since 03 T could be computed from A1, A2, and A3. Based on the following equation:
2 32 32 3
c3 0 s3 a3 c3 c4 0 s 4 a4 c4 c5 s 5 0 a5 c 5
6s3 0 c3 a3 c3 7 6 c 4 a4 s 4 7 6 7
3 6
6T = 4 0
7 6s4 0 7 6s5 c5 0 a5 s5 7 (4)
1 0 d3 5 4 0 1 0 d4 5 4 0 0 1 d5 5
0 0 0 1 0 0 0 1 0 0 0 1

20
 we finally arrive at the angels A4-A6:
1 3
✓3 (A4) = tan (6 T12 , 36 T02 )
1 3
✓4 (A5) = cos (6 T22 )
1 3 3
✓5 (A6) = tan (6 T21 , 6 T20 )

Equation (4) indicates that 36 T is a function of A4, A5, and A6. It is easy to find the following invariance:
3 3
6 T (A4, A5, A6) = 6 T (A4 ± 180, A5, A6 ± 180)

which leads to multiple solutions of A4, A5, and A6, given a specific 36 T .

Additional care should be taken for singularity (36 T22 = 1). The following graph shows that the mapping
from a Cartesian position (of flange) to joint angles is NOT smooth. The origin of graph is the HOME
position (flange’s position in WORLD). When the flange move slightly toward the positive (or negative)
y-axis of WORLD, A4 and A5 change extremely quickly to ±⇡/2.

The following Java codes (or download TestInverse) illustrate how to perform inverse kinematics with
the Robot class:
double[] a = 25, 455, 35, 0, 0, 0 ;
double[] d = 400, 0, 0, 420, 0, 80 ;
Robot kr = new Robot(a, d); // KR6 R900

double[] position = 525, 0, 890, 0, 90, 0 ; // xyzabc, HOME position, flange’s position in WORLD
double[] theta = kr.inverse(position, new Double(0), null, null);
double[] angles = Robot.RadToDeg(theta, “KUKA”);

References:
J. J. Graig, Introduction to Robotics, chapters 2,3,4.
P. Schneider and D. Eberly, Geometric Tools for Computer Graphics, chapters 2,3,4.

21
 INTERACT

Connecting a camera to a KUKA robot enables the robot to observe the environment and react respon-
sively. The low-level information captured by the camera, namely the pixels, is transformed into a certain
meaningful signal that constantly informs the robot’s motion. This chapter uses this example to briefly
introduce the connection and the communication between the vision system and KUKA.

1. System architecture

One can either develop robot software interpreting low-level signals (pixels) and reacting so that the
robot serves as the brain or let the camera transform the original signals into structured data before feeding
it to the robot so the camera is both the eye and the brain. Our example follows the second approach,
as the commercial vision system (COGNEX) already integrates intelligent functionalities, such as pattern
matching, into the standalone camera. We connected a COGNEX In-Sight 1100 with a KUKA KR6 through
the EtherNet/IP protocol. The Ethernet cable connects the camera with the X66 of the C4 compact
controller.

We used WorkVisual to configure the digital inputs (e.g., $IN[200] to $IN[231] receive a 32-bit integer
value, which represents the x-coordinate) from the EtherNet/IP communication. On the camera side, In-
Sight explorer was used to configure the format of the output data to match the WorkVisual settings. The
figure below illustrates the system architecture.

22
2. Communication

The COGNEX camera recognizes the current workpiece (whose pattern is predefined in COGNEX’s
In-sight Explorer) and computes the workpiece’s position X, Y, and orientation A. We convert the three
real variables into 32-bit integers by multiplying them by 1000 respectively. Eventually, the camera will
constantly send the three integers to the KUKA controller through EtherNet/IP.

For KUKA, one needs to firstly define three integers (SIGNAL) variables that receive the data from the
camera (in KRC:/ R1 / System /config.dat):
SIGNAL INPUT X $IN[200] TO $IN[231]
SIGNAL INPUT Y $IN[232] TO $IN[263]
SIGNAL INPUT A $IN[264] TO $IN[295]

and a FRAME variable whose values will be replaced by real-time data:

FRAME pos=X 91, Y 188, Z 128, A 0, B 0, C 0

Second, the main program (.src) uses the real-time values of X, Y, and A to plan the robot motion, for
example, to reach the recognized workpiece’s position:
FOR NI=1 TO 10
wait sec 2
pos.x= 615+ INPUT X *0.001
pos.y= -180+ INPUT Y *0.001
pos.c= INPUT A *0.001
PTP pos C DIS
END FOR

In real applications, the codes could be much more complicated because of issues of the stability of
communication, smoothness of motion, collision, and safety.

3. Gripper

We used a digital signal from KUKA to control a gripper, for example, ON: grasping; OFF: leasing. To
control the gripper, one needs to

23
 1) Wire the gripper to KUKA’s digital I/O. Some robots have an X41 connection on the arm so we can
easily wire the X41 to the gripper. Otherwise, we need to connect the EtherCAT (inside C4 controller) to
the gripper. One can switch a specific digital output (Display / Inputs/ outputs / Digital I/O) and see the
corresponding LED switch on the EtherCAT module.

2) Manipulate the digital output (corresponding to a specific slot of X41) in the main program (.src) to
control the gripper. For instance:
pos.z= 100
$OUT[2]=false
pos.z= 0
PTP pos C DIS
$OUT[2]=true
wait sec 0.5
pos.z= 100
PTP pos C DIS

which opens the gripper ($OUT[2]=false) before grasping and closes the gripper ($OUT[2]=true) when
grasping at the right position.

24
HOT-WIRE CUT

video

25
PLYWOOD SHELL

video

26
 Version 02.01.2018
http://javakuka.com

H. Hua
[email protected]
Institute of Architectural Algorithms & Applications
Southeast University
Nanjing 210096

27

View publication stats