Kuka robot. Calculate a relative position around TCP

Question

I want to turn the position around the TCP like in jog mode does

The first try was to add the value to the position:

$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
e6pOffsetPos.B = e6pOffsetPos.B + 50
PTP e6pOffsetPos C_DIS

Then I tried the geometric operator ":"

$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
f = {X 0.0, Y 0.0, Z 0.0, A 0.0, B 50.0, C 0.0}
PTP e6pOffsetPos:f C_DIS

TOOL_DATA[1]={X -22.5370,Y 145.857,Z 177.617,A 0.0,B 0.0,C 0.0}

somehow I know that the geometric operator works if I change A, B, C of $TOOL correctly. Direction to the grap object. That means a diferent orientation does need other A, B, C of $TOOL and its not very intuitive to get it...

is there a easier way to do this or to understand this?

Answer 1

You are really close. KUKA uses the Euler ZYX system to calculate the TCP orientation. This means that three rotations happen in a specific sequence to achieve the final orientation: The order is:

1. A rotation about the world Z axis
2. B rotation about the new Y axis
3. C rotation about the new X axis

hence, Euler ZYX.

In order to do a rotation, similar to how TOOL TCP jog operates, you need to do a frame transformation from your current position to the target position. If you want to rotate about the current B axis, it takes more than just a B adjustment of POS_ACT to get there, because the B rotation is only a part of a sequence of rotations where you end up.

So our main program should have some code like this:

$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
current_pos = $POS_ACT
offset = {X 0.0, Y 0.0, Z 0.0, A 0.0, B 50.0, C 0.0}
new_pos = transform_frame(offset, current_pos)

Now lets write code to support that function. I make a math_library.src:

DEF math_library()
END

GLOBAL DEFFCT FRAME transform_frame (offset:IN, origin:IN)
   DECL FRAME offset, result_frame, origin
   DECL REAL rot_coeff[3,3], final[3,3], reverse[3,3]
   rot_to_mat(rot_coeff[,], origin.A, origin.B, origin.C)
   result_frame.X = rot_coeff[1,1]*offset.X+rot_coeff[1,2]*offset.Y+rot_coeff[1,3]*offset.Z+origin.X
   result_frame.Y = rot_coeff[2,1]*offset.X+rot_coeff[2,2]*offset.Y+rot_coeff[2,3]*offset.Z+origin.Y
   result_frame.Z = rot_coeff[3,1]*offset.X+rot_coeff[3,2]*offset.Y+rot_coeff[3,3]*offset.Z+origin.Z
   
   rot_to_mat(reverse[,], offset.A, offset.B, offset.C)
   mat_mult(final[,], rot_coeff[,], reverse[,])
   mat_to_rot(final[,], result_frame.A, result_frame.B, result_frame.C)
   return result_frame
ENDFCT

GLOBAL DEF rot_to_mat(t[,]:OUT,a :IN,b :IN,c :IN )
   ;Conversion of ROT angles A, B, C into a rotation matrix T
   ;T = Rot_z (A) * Rot_y (B) * Rot_x (C)
   ;not made by me. This was in KEUWEG2 function
   REAL t[,], a, b, c 
   REAL cos_a, sin_a, cos_b, sin_b, cos_c, sin_c
   cos_a=COS(a)
   sin_a=SIN(a)
   cos_b=COS(b)
   sin_b=SIN(b)
   cos_c=COS(c)
   sin_c=SIN(c)
   t[1,1]  =  cos_a*cos_b
   t[1,2]  = -sin_a*cos_c + cos_a*sin_b*sin_c
   t[1,3]  =  sin_a*sin_c + cos_a*sin_b*cos_c
   t[2,1]  =  sin_a*cos_b
   t[2,2]  =  cos_a*cos_c + sin_a*sin_b*sin_c
   t[2,3]  = -cos_a*sin_c + sin_a*sin_b*cos_c
   t[3,1]  = -sin_b
   t[3,2]  =  cos_b*sin_c
   t[3,3]  =  cos_b*cos_c
END

GLOBAL DEF  mat_to_rot (t[,]:OUT, a:OUT, b:OUT, c:OUT)
   ;Conversion of a rotation matrix T into the angles A, B, C
   ;T = Rot_z(A) * Rot_y(B) * Rot_x(C)
   ;not made by me. This was in KEUWEG2 function
   REAL     t[,], a, b, c
   REAL     sin_a, cos_a, sin_b, abs_cos_b, sin_c, cos_c
   a = ARCTAN2(t[2,1], t[1,1])
   sin_a = SIN(a)    
   cos_a = COS(a) 
   sin_b = -t[3,1]
   abs_cos_b = cos_a*t[1,1] + sin_a*t[2,1]
   b = ARCTAN2(sin_b, abs_cos_b)
   sin_c =  sin_a*t[1,3] - cos_a*t[2,3]
   cos_c = -sin_a*t[1,2] + cos_a*t[2,2]
   c = ARCTAN2(sin_c, cos_c)
END

GLOBAL DEF mat_mult (a[,]:OUT,b[,]:OUT,c[,]:OUT)
   DECL REAL a[,], b[,], c[,]
   DECL INT i, j
   ;multiply two 3x3 matrices
   FOR i = 1 TO 3
      FOR j = 1 TO 3
         a[i, j] = b[i,1]*c[1,j] + b[i,2]*c[2,j] + b[i,3]*c[3,j]
      ENDFOR
   ENDFOR
END

mat_to_rot, and rot_to_mat are used to convert between a 3x3 rotation matrix and A,B,C angles. I won't go into detail about rotation matrices, but they are fundamental for doing rotation math like this. I know this is a huge mouthful, and there are probably better ways, but I've never had any regrets adding this code to a global math library and throwing it on my robots just in case.

Answer 2

Is the arctan2-function from KUE_WEG like this?

    DEFFCT  REAL ARCTAN2 (Y: IN, X: IN)
; Arcustangens mit 2 Argumenten und Check, ob x und y numerisch 0 sind

REAL       X, Y

REAL       ATAN_EPS

ATAN_EPS = 0.00011

IF  (  (ABS(X) < ATAN_EPS)  AND  (ABS(Y) < ATAN_EPS)  )  THEN
    RETURN (0)
ELSE
    RETURN ( ATAN2(Y, X) )
ENDIF

ENDFCT

But i cannot found anything like mat_mult (final[,], rot_coeff[,], reverse[,]). Would be great if you can complete this. Big thanks to you