Calculating Forward kinematics connection between theory and practice on example of KUKA robot

Hello guys,

I have already read a couple of threads on this forum regarding the calculation of forward (direct) kinematics using DH parameters on real examples of KUKA robots.

Most theory books use DH parameters extensively for calculating forward kinematics. I have found an example of DH parameters for KUKA KR500 robot. The datasheet is given here

The table with DH parameters together with joint coordinate frames is given in the attachment.

The direct kinematics is calculated as matrices multiplications (one for each joint) and the result matrix 4x4 matrix, let say denoted M. The last column of this transformation matrix is always in form: X,Y,Z,1. This means that element m41 is the X coordinate of the last coordinate frame in reference to the base coordinate frame, m42 is Y coordinate etc.

Submatrix 3x3 with elements m11, m12, m13, m21,m22,m23,m31,m32,m33) is a rotation part of the transformation matrix M.

So my question is the following:

How to make a connection from the result of direct kinematics and actual position and orientation in KUKA robot language, which is given by X,Y,Z, A,B,C?

It should be: m41 = X, m42 = Y, m43 = Z, and I wonder how to correlate A, B, C with 9 elements of a 3x3 rotation submatrix of matrix M?

I hope you understand my question.

Thank you very much!

No it is not a homework. It is more like professional curiosity. I have used original (standard) DH notation according to this resource here: https://en.wikipedia.org/wiki/…0%93Hartenberg_parameters

I know for revolute joints, angle theta is a variable and for prismatic joints displacement d is variable. parameters a and alpha are constants and they depend on geometry of the robot.

I also know that KUKA uses consecutive rotations A is rotation about Z, B is rotation about new Y, C is rotation about new X. So total rotation should be: Rz(A)*R_new_y(B)*R_new_x(C)

DH convention uses rotations about z and x (previous and new) and translation in the z and x directions. Y axes are only chosen to complete right hand coordinate systems, so there are no displacements in the y directions.

So If I understood correctly, I will be able to obtain 3x3 submatrix by multiplying (to the right) rotation matrices that are created by series of consecutive rotations of coordinate frames starting from the base until it matches last coordinate frame placed on the flange.

I'll try to make a little example and compare results obtained by DH parameters and the result that should be obtained by simply rotation and translation of coordinate frames starting from the base to the last 6th coordinate frame associated to the last joint.

Thank you all for the replies, especially MOM.

Regarding your observation about K{0}, normally I would also expect to choose Z0 axis to points upwards, but in general this coordinate frame can be fixed as initial condition. That is why first translation along the z axis is (d1) is negative.

But in order to follow your example and notation I will use Z0 to point upwards. But can you please tell me from where you have a1 = 350 mm and d1 = 675 mm.

But if we assume that first coordinate frame is already given like shown in the picture, then we have for the first transformation: translation along -z0 by 1045 mm, rotation around z0 by angle theta1, then translation along x1 by 500 mm, and then rotation around x1 by 90 deg. This way K{0} will match K{1}, so DH parameters are: a1 = 500 mm, alpha1 = pi/2, d1 = -1045mm, th1 = joint_angle (A1).

Now the problem I see here is matching K{1} with K{2}. The normal between z1 and z2 axis has a distance 1300 mm along x2, so the a2 = 1300mm. Now in order to match K{1} with K{2}, it is needed th1 - rotation around z1 by pi/2, so the DH parameters in the second row should be:

a2 = 1300, alpha2 = 0, d2 = 0; th2 = joing_angle(A2) - pi/2.

Now, to match {K2} with {K3}, first there should be rotation about z2 for 180° (pi) plus additional joint_angle(A3) translatin along x3 by 55 mm and then rotation around x3 for -pi/2, so DH for the third row would be:

a3 = 55, alpha3 = -pi/2, d3 = 0, th3 = joint_angle(A3) + pi.

As you can see here there is a deviation in this logic to the complete DH table presented in the picture, but I have double checked everything and the DH table and final T06 transformation matrix is given in the attachment

All this time I'm refereeing to the coordinate frames shown in the attached picture in the first post.

So for the angles [0,30,-45,35,25,10] , the result transformation matrix is:

[-0.23857,  0.10868,  0.96503,  2434.2]
[ 0.65418, -0.71644,   0.2424,  70.297]
[ 0.71773,  0.68913, 0.099823, -2354.0]
[       0,        0,        0,     1.0]

So my originally intended question was how to make correlation with the move instruction in KUKA robot programming language.

Here I have 4x4 transformation matrix. X,Y and Z coordinates of the flange (in comparison to the fixed reference coordinate system K{0}) are: 2434.2, 70.3 and -2354.0 mm. So, how I can now connect A,B and C angles for rotation to the 3x3 submatrix inside this T06 matrix?

I hope you understand what is my question here.

I need to add more to this.

If all angles are zeros, (as MOM said, to make my life easier) then T06 is

[6.1232e-17, -6.1232e-17,         1.0,     1815.0]
[6.1232e-17,        -1.0, -6.1232e-17, 4.2863e-15]
[       1.0,  6.1232e-17, -6.1232e-17,    -2290.0]
[         0,           0,           0,        1.0]

In that case, If I look at the initial picture with K{0} and K{6}, then it looks to me that I can match {0} and {6} in the following way:

1. Translate along z by (1045+1300-55) mm, then translate along x by (500+1025+290) mm

2. Rotate about z axis for 180° (A = 180°), then rotate around new y for -90° (B = -90)

If I do this in Matlab:

v = [0 0 -(1045+1300-55)];
T1 = trvec2tform(v);
v = [500+1025+290 0 0];
T2 = trvec2tform(v);
T3r = rotz(180);
T3 = rotm2tform(T3r);
T4r = roty(-90);
T4 = rotm2tform(T4r);

Tres = T1*T2*T3*T4

The result is:

Tres =

           0           0           1        1815
           0          -1           0           0
           1           0           0       -2290
           0           0           0           1

So in this case, there is a match in calculation.

rotz(A=180)*roty(B=-90) would produce 3x3 submatrix.

Hello Mom,

I assumed that all angles are zero when robot is in configuration like shown in the attached picture (this is how it is shown in the datasheet as well).

Can you please, in your software set {0} in such way that z points downward and then recalculate. I'm pretty sure my result is correct and the main difference is the inital (or maybe home) configuration of the robot. I just assumed and calculated everything as if the robot is in the shown position when all angles are zero. Because base z axis points downward, that is why I had for Z coordinate: -2290.0.

Okay, if I choose to have the base coordinate frame K{0} with the z axis points up, then the D_H parameters are:

% D_H parameters:
a1 = 500;   alpha1 = -pi/2;   d1 = 1045; th1 = 0;
a2 = 1300;  alpha2 = 0;       d2 = 0;     th2 = deg2rad(0.0)-pi/2;
a3 = 55;    alpha3 = -pi/2;   d3 = 0;     th3 = deg2rad(0.0)+pi;
a4 = 0;     alpha4 = pi/2;    d4 = -1025; th4 = deg2rad(0.0);
a5 = 0;     alpha5 = -pi/2;   d5 = 0;     th5 = deg2rad(0.0);
a6 = 0;     alpha6 = pi;      d6 = -290;  th6 = deg2rad(0.0);

This would produce the following homogeneous transformation matrices:

T01 = 
[ 6.12e-17,       1.0,        0,     500.0]
[-6.12e-17,  3.75e-33,      1.0, -7.96e-14]
[      1.0, -6.12e-17, 6.12e-17,    2340.0]
[        0,         0,        0,       1.0]
T02 =
[ 6.1232e-17,         1.0,          0,       500.0]
[-6.1232e-17,  3.7494e-33,        1.0, -7.9602e-14]
[        1.0, -6.1232e-17, 6.1232e-17,      2345.0]
[          0,           0,          0,         1.0]
T03 =
[6.1232e-17, -6.1232e-17,        -1.0,       500.0]
[6.1232e-17,        -1.0,  6.1232e-17, -7.6234e-14]
[      -1.0, -6.1232e-17, -6.1232e-17,      2290.0]
[         0,           0,           0,         1.0]
T04 =
[6.1232e-17,        -1.0,          0,    1525.0]
[6.1232e-17,  3.7494e-33,        1.0, -1.39e-13]
[      -1.0, -6.1232e-17, 6.1232e-17,    2290.0]
[         0,           0,          0,       1.0]
T05 = 
[6.1232e-17, -6.1232e-17,        -1.0,    1525.0]
[6.1232e-17,        -1.0,  6.1232e-17, -1.39e-13]
[      -1.0, -6.1232e-17, -6.1232e-17,    2290.0]
[         0,           0,           0,       1.0]
T06 =
[6.1232e-17, -6.1232e-17,        1.0,      1815.0]
[6.1232e-17,         1.0, 6.1232e-17, -1.5675e-13]
[      -1.0,  6.1232e-17, 6.1232e-17,      2290.0]
[         0,           0,          0,         1.0]

As you can see the final result is the same. There are differences in the intermediate results, but they are there because of the way I defined intermediate coordinate frames. You can see this as well as DH parameters table in the attachment.

Which software do you use for making this plots and calculating matrices. Is it RTSX Ninja for SCILAB?

Since we have the same T06 matrix, which is 4x4, now for joint angles: th = [ 0.0, -60.0, 90.0, 0.0, 0.0, 0.0], the result would be:

ans =
 
[       -0.5, -5.30288e-17,    0.866025,       485.49]
[5.30288e-17,          1.0, 9.18485e-17, -7.71447e-14]
[  -0.866025,  9.18485e-17,        -0.5,      989.869]
[          0,            0,           0,          1.0]

How this could be converted to the command in KUKA with A,B and C?

DECL E6POS K0P1={X 485.0,Y 0.0,Z  989.9,A ??,B ??,C ??,S 18,T 34,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}

OK,

I have made a mistake in calculation for th = [0 -pi/3 pi/2 0 0 0]; and I corrected that, but the results are indeed different.

Here is my Matlab code, it should be very easy to translate to Python:

th = [0 -pi/3 pi/2 0 0 0];
a1 = 500;   alpha1 = -pi/2;   d1 = 1045;  th1 = th(1);
a2 = 1300;  alpha2 = 0;       d2 = 0;     th2 = th(2)-pi/2;
a3 = 55;    alpha3 = -pi/2;   d3 = 0;     th3 = th(3)+pi;
a4 = 0;     alpha4 = pi/2;    d4 = -1025; th4 = th(4);
a5 = 0;     alpha5 = -pi/2;   d5 = 0;     th5 = th(5);
a6 = 0;     alpha6 = pi;      d6 = -290;  th6 = th(6);

T1 = DH_calc(a1,alpha1,d1,th1);
T2 = DH_calc(a2,alpha2,d2,th2);
T3 = DH_calc(a3,alpha3,d3,th3);
T4 = DH_calc(a4,alpha4,d4,th4);
T5 = DH_calc(a5,alpha5,d5,th5);
T6 = DH_calc(a6,alpha6,d6,th6);

T06_dh = T1*T2*T3*T4*T5*T6;

function  T = DH_calc(a, alpha, d, th)
    T = [cos(th), -sin(th)*cos(alpha), sin(th)*sin(alpha), a*cos(th);
         sin(th), cos(th)*cos(alpha), -cos(th)*sin(alpha), a*sin(th);
         0,       sin(alpha),          cos(alpha),          d;
         0,         0,                  0,                  1];
end

And this is my result for T06:

[       -0.5, -5.30288e-17,    0.866025,       485.49]
[5.30288e-17,          1.0, 9.18485e-17, -7.71447e-14]
[  -0.866025,  9.18485e-17,        -0.5,      989.869]
[          0,            0,           0,          1.0]

But why there is a difference in X,Y and Z coordinates.

I know exactly why there is a difference.

First of all, in my case for all zero angles, the result is:

[       -0.5, -5.30288e-17,    0.866025,       485.49]
[5.30288e-17,          1.0, 9.18485e-17, -7.71447e-14]
[  -0.866025,  9.18485e-17,        -0.5,      989.869]
[          0,            0,           0,          1.0]

which is the same result as in your case for A2 = -90° and A3 = 90°.

So, there is an offset in my case, so If I want to move robot in the same position like in your case, then I need to calculate FW kinematics for the following input: th = [0 pi/6 0 0 0 0];

In that case, the Matlab code I used above will give the same result as in your case for th= [0 -60 90 0 0 0].

It is because of the definition of zero. In my case for all zero angles robot looked exactly like your position for [0 -90 90 0 0 0]

P.S. Can you please share your Python matlplotlib code, I'd like to learn how this is plotted in Python.