视觉一点法计算TCP

1. 假设相机已经与机器人做过标定，相机能直接给出对应特征点在机器人wobj0（世界坐标系）下的坐标，则可以利用当前特征点坐标和当前机器人tool0的笛卡尔坐标，直接获得当前TCP。

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函数实现