#### javaKUKAplay KUKA in Java

###### Kinematics

This post explains position and orientation of the manipulator linkages in static situations. 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 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): It’s convenient to put translation and rotation (about the z-axis) into one matrix: Translation and rotation about the y-axis amounts to 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 (x’, y’, z’) in the original {0}: We denote the matrix description of {2} relative to {0} with : The principle is that the matrix of a successive transformation is at the right side of the multiplication: 2. Forward kinematics
Following the rule of compound transformation (3), it is straightforward to computer , namely, the description of the FLANGE {6} relative to the WORLD {0}: Where the six matrices are given by ( stands for and stands for )  Where and (i=0,1,.., 5) are the DH (Denavit–Hartenberg) 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}: 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 x’, y’, z’ are the coordinates of the link in the frame {0}, so we can render all the vertices (x’, y’, z’) in {0} correctly.

The vertices of the second link are given by Likewise for other succeeding links. Finally, the position of the flange (the sixth link) would be As such, one can visualize the links one by one using the transformation matrices for i=1, 2,…6: DisplayKuka (dependencies: Processing and Peasycam) demonstrates the forward kinematics using a 3D model of KR6 R900. Please download the 3D 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 .

Another function double[][][] forwardSequence(double[] ts) returns the six matrices: , , , , , and .

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.  denotes the BASE frame relative to WORLD. Regarding the tool as the 7th link, we denote the TOOL frame (relative to FLANGE) as , as a result:  where is the very data that one mostly cares for, because 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) which converts joint angles A1-A6 (with TOOL and BASE) to (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 ), there could be multiple solutions of joint angles. KUKA uses S and T parameters to reduce ambiguity. The following image shows four configurations of joint angles with the same position . The robot arm has a characteristic feature: the center of the fifth joint ( ), as well as , , , and , always lies on the XZ plane of the first joint. Therefore angle A1 is obtained by  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 :   where denote the projection of a vector onto the XY plane of WORLD. Notice that is available once A1 is obtained. Then we compute  so A2 and A3 are given by  Notice that the above formula only gives one possible pair of A2-A3 values that lead to a given position .

Now, we take an algebraic approach to find A4, A5, and A6. First, one can obtain the matrix’s value: since could be computed from A1, A2, and A3. Based on the following equation: we finally arrive at the angels A4-A6:   Equation (4) indicates that is a function of A4, A5, and A6. It is easy to find the following invariance: which leads to multiple solutions of A4, A5, and A6 given a specific .

Additional care should be taken for singularity ( ). The following graph shows that the mapping from a Cartesian position (of flange) to joint angles is not necessarily 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 . 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);