FANUC(发那科)抓取样例程序

本章将介绍用于完成简单识别并移动的机器人样例程序。

前提

  1. 已参照 FANUC(发那科)标准接口通信配置 完成标准接口通信配置。

  2. 已参照 FANUC(发那科)标定操作流程 完成外参标定。

  3. Mech-Vision 和 Mech-Viz 工程已搭建,并已勾选 自动加载当前工程

  4. 机器人工具中心点已定义好。

  5. 调低机器人运行速度,以避免调试过程中发生碰撞。

样例程序介绍

机器人简单抓取和放置的样例程序位于梅卡曼德系统软件安装目录下 Mech-Center/Robot_Interface/FANUC/sample 文件夹内。

按照使用方式分为三类:

使用 Mech-Vision 进行视觉识别并获取结果

 1:  !FUNCTION:Eye to Hand simple pick ;
 2:  !MechMind,2022-05-30 ;
 3:   ;
 4:  !SET Tool ;
 5:  UTOOL_NUM=1 ;
 6:  !Move to HOME Position ;
 7:J P[1] 100% FINE    ;
 8:  !Move to Camera capture Position ;
 9:L P[2] 3000mm/sec FINE    ;
10:  !Set IP address and Port ;
11:  CALL MM_INIT_SKT('8','192.168.1.20',50000,1) ;
12:  WAIT    .10(sec) ;
13:  !Set Vision Recipe ;
14:  //CALL MM_SET_MOD(1,1) ;
15:  !Run Vision Project ;
16:  CALL MM_START_VIS(1,1,2,10) ;
17:  WAIT   1.00(sec) ;
18:  CALL MM_GET_VIS(1,50,51,52) ;
19:  IF (R[52]<>1100) THEN ;
20:  PAUSE ;
21:  ENDIF ;
22:  CALL MM_GET_POS(1,60,70,80) ;
23:L PR[60] 800mm/sec CNT100 Offset,PR[1]    ;
24:L PR[60] 800mm/sec FINE    ;
25:  !Add object grasping logic here ;
26:   ;
27:L PR[60] 800mm/sec CNT100 Offset,PR[1]    ;
28:  !Add transition point ;
29:L P[3] 800mm/sec FINE    ;
30:  !Move to DROP Position ;
31:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
32:L P[4] 200mm/sec FINE    ;
33:  !Add object releasing logic here ;
34:   ;
35:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
36:  !Move to HOME Position ;
37:J P[1] 100% FINE    ;

程序逻辑解读

  1. 移动机器人至 HOME 点。

  2. 移动到视觉拍照位置。

  3. 调用 MM_INIT_SKT 对通信进行初始化设置。

  4. 如需设置 Mech-Vision 工程配方,可在 MM_SET_MOD 设置添加 Mech-Vision 工程配方。

  5. MM_START_VIS 触发 Mech-Vision 工程运行。

  6. 等待 1 秒。Eye-To-Hand 模式下,若在 MM_START_VIS 和 MM_GET_VIS 之间有移动步骤,则不需要在此设置等待;Eye-In-Hand 模式下则必须等待一秒,在相机拍照完成之前保持相机静止。

  7. 获取 Mech-Vision 工程计算出的位姿。

  8. 判断获取位姿的状态码是否正常,若状态码返回异常,机器人程序将暂停,用户需要进行处理。

  9. 移动到抓取点。

  10. 移动到抓取点和放置点之间的安全区域。

  11. 移动到固定的放置点。

用户自定义部分

  • 定义机器人 HOME 点

    J P[1] 100% FINE:用户需将 HOME 点位置记录在 P[1] 位姿变量中。

  • 定义工具中心点 TCP

    UTOOL_NUM=1:用户需将调用坐标系改为存储了实际 TCP 值的坐标系。用户可以通过更改工具坐标系 1 的值调整 TCP 位置。

  • 示教拍照点

    L P[2] 1000mm/sec FINE:用户需将拍照点位置记录在 P[2] 位姿变量中。

  • 示教过渡点

    L P[3] 800mm/sec FINE:用户可添加一个或几个中间点,使机器人从抓取点移动至放置点过程中,无碰撞。

  • 示教放置点

    L P[4] 200mm/sec FINE:用户需将放置位置记录在 P[4] 位姿变量中。

  • 定义抓取前后,放置前后的接近距离

    • 接近抓取点

      L PR[60] 800mm/sec CNT100 Offset,PR[1]:移动到抓取点上方,偏差设置在 PR[1] 里面,用户需要修改偏移量,保证接近过程无碰撞。

    • 离开抓取点

      L PR[60] 800mm/sec CNT100 Offset,PR[1]:从抓取点沿工具 Z 轴方向向上移动,偏差也设置在 PR[1] 里面,用户需要修改偏移量,保证离开过程无碰撞。

    • 接近放置点

      L P[4] 800mm/sec CNT100 Offset,PR[2]:移动到放置点上方,用户需要修改偏移量,偏差设置在 PR[2] 里面,保证接近过程无碰撞。

    • 离开放置点

      L P[4] 800mm/sec CNT100 Offset,PR[2]:从放置点沿工具 Z 轴方向向上移动,偏差也设置在 PR[2] 里面,用户需要修改偏移量,保证离开过程无碰撞。

  • 定义夹具动作

    在移动到抓取点和放置点时,需添加夹具控制逻辑用来开关夹具。

使用 Mech-Viz 规划路径

 1:  !FUNCTION:Eye to Hand simple pick ;
 2:  !and place with Mech-Viz ;
 3:  !MechMind,2022-05-30 ;
 4:   ;
 5:  !SET Tool ;
 6:  UTOOL_NUM=1 ;
 7:  !Move to HOME Position ;
 8:J P[1] 100% FINE    ;
 9:  !Move to Camera capture Position ;
10:L P[2] 3000mm/sec FINE    ;
11:  !Set IP address and Port ;
12:  CALL MM_INIT_SKT('8','192.168.1.20',50000,1) ;
13:  WAIT    .10(sec) ;
14:  !Set Vision Recipe ;
15:  //CALL MM_SET_MOD(1,1) ;
16:  !Run Viz Project ;
17:  CALL MM_START_VIZ(1,10) ;
18:  WAIT    .10(sec) ;
19:  !set branch exitport ;
20:  //CALL MM_SET_BCH(1,1) ;
21:  !get planned path ;
22:  CALL MM_GET_VIZ(2,50,51,52,53) ;
23:  IF (R[53]<>2100) THEN ;
24:  PAUSE ;
25:  ENDIF ;
26:  CALL MM_GET_POS(1,60,70,80) ;
27:  CALL MM_GET_POS(2,61,71,81) ;
28:  CALL MM_GET_POS(3,62,72,82) ;
29:  !follow the planned path to pick ;
30:L PR[60] R[80]mm/sec FINE    ;
31:L PR[61] R[81]mm/sec FINE    ;
32:  !Add object grasping logic here ;
33:   ;
34:L PR[62] R[82]mm/sec FINE    ;
35:  !Add transition point ;
36:L P[3] 800mm/sec FINE    ;
37:  !Move to DROP Position ;
38:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
39:L P[4] 200mm/sec FINE    ;
40:  !Add object releasing logic here ;
41:   ;
42:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
43:  !Move to HOME Position ;
44:J P[1] 100% FINE    ;

程序逻辑解读

此样例使用 Mech-Viz 进行视觉引导抓取,然后放置到固定的放置点。

  1. 移动机器人至 HOME 点。

  2. 移动到视觉拍照位置。

  3. 调用 MM_INIT_SKT 对通信进行初始化设置。

  4. 如需设置 Mech-Vision 工程配方,可以在这里调用 MM_SET_MOD 设置 Mech-Vision 工程使用配方。

  5. 触发 Mech-Viz 工程运行。

  6. 获取 Mech-Viz 工程规划的移动路径。

  7. 判断状态码是否正常,若状态码返回异常,机器人程序将暂停,用户需要进行处理。

  8. 将获取的点位依次放到 PR[60],PR[61] 和 PR[62] 中。

  9. 使用运动指令使机器人沿 Mech-Viz 规划路径移动,完成视觉抓取并离开视觉区域。

  10. 移动到抓取点和放置点之间的中间区域。

  11. 移动到固定放置点进行放置。

用户自定义部分

  • 定义机器人 HOME 点

    J P[1] 100% FINE:用户需将 HOME 点位置记录在 P[1] 位姿变量中。

  • 定义工具中心点 TCP

    UTOOL_NUM=1:用户需将调用坐标系改为存储了实际 TCP 值的坐标系。用户可以通过更改工具坐标系 1 的值调整 TCP 位置。

  • 示教拍照点

    L P[2] 1000mm/sec FINE:用户需将拍照点位置记录在 P[2] 位姿变量中。

  • 示教过渡点

    L P[3] 800mm/sec FINE:用户可添加一个或几个中间点,使机器人从抓取点移动至放置点过程中,无碰撞。

  • 示教放置点

    L P[4] 200mm/sec FINE:用户需将放置位置记录在 P[4] 位姿变量中。

  • 定义夹具动作

    在移动到抓取点和放置点时,需添加夹具控制逻辑用来开关夹具。

使用 Mech-Vision “路径规划”步骤获取规划路径

 1:  !FUNCTION:Eye to Hand simple pick ;
 2:  !and place with Mech-Vision Path ;
 3:  !Planning Step ;
 4:  !MechMind,2023-01-10 ;
 5:   ;
 6:  !SET Tool ;
 7:  UTOOL_NUM=1 ;
 8:  !Move to HOME Position ;
 9:J P[1] 100% FINE    ;
10:  !Move to Camera capture Position ;
11:L P[2] 3000mm/sec FINE    ;
12:  !Set IP address and Port ;
13:  CALL MM_INIT_SKT('8','192.168.1.20',50000,1) ;
14:  WAIT    .10(sec) ;
15:  !Set Vision Recipe ;
16:  //CALL MM_SET_MOD(1,1) ;
17:  !Run Vision Project ;
18:  CALL MM_START_VIS(1,1,2,10) ;
19:  WAIT   1.00(sec) ;
20:  CALL MM_GET_VISP(1,2,50,51,52,53) ;
21:  IF (R[53:status]<>1103) THEN ;
22:  PAUSE ;
23:  ENDIF ;
24:  FOR R[100:i]=1 TO R[51:pos_num] ;
25:  R[101:preg no]=59+R[100:i]    ;
26:  R[102:lbl no]=69+R[100:i]    ;
27:  R[103:spd no]=79+R[100:i]    ;
28:  CALL MM_GET_POS(R[100:i],R[101:preg no],R[102:lbl no],R[103:spd no]) ;
29:  ENDFOR ;
30:  !follow the planned path to pick ;
31:  FOR R[110:j]=1 TO R[51:pos_num] ;
32:  R[111:preg]=59+R[110:j]    ;
33:  R[112:spd]=79+R[110:j]    ;
34:L PR[R[111]] R[R[112]]mm/sec FINE    ;
35:  IF (R[110:j]=R[52:vpos_num]) THEN ;
36:  PAUSE ;
37:  !Add object grasping logic here ;
38:  ENDIF ;
39:  ENDFOR ;
40:  !Add transition point ;
41:L P[3] 800mm/sec FINE    ;
42:  !Move to DROP Position ;
43:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
44:L P[5] 200mm/sec FINE    ;
45:  !Add object releasing logic here ;
46:   ;
47:L P[6] 800mm/sec CNT100 Offset,PR[2]    ;
48:  !Move to HOME Position ;
49:J P[7] 100% FINE    ;

程序逻辑解读

此样例使用 Mech-Vision “路径规划”步骤获取规划路径,从而进行抓取,然后放置到固定的放置点。

  1. 移动机器人至 HOME 点。

  2. 移动到视觉拍照位置。

  3. 调用 MM_INIT_SKT 对通信进行初始化设置。

  4. 如需设置 Mech-Vision 工程配方,可以调用 MM_SET_MOD 设置 Mech-Vision 工程所使用的配方。

  5. MM_START_VIS 触发 Mech-Vision 工程运行。

  6. MM_GET_VISP 获取 Mech-Vision “路径规划”步骤输出的移动路径。

  7. 判断状态码是否正常,若状态码返回异常,机器人程序将暂停,用户需要进行处理。

  8. 根据规划的路径点数量,使用循环将获取的路径点位姿从 PR[60] 开始依次存放至位置寄存器。

  9. 使用运动指令使机器人沿规划路径移动,完成视觉抓取并离开视觉区域。

  10. 移动到抓取点和放置点之间的中间区域。

  11. 移动到固定放置点进行放置。

用户自定义部分

  • 定义机器人 HOME 点

    J P[1] 100% FINE:用户需将 HOME 点位置记录在 P[1] 位姿变量中。

  • 定义工具中心点 TCP

    UTOOL_NUM=1:用户需将调用坐标系改为存储了实际 TCP 值的坐标系。用户可以通过更改工具坐标系 1 的值调整 TCP 位置。

  • 示教拍照点

    L P[2] 1000mm/sec FINE:用户需将拍照点位置记录在 P[2] 位姿变量中。

  • 示教过渡点

    L P[3] 800mm/sec FINE:用户可添加一个或几个中间点,使机器人从抓取点移动至放置点过程中,无碰撞。

  • 示教放置点

    L P[4] 200mm/sec FINE:用户需将放置位置记录在 P[4] 位姿变量中。

  • 定义夹具动作

    在移动到抓取点和放置点时,需添加夹具控制逻辑用来开关夹具。