产品:50
联系我们
工件数据简介
一般情况下，我们建立的工件数据的工件安装形式、工装安装形式、运动单元名称、工件坐标系与wobj0是一致的。所以，在我们建立工件数据时，只需要确定用户坐标系的原点位置和坐标轴方向即可。ABB机器人默认工件数据wobj0的定义如下所示：
PERS wobjdata wobj0 := [FALSE, TRUE, "", [[0, 0, 0],[1, 0, 0, 0]],[[0, 0, 0],[1, 0, 0, 0]]];
自定义工件数据的建立
如下图所示，已知空间不在同一直线上的任意三点p1（x1,y1,z1）、p2 （x2,y2,z2）和p3（x3,y3,z3）的坐标。
从工件数据的定义可知，我们自定义的工件数据只需要确定两个方面的内容，分别是用户坐标系的原点位置和用户坐标系的坐标轴原点姿态（方向）。
1、 原点位置的确定
1、如上图所示，假设p1、p2点所确定的直线为L1为，我们从直线外一点p3向直线L1做垂线，其垂足为p4。
2、ABB机器人默认定义工件坐标系时，采用上述方法，即p1、p2所在的直线构成x轴方向，p3到p1、p2的垂线交点为坐标系原点，即p4（x,y,z）点。p3、p4点所在的直线为y轴方向，记为直线L2。
3、由空间直线方程式，我们可以把经过p1、p2的直线L1可以表述为公式一：
4、p1、p2所在的直线L1，垂直于p3、p4所在的直线L2 。由直线垂直向量关系可知：
把各个坐标代入可得公式二：
5、为了简便计算，我们假设：
联立公式一和公式二，可以得到如下矩阵方程式：
6、使用PAPID指令MatrixSolve A1,b1,x1 可以求解如上方程组，注意MatrixSolve指令后的数据为dnum类型，其代码如下所示：
FUNC pos cal_frame
VAR dnum a;
VAR dnum b;
VAR dnum c;
VAR dnum A1{3,3};
VAR dnum b1{3};
VAR dnum x1{3};
VAR pos pos_return;
VAR pose pose_return;
VAR num rz;
VAR num ry;
VAR num rx;
a:=NumToDnum((p1.trans.x-p2.trans.x));
b:=NumToDnum((p1.trans.y-p2.trans.y));
c:=NumToDnum((p1.trans.z-p2.trans.z));
A1:=[[a,b,c],[b,-a,0],[c,0,-a]];
b1{1}:=(a*NumToDnum(p3.trans.x)+b*NumToDnum(p3.trans.y)+c*NumToDnum(p3.trans.z));
b1{2}:=(NumToDnum(p1.trans.x)*b-NumToDnum(p1.trans.y)*a);
b1{3}:=(NumToDnum(p1.trans.x)*c-NumToDnum(p1.trans.z)*a);
MatrixSolve A1,b1,x1;
pos_return.x:=DnumToNum(x1{1});
pos_return.y:=DnumToNum(x1{2});
pos_return.z:=DnumToNum(x1{3});
RETURN pos_return;
ENDFUNC
2、坐标轴原点姿态的确定
1、通过上述步骤我们得到了原点p4的坐标（x,y,z），那么就可获取向量p4p1，单位化后得到ox，获取向量p4p3，单位化后得到oy。单位向量计算公式如下所示：
2、求单位向量oz，向量oz等于单位向量ox叉乘单位向量oy。
3、将旋转矩阵[ox,oy,oz]T转化为欧拉角。
4、利用OrientZYX函数将欧拉角转化为四元数。其代码如下所示：
PROC main()
p4.trans:=cal_frame
cal_frame
p4.rot:=OrientZYX(rz,ry,rx);
UserwobjTest.uframe
UserwobjTest.uframe
ENDPROC
PROC cal_frame
!A:Rot_z() , B:Rot_y() ,c: Rot_x()
VAR pos vpx;
VAR pos vpxy;
VAR pos vpy;
VAR pos vpz;
vpx:=px2-px1;
!获取向量x1x2
vpx:=vec_nor(vpx);
!单位化向量vpx
vpxy:=py-px1;
!获取向量x1y1
vpxy:=vec_nor(vpxy);
!单位化向量vpx
vpz:=vpx*vpxy;
!向量vpx叉乘向量vpxy
vpy:=vpz*vpx;
!向量vpz叉乘向量vpx
!将单位向量存入数组nrT
nrT{1,1}:=vpx.x;
nrT{1,2}:=vpy.x;
nrT{1,3}:=vpz.x;
nrT{2,1}:=vpx.y;
nrT{2,2}:=vpy.y;
nrT{2,3}:=vpz.y;
nrT{3,1}:=vpx.z;
nrT{3,2}:=vpy.z;
nrT{3,3}:=vpz.z;
MatrixToRpy nrT,a,b,c;
!调用旋转矩阵转欧拉角函数
ENDPROC
PROC MatrixToRpy(num nrT{*,*},INOUT num nrA,INOUT num nrB,INOUT num nrC)
!Transform Trafo-Matrix to RPY-Angle A, B, C
!T = Rot_z(A) * Rot_y(B) * Rot_x(C)
VAR num nrSinA;
VAR num nrCosA;
VAR num nrSinB;
VAR num nrAbsCosB;
VAR num nrSinC;
VAR num nrCosC;
nrA:=ATan2(nrT{2,1},nrT{1,1});
nrSinA:=Sin(nrA);
nrCosA:=Cos(nrA);
nrSinB:=-nrT{3,1};
nrAbsCosB:=nrCosA*nrT{1,1}+nrSinA*nrT{2,1};
!Value: -90 <:= B <:= +90 !!
nrB:=ATan2(nrSinB,nrAbsCosB);
nrSinC:=nrSinA*nrT{1,3}-nrCosA*nrT{2,3};
nrCosC:=-nrSinA*nrT{1,2}+nrCosA*nrT{2,2};
nrC:=ATan2(nrSinC,nrCosC);
ENDPROC
FUNC pos vec_nor(pos pos1)
VAR pos pos2;
pos2.x:=pos1.x/VectMagn(pos1);
pos2.y:=pos1.y/VectMagn(pos1);
pos2.z:=pos1.z/VectMagn(pos1);
RETURN pos2;
ENDFUNC
完整代码如下所示
MODULE UserWobjdata
!计算出来的工件数据
PERS wobjdata UserwobjTest:=[FALSE,TRUE,"",[[1159.71,-499.998,1100],[0.965931,0,0,0.258798]],[[0,0,0],[1,0,0,0]]];
VAR robtarget p1:=[[1221.60,-464.27,1100.00],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget p2:=[[1332.93,-400.00,1100.00],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget p3:=[[1059.72,-326.79,1100.00],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget p4:=[[1253.322,0,1100],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR num nrT{3,3}:=[[0,0,0],[0,0,0],[0,0,0]];
VAR num rx;
VAR num ry;
VAR num rz;
!用示教器创建的工件数据
TASK PERS wobjdata Workobject_1:=[FALSE,TRUE,"",[[1159.722,-500,1100],[0.9659258,0,0,0.258819143]],[[0,0,0],[1,0,0,0]]];
PROC main()
p4.trans:=cal_frame
cal_frame
p4.rot:=OrientZYX(rz,ry,rx);
UserwobjTest.uframe
UserwobjTest.uframe
ENDPROC
PROC cal_frame
!A:Rot_z() , B:Rot_y() ,c: Rot_x()
VAR pos vpx;
VAR pos vpxy;
VAR pos vpy;
VAR pos vpz;
vpx:=px2-px1;
!获取向量x1x2
vpx:=vec_nor(vpx);
!单位化向量vpx
vpxy:=py-px1;
!获取向量x1y1
vpxy:=vec_nor(vpxy);
!单位化向量vpx
vpz:=vpx*vpxy;
!向量vpx叉乘向量vpxy
vpy:=vpz*vpx;
!向量vpz叉乘向量vpx
!将单位向量存入数组nrT
nrT{1,1}:=vpx.x;
nrT{1,2}:=vpy.x;
nrT{1,3}:=vpz.x;
nrT{2,1}:=vpx.y;
nrT{2,2}:=vpy.y;
nrT{2,3}:=vpz.y;
nrT{3,1}:=vpx.z;
nrT{3,2}:=vpy.z;
nrT{3,3}:=vpz.z;
MatrixToRpy nrT,a,b,c;
!调用旋转矩阵转欧拉角函数
ENDPROC
PROC MatrixToRpy(num nrT{*,*},INOUT num nrA,INOUT num nrB,INOUT num nrC)
!Transform Trafo-Matrix to RPY-Angle A, B, C
!T = Rot_z(A) * Rot_y(B) * Rot_x(C)
VAR num nrSinA;
VAR num nrCosA;
VAR num nrSinB;
VAR num nrAbsCosB;
VAR num nrSinC;
VAR num nrCosC;
nrA:=ATan2(nrT{2,1},nrT{1,1});
nrSinA:=Sin(nrA);
nrCosA:=Cos(nrA);
nrSinB:=-nrT{3,1};
nrAbsCosB:=nrCosA*nrT{1,1}+nrSinA*nrT{2,1};
!Value: -90 <:= B <:= +90 !!
nrB:=ATan2(nrSinB,nrAbsCosB);
nrSinC:=nrSinA*nrT{1,3}-nrCosA*nrT{2,3};
nrCosC:=-nrSinA*nrT{1,2}+nrCosA*nrT{2,2};
nrC:=ATan2(nrSinC,nrCosC);
ENDPROC
FUNC pos vec_nor(pos pos1)
VAR pos pos2;
pos2.x:=pos1.x/VectMagn(pos1);
pos2.y:=pos1.y/VectMagn(pos1);
pos2.z:=pos1.z/VectMagn(pos1);
RETURN pos2;
ENDFUNC
FUNC pos cal_frame
VAR dnum a;
VAR dnum b;
VAR dnum c;
VAR dnum A1{3,3};
VAR dnum b1{3};
VAR dnum x1{3};
VAR pos pos_return;
VAR pose pose_return;
VAR num rz;
VAR num ry;
VAR num rx;
a:=NumToDnum((p1.trans.x-p2.trans.x));
b:=NumToDnum((p1.trans.y-p2.trans.y));
c:=NumToDnum((p1.trans.z-p2.trans.z));
A1:=[[a,b,c],[b,-a,0],[c,0,-a]];
b1{1}:=(a*NumToDnum(p3.trans.x)+b*NumToDnum(p3.trans.y)+c*NumToDnum(p3.trans.z));
b1{2}:=(NumToDnum(p1.trans.x)*b-NumToDnum(p1.trans.y)*a);
b1{3}:=(NumToDnum(p1.trans.x)*c-NumToDnum(p1.trans.z)*a);
MatrixSolve A1,b1,x1;
pos_return.x:=DnumToNum(x1{1});
pos_return.y:=DnumToNum(x1{2});
pos_return.z:=DnumToNum(x1{3});
RETURN pos_return;
ENDFUNC
ENDMODULE
了解更多ABB机器人