javakuka.com Create KUKA Robot Language (KRL) codes with

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 tively (here, we replace cos with c and sin with s) 2 3 2 3 2 1 0 0 c✓y 0 s✓y c✓z s✓z 40 c✓x 4 0 4s✓z c✓z s✓x 5 1 0 5 0 s✓x c✓x s✓y 0 c✓y 0 0 Therefore, rotating a point (x, y, z) z-axis leads to 2 03 2 x c✓z s✓z 4y 0 5 = 4s✓z c✓z z0 0 0

the x, y, and z-axis respec3 0 05 1

first about the x-axis, then about the y-axis, and finally about the 32 0 c✓y 05 4 0 1 s✓y

0 1 0

32 s✓y 1 0 5 40 c✓y 0

0 c✓x s✓x

32 3 0 x s✓x 5 4y 5 c✓x z

(1)

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 by:

The following transformation {X 0, Y 0, Z 0, A 90, B -90, C -90} for KUKA can be virtually performed 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 cA sA 0 cB 0 sB 1 0 4sA cA 05 4 0 1 0 5 40 cC 0 0 1 sB 0 cB 0 sC

32 3 0 x sC 5 4y 5 cC z

(2)

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 x0 y0 x = 4x1 5 y = 4 y1 5 x2 y2

2 3 z0 z = 4 z1 5 z2

a straightforward representation of orientation is a 3 ⇥ 3 orthogonal matrix (whose columns are orthonormal 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 03 2 x cAcB 4y 0 5 = 4sAcB z0 sB

cAsBsC sAcC sAsBsC + cAcC cBsC

cAsBcC + sAsC sAsBcC cAsC cBcC

2 3 3 x x 6 7 y7 y5 6 4z 5 z 1

(4)

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 y 1 z1 5 r T = x1 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

· 6· 6 4· 0

· · · 0

· · · 0 11

3 d 07 7 05 1

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 0 0 4 cos ✓ 05 (1) r T = r T sin ✓ 0 0 1 Combining the origin O leads to a 4 ⇥ 4 matrix (the (1)) 2 · · · 6· · · 0 0 6 rT 4· · · 0 0 0

top left 3 ⇥ 3 elements are taken from matrix 0r T 0

Therefore, the rotated BASE (relative to WORLD) is

3 Ox Oy 7 7 Oz 5 1

= 0r T 0 rb T

(3)

0 bT

(2)

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 one.

2✓ from the first

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 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 y 1 z1 5 r T = x1 x 2 y 2 z2 T

T

O, Pxy

O,

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 O), respectively, relative to the rotary table. Thus, rb T (T2 in Java) can be obtained by: r T (Pxy 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 translations 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 c✓z s✓z 0 x 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 6 7 y7 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 x c✓y 0 s✓y 6y 0 7 6 0 1 0 6 07 = 6 4 z 5 4 s✓y 0 c✓y 1 0 0 0

32 3 x x 6y 7 y7 76 7 z 5 4z 5 1 1

(2)

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 76 0 7 6y 7 y 1 0 y 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 We denote the matrix description of {2} relative to {0} with 2 32 c✓z s✓z 0 x c✓y 6 7 6 s✓ c✓ 0 y z 0 6 z 76 0 2T = 4 0 0 1 z 5 4 s✓y 0 0 0 1 0

0 2T

0 1 0 0

s✓y 0 c✓y 0

3 x y7 7 z5 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 16

(3)

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

We can obtain the general form of

= 01 T 12 T 23 T 34 T 45 T 56 T

i i+1 T

from 2 ci 6 s i 6 i i+1 T = 4 0 0

the DH parameters si c↵i ci c↵i s↵i 0

si s↵i ci s↵i c↵i 0

3 ai c i ai s i 7 7 di 5 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 6s 1 c1 0 a1 s 1 7 6s2 0 c0 a0 s 0 7 c2 a2 s 2 7 0 1 2 6 7 6 7 6 7 1T = 4 0 2T = 4 0 3T = 4 0 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

c3 6 s 3 6 3 4T = 4 0 0

0 0 1 0

s3 c3 0 0

3 a3 c3 a3 c3 7 7 d3 5 1

2

c4 6 s 4 6 4 5T = 4 0 0

0 0 1 0

s4 c4 0 0

3 a4 c4 a4 s 4 7 7 d4 5 1

2

c5 6 s 5 6 5 6T = 4 0 0

s5 c5 0 0

3 0 a5 c 5 0 a5 s 5 7 7 1 d5 5 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 bT

denotes 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 b 7T

=

= 06 T

0 bT

6 7T

10 6 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 ✓0 (A1) = tan

19

1

(p5y , p5x )

2

3 0 6 0 7 7 p5 = 06 T 6 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 ↵ ↵ = tan

g = s||p5xy

p2xy ||

1

(h, g)

h = p5z d0 ( 1 if (p5xy p2xy ) · (p1xy s= 1 otherwise

p2xy ) < 0

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 ◆ l1 + l32 l22 1 = cos 2l1 l3 ✓ 2 ◆ l1 + l22 l32 1 = cos 2l1 l2 so A2 and A3 are given by ✓1 (A2) = ↵ + ⇡ 2 Notice that the above formula only gives one possible pair of A2-A3 values that lead to a given position ✓2 (A3) =

tan

1

(a2 , d3 )

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 6 6 7 c3 a3 c3 7 c 4 a4 s 4 7 3 6 7 6s4 0 7 6s5 c5 0 a5 s5 7 6T = 4 0 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

(4)

we finally arrive at the angels A4-A6: ✓3 (A4) = tan

1 3 (6 T12 , 36 T02 )

✓4 (A5) = cos ✓5 (A6) = tan

1 3 (6 T22 )

1 3 (6 T21 ,

3 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 6 T (A4, A5, A6)

=

3 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 responsively. 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, InSight 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