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
Alles anzeigen