2. Pcam=PTool0*TCP.t
其中,Pcam为当前工具在机器人世界坐标系下的值x100,y100,z100,a100,b100,c100,
PTool0为当前tool0在机器人世界坐标系下的值x0,y0,z0,a0,b0,c0,
TCP.t
由于Pcam(由相机提供数据,对于平面相机,可以事先固定Z和RX,RY,仅使用相机提供的X,Y和THETA)和PTool0已知,则
PTool0-1*Pcam= PTool0-1*PTool0*TCP.t
整理得到:
TCP.t
Pose数据的相乘和求逆,可以使用ABB机器人PoseMult和PoseInv函数实现
3. 代码如下:
PROC TCP_CAL()
VAR pose pose100;
VAR pose pose200;
VAR num rx;
VAR num ry;
VAR num rz;
VARrobtarget ptmp;
VAR num xx;
VAR num yy;
VAR num zz;
pose100.trans:=[xx,yy,zz];
!相机提供当前工具在机器人坐标系下的值
=p100.rot; :
=orientzyx(rz,ry,rx); :
!相机提供当前工具在机器人坐标系下的欧拉角
=crobt(Tool:=tool0Wobj:=wobj0); :
!获取当前位置的TOOL0对应的机器人位置
= Ptmp.trans; :
= Ptmp.rot; :
=PoseMult(PoseInv(pose200),pose100); :
!计算获得TCP
stop;