1.5.2 PDispOn用法及原理实现

如图1-69所示，已经在左侧产品完成了轨迹示教（轨迹移动指令使用wobj0）。为方便演示，轨迹起点处（Target_10），工具TCP的y方向与产品短边平行，工具TCP的x方向与产品的长边平行，工具TCP的z方向垂直于产品表面。此时，另一个同样的产品被摆放到右边。由于产品相同，理论只需要示教右边产品的起点即可完成整个右边轨迹。

针对图1-69的情况，可以使用PDispOn指令。由于轨迹涉及旋转，需保证右侧示教的起点（Target_10_New）姿态和右侧产品关系与左侧起点（Target_10）姿态和左侧产品关系一致。例如，左侧TCP的y方向与左侧产品短边平行，工具TCP的x方向与左侧产品的长边平行，工具TCP的z方向垂直于左侧产品表面；右侧的起点的TCP的y方向与右侧产品短边平行，工具TCP的x方向与右侧产品的长边平行，工具TCP的z方向垂直于右侧产品表面：

PDispOn [\Rot] [\ExeP:=ExePoint] ProgPoint Tool;

（1）ProgPoint为旧轨迹的起点，如图1-69中的左侧起点Target_10。

（2）Tool为使用的工具坐标系。

（3）可选参数ExeP表示新轨迹的起点，即指令PDispOn会计算ProgPoint到ExePoint的转化关系并存入后续使用的位移坐标系。

（4）若不选ExeP参数，新轨迹的点默认使用机器人当前停留的点。

（5）若选可选参数Rot，则计算ProgPoint到ExePoint的平移和旋转关系，否则只计算ProgPoint到ExePoint的平移关系。

针对图1-69，示教完左侧轨迹，并且示教完右侧起点Target_10_New，可以使用如下代码完成左右轨迹：

对于“MoveL Target_100，v1000，fine，MyTool”，机器人在执行时会先将坐标系wobj1下的点Target_100转化到wobj0下，然后做轨迹插补和运动学逆解（具体解释见第2章），即转到wobj0下的p100的位姿表达式：

若使用了PDispSet或者PDispOn指令后，机器人在工件坐标系的oframe上右乘了一个DispCoordinate，则Target_100转化到wobj0下的位姿表达式：

也可用上式去验证1.5.1节中的PDispSet功能：

执行完以上代码第一行后，机器人走到图1-69中的左侧起点位置。当执行完第二和第三行后，发现机器人走到图1-69中的右侧起点位置。而第一行和第三行的代码是一样的。第三行代码的Target_10转化到wobj0下的表达式，如式（1-35）。实质上第三行代码机器人走到的位置就是Target_10_New，所以可以得到式（1-36）。其中dispFrame就是要求的位移坐标系：

根据式（1-36），编写如下代码，计算得到位移坐标系pose数据。

RAPID编程提供了progdisp数据类型用来存储位移坐标系，提供系统数据C_PROGDISP获取当前使用的位移坐标系。运行自行编写的cal_disp函数，计算结果与系统数据C_PROGDISP返回的结果一致，可知式（1-36）正确：

根据以上代码，整理后可以编写自定义指令PDispOn1（PDispOn指令的原理实现），后续参数和实现功能与系统提供的PDispOn指令完全相同：

“PDispOn\Rot\ExeP：=Target_10_New，Target_10，MyTool”指令实质就是希望构建一个dispFrame坐标系，使得在dispFrame坐标系下的Target_10转化到wobj0下就是Target_10_New位姿。由于原始轨迹path_10中的其他点与Target_10参考的坐标系相同，那这些点在位移dispFrame坐标系下的点若转化到wobj0下则与新产品的特征点刚好对齐，如图1-70所示。