Beiträge von Tefysv

Hi, I have some questions and I appreciate all your help.

I need to make with the Robot combined movements and turns between the TOOL and BASE coordinate systems. I must always move in the x-direction with respect to the base, but turn around the Tool. The tool has 4 tools inside that are not in the center of Axis 6, so a constant tool change is required during programming. To carry out these movements, I calculated the rotations and translations based on multiplication of Homogeneous transformation matrices in Matlab and RoboDK and it works there, but I have not been able to implement this in KRL

I have attached some of the functions that I implemented but in the end I only get zero matrices in KUKA and I cannot debug them. Is this possible? (I know the program still has errors).

Is there any other way I can make a combined move? Movement in X and rotation in Y with respect to the base and also rotation with respect to Tool Z?

Thank you in advance for your help and information.

RPY_TO_MAT_HOMOG(POSE[,],P2.X,P2.Y,P2.Z,P2.A,P2.B,P2.C); Get Pose Matrix of actual position
RotationZ_matriX(ROTZ[,],30); 
TRANSLATION_MAT(TRANS_X[,],-dist, 0, 0); X
Multiply_MAT_4x4(MULT_1[,],POSE[,],ROTZ[,])
Multiply_MAT_4x4(RESULT[,],TRANS_X[,],MULT_1[,])
MATHOMOG_TO_XYZABC (RESULT[,],P1.X,P1.Y,P1.Z, P1.A, P1.B, P1.C)




DEF  RPY_TO_MAT_HOMOG (T[,]:OUT,X :IN,Y :IN,Z :IN,A :IN,B :IN,C :IN )
REAL T[,],X, Y, Z, 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[1,4]  =  X

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[2,4]  =  Y

T[3,1]  = -SIN_B
T[3,2]  =  COS_B*SIN_C
T[3,3]  =  COS_B*COS_C
T[3,4]  =  Z

T[4,1]  = 0
T[4,2]  = 0
T[4,3]  = 0
T[4,4]  = 1

END  

DEF  TRANSLATION_MAT (T[,]:OUT,X :IN,Y :IN,Z :IN)

REAL T[,],X, Y, Z

T[1,1]  =  1
T[1,2]  =  0
T[1,3]  =  0
T[1,4]  =  X

T[2,1]  =  0
T[2,2]  =  1
T[2,3]  =  0
T[2,4]  =  Y

T[3,1]  =  0
T[3,2]  =  0
T[3,3]  =  1
T[3,4]  =  Z

T[4,1]  = 0
T[4,2]  = 0
T[4,3]  = 0
T[4,4]  = 1

END

DEF  RotationX_matrix (T[,]: OUT,C :IN )

REAL T[,], C
REAL COS_C, SIN_C
COS_C=COS(C)
SIN_C=SIN(C)

T[1,1]  =  1
T[1,2]  =  0
T[1,3]  =  0
T[1,4]  =  0

T[2,1]  =  0
T[2,2]  =  COS_C
T[2,3]  = -SIN_C
T[2,4]  =  0

T[3,1]  =  0
T[3,2]  =  SIN_C
T[3,3]  =  COS_C
T[3,4]  =  0

T[4,1]  = 0
T[4,2]  = 0
T[4,3]  = 0
T[4,4]  = 1

END

DEF  RotationY_matrix (T[,]: OUT,B :IN )

REAL T[,], B
CHAR VAR
REAL  COS_B, SIN_B
COS_B=COS(B)
SIN_B=SIN(B)

T[1,1]  =  COS_B  
T[1,2]  =  0
T[1,3]  =  SIN_B
T[1,4]  =  0

T[2,1]  =  0
T[2,2]  =  1
T[2,3]  =  0
T[2,4]  =  0

T[3,1]  =  -SIN_B
T[3,2]  =  0
T[3,3]  =  COS_B
T[3,4]  =  0

T[4,1]  = 0
T[4,2]  = 0
T[4,3]  = 0
T[4,4]  = 1

END

DEF  RotationZ_matrix (T[,]: OUT,A :IN )

REAL T[,], A
CHAR VAR
REAL COS_A, SIN_A
COS_A=COS(A)
SIN_A=SIN(A)

T[1,1]  =  COS_A
T[1,2]  =  -SIN_A
T[1,3]  =  0
T[1,4]  =  0

T[2,1]  =  SIN_A
T[2,2]  =  COS_A
T[2,3]  =  0
T[2,4]  =  0

T[3,1]  =  0
T[3,2]  =  0
T[3,3]  =  1
T[3,4]  =  0

T[4,1]  = 0
T[4,2]  = 0
T[4,3]  = 0
T[4,4]  = 1

END

DEF  MATHOMOG_TO_XYZABC (T[,]:OUT, X:OUT, Y:OUT, Z:OUT,A:OUT, B:OUT, C:OUT)
; T = Rot_z(A) * Rot_y(B) * Rot_x(C)

REAL     T[,],X,Y,Z,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);       Hier: -90 <= B <= +90 !!

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)

X=T[1,4]
Y=T[2,4]
Z=T[3,4]

END

DEF  Multiply_MAT_4x4 (T[,]:OUT,T_in1 :OUT,T_in2 :OUT)
; Function to multiply two 4x4 matrix

REAL T[,],T_in1[,],T_in2[,]
INT i,j,k
i=1
j=1
k=1

FOR  i=1 to 4 step 1   
   FOR j=1 to 4 step 1 

I appreciate all your comments,

Finally I managed to make the continuous movement by making an Array in which I saved the desired values of the relative movement in this case the number of points of the loop and then I execute the saved points, so I understand that the Robot already has data to perform the movement.

Now I will open a new case for other doubts. Additionally, only with $TOOL I didn't achieve the tool change during execution but with $ACT_TOOL.

Thanks to everyone!

Zitat von panic mode

Any non-zero value will allow approximated motion, including $ADVANCE=1 but... there may be other things in program that advance run pointer pause...

But i have few questions:......

Hi,

thanks for the clarification of $Advance

what I'm trying to move is within functions, actually the program is like this. I publish here in summary what I try to do and I don't know if the jump to the function and the mix with For can be the problem now that I understand that $Advance and Bas() are not the cause.

I appreciate your questions, and here I explain what I can understand:

1. I calculate in a loop over and over again because if I send the robot to rotate in tool 250 degrees around Z. It arrives at the end point rotated 250 degrees but does not maintain the orientation. And by doing it in small steps it does.

2. As you see in the code I need both tool and base movements. For example move the robot in X together with rotation in B relative to Base and rotate with Tool in Z. And I can't find any other way than to combine both movements in a step-by-step loop.

3. I think the previous two points answer this. I don't really plan on stopping between these points, it's a sequence.

I apologize if something doesn't make sense, I appreciate your answers.

DEF Wicklung( )
DECL INT i,N_Punkte
DECL REAL dist

;FOLD INI;%{PE}
;ENDFOLD (INI)  

;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.42,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME 
;ENDFOLD

;FOLD INI 
$VEL_AXIS[1] = 100
$VEL_AXIS[2] = 100
$VEL_AXIS[3] = 100
$VEL_AXIS[4] = 100
$VEL_AXIS[5] = 100
$VEL_AXIS[6] = 100



$ACC_AXIS[1] = 100
$ACC_AXIS[2] = 100
$ACC_AXIS[3] = 100
$ACC_AXIS[4] = 100
$ACC_AXIS[5] = 100
$ACC_AXIS[6] = 100


$VEL.ORI1 = 100
$VEL.ORI2 = 100
$ACC.CP = 1.0
$ACC.ORI1 = 100
$ACC.ORI2 = 100


$APO.CDIS=40
$APO.CPTP=40
;ENDFOLD
;===================================================================
;...Pneumatik
;HALT

;FOLD PTP P1 CONT Vel=100 % PDAT1 Tool[5] Base[0];%{PE}%R 8.3.42,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT1
$BWDSTART=FALSE
PDAT_ACT=PPDAT1
FDAT_ACT=FP1
BAS(#PTP_PARAMS,100)
PTP XP1 C_PTP
;ENDFOLD


;FOLD WAIT Time=1 sec;%{PE}%R 8.3.42,%MKUKATPBASIS,%CWAIT,%VWAIT,%P 3:1
WAIT SEC 1
;ENDFOLD
PTP $POS_ACT
;Pneumatik

$ADVANCE=1
 dist=5
 N_Punkte=4;
 HALT
;---------Rotate Tool
For i=1 to N_Punkte step 1    
   Rotate_Tool(dist,0,0,180, 0 , 0 , N_Punkte)
ENDFOR
;-------Rotate Tool and base 
$TOOL = TOOL_DATA[6]
BAS(#TOOL,6)
   
For i=1 to N_Punkte step 1
   Rotate_Base(-dist,0 ,-50 , 0, -60 ,0 , N_Punkte)
   Rotate_Tool(0, 0,0,50, 0  ,0 , N_Punkte)  
ENDFOR
   Rotate_Tool(0, 0,0 ,15 ,0  ,0  , 1)    
HALT
;FOLD PTP HOME  Vel= 100 % DEFAULT;%{PE}%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART = FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS (#PTP_PARAMS,100 )
$H_POS=XHOME
PTP  XHOME
;ENDFOLD
END

DEF Rotate_Base(T_X :IN,T_Y :IN,T_Z :IN,R_A :IN,R_B :IN,R_C :IN, N_p: IN)
REAL T_X, T_Y, T_Z,R_A,R_B,R_C
INT N_p
decl Frame Point1
   Point1 = {x 0,y 0,z 0,a 0,b 0,c 0}
   Point1.x= T_X/N_p
   Point1.y= T_Y/N_p
   Point1.z= T_Z/N_p
   Point1.A= R_A/N_p
   Point1.B= R_B/N_p
   Point1.C=R_C/N_p   
   PTP_REL Point1 C_DIS #BASE    
END

DEF Rotate_Tool(T_X :IN,T_Y :IN,T_Z :IN,R_A :IN,R_B :IN,R_C :IN, N_p: IN)
REAL T_X, T_Y, T_Z,R_A,R_B,R_C
INT N_p
decl Frame Point1
   Point1 = {x 0,y 0,z 0,a 0,b 0,c 0}
   Point1.x= T_X/N_p
   Point1.y= T_Y/N_p
   Point1.z= T_Z/N_p
   Point1.A= R_A/N_p
   Point1.B= R_B/N_p
   Point1.C=R_C/N_p 
   PTP_REL Point1 C_DIS #TOOL    
END

Thank you for your answer,

I was suspicious of the $Advance command, but I wasn't sure, since I understand that $Advance=1 is necessary to perform all the calculations. I'll prove it with 3. Thank you very much.

I'm new on the forum, I made a mistake uploading the Thread, I'll delete this one now better. I'm sorry.

Thank you very much,

At first I only tried with $TOOL = TOOL_DATA[5], but when running the program the Robot did not make the move with that tool and kept selecting TOOL 1 without making the change. Maybe I don't know if $Advance has anything to do with it. That's why I found that with BAS(#TOOL,5) it activates the tool I want. I attach a summary of the code I try to implement. It possibly helps to understand my problem better. I appreciate your help and comments

$VEL_AXIS[1] = 100

$VEL_AXIS[2] = 100

$VEL_AXIS[3] = 100

$VEL_AXIS[4] = 100

$VEL_AXIS[5] = 100

$VEL_AXIS[6] = 100

$ACC_AXIS[1] = 100

$ACC_AXIS[2] = 100

$ACC_AXIS[3] = 100

$ACC_AXIS[4] = 100

$ACC_AXIS[5] = 100

$ACC_AXIS[6] = 100

$VEL.ORI1 = 100

$VEL.ORI2 = 100

$ACC.CP = 1.0

$ACC.ORI1 = 100

$ACC.ORI2 = 100

$APO.CDIS=30

$APO.CPTP=30

;FOLD PTP P1 CONT Vel=100 % PDAT1 Tool[5] Base[0];%{PE}%R 8.3.42,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT1

$BWDSTART=FALSE

PDAT_ACT=PPDAT1

FDAT_ACT=FP1

BAS(#PTP_PARAMS,100)

PTP XP1 C_PTP

;ENDFOLD

$ADVANCE=1

For i=1 to N_Punkte step 1

Point1 = {x 0,y 0,z 0,a 0,b 0,c 0}

Point1.A= R_A/Num_Points

Point1.B= R_B/Num_Points

Point1.C=R_C/Num_Points

PTP_REL Point1 C_DIS #BASE

ENDFOR

$TOOL = TOOL_DATA[6]

BAS(#TOOL,6)

For i=1 to N_Punkte step 1

Point1 = {x 0,y 0,z 0,a 0,b 0,c 0}

Point1.x= Trans_X/Num_Points

Point1.y= Trans_Y/Num_Points

Point1.z= Trans_Z/Num_Points

PTP_REL Point1 C_DIS #BASE

ENDFOR

Hi,

first I perform a movement to a defined point, and then I make a loop to generate a sequence of points. And I can see that the robot moves step by step with a pause at each point, I don't know if the relative movement affects or if the $advance has something to do with it too. I appreciate your help.

Help.txt.

Hi,

I am currently developing a program with a Kuka Robot, in which I constantly change tools with these two commands:

$TOOL = TOOL_DATA[5]

BAS(#TOOL,5)

then I make movement with PTP_REL, however the movement is not continuous like when you move the robot from one PTP to another. I have noticed that a PTP is defined with BAS(#PTP_PARAMS,100), and there the motion conditions are defined. But since it is not a PTP but PTP_REL, I don't know how I can define the motion conditions to always have a continuous motion and not a step-by-step one.

Greetings and thank you very much for your answers

Hi everybody,

I am currently developing a program with a Kuka Robot, in which I constantly change tools with these two commands:

$TOOL = TOOL_DATA[5]

BAS(#TOOL,5)

then I make movement with PTP_REL, however the movement is not continuous like when you move the robot from one PTP to another. I have noticed that a PTP is defined with BAS(#PTP_PARAMS,100), and there the motion conditions are defined. But since it is not a PTP but PTP_REL, I don't know how I can define the motion conditions to always have a continuous motion and not a step-by-step one.

Greetings and thank you very much for your answers