2. Pcam=PTool0*TCP.tframe

其中，Pcam为当前工具在机器人世界坐标系下的值x100,y100,z100,a100,b100,c100,

PTool0为当前tool0在机器人世界坐标系下的值x0,y0,z0,a0,b0,c0,

TCP.tframe为待计算的TCP坐标系xt,yt,zt,at,bt,ct.

由于Pcam（由相机提供数据，对于平面相机，可以事先固定Z和RX,RY，仅使用相机提供的X,Y和THETA）和PTool0已知，则

PTool0-1*Pcam= PTool0-1*PTool0*TCP.tframe  （两边左乘PTool0矩阵的逆矩阵）

整理得到：

TCP.tframe= PTool0-1*Pcam

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];      !相机提供当前工具在机器人坐标系下的值       pose100.rot:=p100.rot;       pose100.rot:=orientzyx(rz,ry,rx);       !相机提供当前工具在机器人坐标系下的欧拉角       Ptmp:=crobt(Tool:=tool0Wobj:=wobj0);       !获取当前位置的TOOL0对应的机器人位置       pose200.trans:= Ptmp.trans;       pose200.rot:= Ptmp.rot;       tCal.tframe:=PoseMult(PoseInv(pose200),pose100);      !计算获得TCP       stop;