【ABB】ABB机器人如何计算直线上到某点指定距离的点位及平移指令

（一）ABB机器人如何计算直线上到某点指定距离的点位

1. 已知点Curr和点Refer，点Curr和点Refer构成直线，方向由Curr指向Refer

2. 要求Result点距离Curr为指定距离，且Result在直线Curr_Refer上，如何计算Result点位置

3.  可以计算Curr到Refer的距离，根据要求的Result到Curr的距离和Curr到Refer距离的倍率，计算Result的位置

4. 代码如下：

FUNC robtarget GetTracePoint(robtarget Current,robtarget Refer,num Dist)

        !line direction: Current to Refer

        !Dist: distance between Current and Return position

        VAR num dis_tmp;

        VAR num dx;

        VAR num dy;

        VAR num dz;

        VAR num ratio;

        VAR robtarget pResult:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

        pResult:=Current;

        dx:=Refer.trans.x-Current.trans.x;

        dy:=Refer.trans.y-Current.trans.y;

        dz:=Refer.trans.z-Current.trans.z;

        dis_tmp:=distance(Current.trans,Refer.trans);

        ratio:=Dist/dis_tmp;

        pResult.trans.x:=Current.trans.x+ratio*dx;

        pResult.trans.y:=Current.trans.y+ratio*dy;

        pResult.trans.z:=Current.trans.z+ratio*dz;

        RETURN pResult;

    endfunc

（二）ABB机器人平移指令

1、现场机器人在wobj0下有轨迹path_0，现在希望实际运行的时候，轨迹能整体沿着wobj0的x和y方向各偏移30mm，如何最快速实现？

2. 安川机器人有平移指令SFTON，使用如下：

SFTON P003 RF

//基于机器人坐标系平移p003，p003中设置平移的xyz数值

//以下轨迹均基于RF平移p003

MOVL V=290.0

MOVL V=290.0

MOVL V=290.0

SFTOF

//关闭平移

3. 对应ABB机器人RAPID指令为：PDISPSET pose100，其中pose100为pose类型数据，包括trans（xyz）和rot（q1-q4）。Pdispset的偏移和旋转均基于当前使用坐标系，即对于当前坐标系的右乘。

 VAR posepose500:=[[0,0,0],[1,0,0,0]];

 PROC main()

        path_t1;

        !原始轨迹

        pose500.trans:=[30,30,0];

        !设置pose500数据的xyz

        PDispSet pose500;

        !以下轨迹基于当前轨迹使用的坐标系偏移pose500

        path_t1;

       PDispOff;

       !关闭偏移

  ENDPROC

  PROC path_t1()

        reg1:=100;

        MoveLp500,v100,fine,tWeldGunWObj:=wobj0;

        MoveLoffs(p500,reg1,0,0),v100,fine,tWeldGunWObj:=wobj0;

        MoveLoffs(p500,reg1,reg1,0),v100,fine,tWeldGunWObj:=wobj0;

        MoveLoffs(p500,0,reg1,0),v100,fine,tWeldGunWObj:=wobj0;

        MoveLp500,v100,fine,tWeldGunWObj:=wobj0;

ENDPROC

4. 若机器人轨迹参考wobj1，wobj1不与wobj0平行，则使用pdispset后，轨迹偏移如下图，即偏移参考wobj1的方向。