abb四点法做tcp_【ABB】四点计算工具TCP实现代码

1.TCP(toolcenter point)机器人工具中心点。该坐标系非常重要，ABB机器人通常走点位都以TCP为基准。

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)

FUNC pos cal_ball_center(robtarget p1,robtarget p2,robtarget p3,robtarget p4)

VAR pos pos1;

var num x1;

var num x2;

var num x3;

var num x4;

var num y1;

var num y2;

var num y3;

var num y4;

var num z1;

var num z2