# 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!

## Images

• is this homework?

it is a six axis robot so each individual transform T1..T6 will have own set of XYZABC values that come from the DH table. that is six values but DH table only shows 4 two translations and two rotations. the "theta offset" is just fixed part of theta, not another degree of freedom

d1 is displacement in the Z direction

a1 is displacement in the X direction

and can you guess what is the displacement value in the Y direction?

Kuka uses order of rotations Rz(A)Ry(B)Rz(C) so the first rotation is about Z and by value A (theta), second rotations is about Y by value B (zero) and third is about X by value C (alpha).

1) read pinned topic: READ FIRST...

2) if you have an issue with robot, post question in the correct forum section... do NOT contact me directly

3) read 1 and 2

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

• This is the symbolic rotation matrix of Rz(A)*R_new_y(B)*R_new_x(C)

(A = 1; B=2;C=3)

In KUE_WEG.SRC you will find the function MAT_TO_RPY

This should answer your questrion

• I found this to be of assistance the last time I had to dig into this question, but I ended up not digging very far, as that part of the project was handed off to someone who already knew DH backwards and forwards.

Still, since SciLab and the RTSX package are all open-source, there should be a lot there you can use.

• When you do a forward transformation there are a few rules:

Rule 1:

Two coordinate systems are given and must not be changed!

RCS: Robot Coordinate System

FCS: Flange Coordinate System

• Notice:

Denavit Hartenberg is NOT Denavit Hartenberg

You Have Denavit Hartenberg introduced by Denavit Hartenberg:

T = rot.z(theta) * trans.z(d) * rot.x(alpha) * trans.x(a)

(position of trans may vary)

You have Denavit Hartenberg introduced by Craig:

T = rot.x(alpha) * trans.x(a) * rot.z(theta) * trans.z(d)

(position of trans may vary)

Thus:

you will get two sets of DH parameters! and have to choose

• Notice:

You should draw the kinematic model of your robot all axes in zero position (makes live easier)

Rule 2:

The direction of Z-Axis should show the mathematical positive direction of the rotation.

A1, A4 and A6 are inverted

Example A1:

Here you have two options:

1) ignore it and use \$AXIS_DIR[1] as given in \$machine.dat (left sketch)

2) use right sketch

S0 = K0

S1 = Kiro (internal robot Coordinate System)

S2 = K1

• A little exercise:

If we compare the two matrices we will find out

cos(theta1) = 1 => theta1 = 0°

sin(alpha1) = -1 => alpha1 = -90°

a1 = a1*cos(theta1) = 350mm

d1 = 675mm

DH(DH) parameters:

theta1 = 0°; d1 = 675mm; alpha1 = -90°; a = 350mm

Ergo:

Having the DH-Transformation and the corresponing transformation matrix we are able to extract the DH Parameters!

In this case it worked - let us check other possibilities

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

Code
``````[-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

Code
``````[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:

Code
``````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:

Code
``````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.

## Images

Edited 2 times, last by Askic ().

• th = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]:

th = [ 0.0, -90.0, 90.0, 0.0, 0.0, 0.0]:

Then my robot looks like this:

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

## Images

• Still asking why is your z-axis of the robot coordinate system looking downwards?

the initial coordinate system is the world coodinate system z-axis pointing up (like in my drawing)

the robot coordinate system is shifted and rotated using the frame \$ROBROOT relative to the world coordinate system

for floor mounted robots \$ROBROOT = {X 0, Y 0, Z 0, A 0, B 0, C 0}

For ceiling mounted robots \$ROBROOT = {X 0, Y 0, Z 0, A 0, B 0, C 180}

I also do not understand how you get your -2290!

(was my fault: used 55mm instead of -55mm)

Following my picture from post #12

we have 1045 - 55 + 1300 = 2290

• OK,

you were actually using \$BASE = {X 0, Y 0, Z 0, A 0, B 0 ,C 180}

Normally the Forward transformation should be done with \$BASE and \$TOOL with \$NULLFRAME

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

Code
``````% 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:

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:

Code
``````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?

Code
``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}``

## Images

Edited once, last by Askic ().

• I am using Anaconda (Python and mathplotlib) and doing all the calculation myself

My result look different:

and the corresponding transformation matrices:

RoboDK gives the same values

You actually can download a testversion of RoboDK

The robot would be a KR500 3

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

And this is my result for T06:

Code
``````[       -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:

Code
``````[       -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.

Edited 2 times, last by Askic ().

• You are using the DH transformation introduced by Denavit-Hartenberg

So some of your parameters are wrong (a3 for instance is -55)

## Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account
Sign up for a new account in our community. It's easy!
Sign in
Already have an account? Sign in here.