——————————————————————————————————————
标准DH方法
——————————————————————————————————————
FUNCTION FUN_standard_DH_method : ARRAY[0..3,0..3] OF LREAL
VAR_INPUT
theta:LREAL;
d:LREAL;
a:LREAL;
alpha:LREAL;
END_VAR
VAR
END_VAR
FUN_standard_DH_method[0,0]:=COS(theta);
FUN_standard_DH_method[0,1]:=-SIN(theta) * COS(alpha);
FUN_standard_DH_method[0,2]:=SIN(theta) * SIN(alpha);
FUN_standard_DH_method[0,3]:=a * COS(theta);
FUN_standard_DH_method[1,0]:=SIN(theta);
FUN_standard_DH_method[1,1]:=COS(theta) * COS(alpha);
FUN_standard_DH_method[1,2]:=-COS(theta) * SIN(alpha);
FUN_standard_DH_method[1,3]:=a * SIN(theta);
FUN_standard_DH_method[2,0]:=0;
FUN_standard_DH_method[2,1]:=SIN(alpha);
FUN_standard_DH_method[2,2]:=COS(alpha);
FUN_standard_DH_method[2,3]:=d;
FUN_standard_DH_method[3,0]:=0;
FUN_standard_DH_method[3,1]:=0;
FUN_standard_DH_method[3,2]:=0;
FUN_standard_DH_method[3,3]:=1;
——————————————————————————————————————
矩阵相乘
——————————————————————————————————————
FUNCTION FUN_matrix_44_MUL : ARRAY[0..3,0..3] OF LREAL
VAR_INPUT
matrix1:ARRAY[0..3,0..3] OF LREAL;
matrix2:ARRAY[0..3,0..3] OF LREAL;
END_VAR
VAR
i:INT;
j:INT;
END_VAR
FOR i:=0 TO 3 BY 1 DO
FOR j:=0 TO 3 BY 1 DO
FUN_matrix_44_MUL[i,j]:=matrix1[i,0] * matrix2[0,j] +
matrix1[i,1] * matrix2[1,j] +
matrix1[i,2] * matrix2[2,j] +
matrix1[i,3] * matrix2[3,j];
END_FOR
END_FOR
——————————————————————————————————————
旋转矩阵变换RPY角
——————————————————————————————————————
FUNCTION FUN_matrix_33_to_RPY : BOOL
VAR_INPUT
matrix:ARRAY[0..3,0..3] OF LREAL;
END_VAR
VAR_IN_OUT
alpha:LREAL;
bata:LREAL;
gama:LREAL;
END_VAR
VAR
END_VAR
bata:=SM3M.SMC_atan2(SQRT(EXPT(matrix[2,0],2) + EXPT(matrix[2,1],2)), matrix[2,2]);
alpha:=SM3M.SMC_atan2(matrix[1,2] / SIN(bata), matrix[0,2] / SIN(bata));
gama:=SM3M.SMC_atan2(matrix[2,1] / SIN(bata), -matrix[2,0] / SIN(bata));
——————————————————————————————————————
程序
——————————————————————————————————————
VAR_GLOBAL
theta:ARRAY[1..6] OF LREAL;
d:ARRAY[1..6] OF LREAL;
a:ARRAY[1..6] OF LREAL;
alpha:ARRAY[1..6] OF LREAL;
offset:ARRAY[1..6] OF LREAL;
matrix:ARRAY [1..6] OF ARRAY [0..3,0..3] OF LREAL;
endMatrix:ARRAY [0..3,0..3] OF LREAL;
END_VAR
PROGRAM PLC_PRG
VAR
i:INT;
x:LREAL;
y:LREAL;
z:LREAL;
rx:LREAL;
ry:LREAL;
rz:LREAL;
END_VAR
P_axisGroup();
theta[1]:=0+SM_Drive_Virtual_1.fActPosition/180*SM3M.SMC_PI;
theta[2]:=SM3M.SMC_PI/2+SM_Drive_Virtual_2.fActPosition/180*SM3M.SMC_PI;
theta[3]:=0+SM_Drive_Virtual_3.fActPosition/180*SM3M.SMC_PI;
theta[4]:=0+SM_Drive_Virtual_4.fActPosition/180*SM3M.SMC_PI;
theta[5]:=0+SM_Drive_Virtual_5.fActPosition/180*SM3M.SMC_PI;
theta[6]:=0+SM_Drive_Virtual_6.fActPosition/180*SM3M.SMC_PI;
alpha[1]:=SM3M.SMC_PI/2;
alpha[2]:=0;
alpha[3]:=SM3M.SMC_PI/2;
alpha[4]:=SM3M.SMC_PI/2;
alpha[5]:=-SM3M.SMC_PI/2;
alpha[6]:=0;
d[1]:=80;
d[2]:=0;
d[3]:=20;
d[4]:=70;
d[5]:=0;
d[6]:=10;
a[1]:=10;
a[2]:=100;
a[3]:=30;
a[4]:=0;
a[5]:=0;
a[6]:=0;
FOR i:=1 TO 6 BY 1 DO
matrix[i]:=FUN_standard_DH_method(theta:=theta[i] ,d:=d[i] ,a:=a[i] ,alpha:=alpha[i]);
END_FOR
endMatrix:=matrix[1];
FOR i:=2 TO 6 BY 1 DO
endMatrix:=FUN_matrix_44_MUL(endMatrix,matrix[i]);
END_FOR
x:=endMatrix[0,3];
y:=endMatrix[1,3];
z:=endMatrix[2,3];
FUN_matrix_33_to_RPY(matrix:=endMatrix , alpha:=rx , bata:=ry , gama:=rz );
rx:=rx/SM3M.SMC_PI*180;
ry:=ry/SM3M.SMC_PI*180;
rz:=rz/SM3M.SMC_PI*180;
——————————————————————————————————————
机器人轴组程序查看之前的文章
——————————————————————————————————————