2. ABB可以通过四点法计算TCP（默认方向），

 

3. ABB机器人提供MToolTCPCalib p1,p2,p3,p4,tool1,max_err,mean_err;指令计算tcp，其中p1,p2,p3,p4为机器人示教时四个点的jointtarget（6个轴坐标值），计算结果赋值到tool1的坐标系

 

 

4. 计算TCP，通常使用矩阵运算，并结合*小二乘法得到TCP的xyz，以及对应误差。

 

5. 通过四点自动计算TCP方法。

 

6. 在完成四点示教后，四点构成一个球面，球心即为当前TCP所在wobj0下的坐标值xyz。

 

7. 故通过四点计算tcp，即通过四点计算该四点对应的球心。

 

8. 创建四点计算球心函数，返回值为pos类型（即球心的xyz）