FANUC（发那科）抓取样例程序¶
本章将介绍用于完成简单识别并移动的机器人样例程序。
前提¶
已参照 FANUC（发那科）标准接口通信配置 完成标准接口通信配置。
已参照 FANUC（发那科）标定操作流程 完成外参标定。
Mech-Vision 和 Mech-Viz 工程已搭建，并已勾选 自动加载当前工程 。
机器人工具中心点已定义好。
调低机器人运行速度，以避免调试过程中发生碰撞。
使用 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 ;
程序逻辑解读¶
移动机器人至 HOME 点。
移动到视觉拍照位置。
调用 MM_INIT_SKT 对通信进行初始化设置。
如需设置 Mech-Vision 工程配方，可在 MM_SET_MOD 设置添加 Mech-Vision 工程配方。
MM_START_VIS 触发 Mech-Vision 工程运行。
等待 1 秒。Eye-To-Hand 模式下，若在 MM_START_VIS 和 MM_GET_VIS 之间有移动步骤，则不需要在此设置等待；Eye-In-Hand 模式下则必须等待一秒，在相机拍照完成之前保持相机静止。
获取 Mech-Vision 工程计算出的位姿。
判断获取位姿的状态码是否正常，若状态码返回异常，机器人程序将暂停，用户需要进行处理。
移动到抓取点。
移动到抓取点和放置点之间的安全区域。
移动到固定的放置点。
用户自定义部分¶
定义机器人 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 进行视觉引导抓取，然后放置到固定的放置点。
移动机器人至 HOME 点。
移动到视觉拍照位置。
调用 MM_INIT_SKT 对通信进行初始化设置。
如需设置 Mech-Vision 工程配方，可以在这里调用 MM_SET_MOD 设置 Mech-Vision 工程使用配方。
触发 Mech-Viz 工程运行。
获取 Mech-Viz 工程规划的移动路径。
判断状态码是否正常，若状态码返回异常，机器人程序将暂停，用户需要进行处理。
将获取的点位依次放到 PR[60]，PR[61] 和 PR[62] 中。
使用运动指令使机器人沿 Mech-Viz 规划路径移动，完成视觉抓取并离开视觉区域。
移动到抓取点和放置点之间的中间区域。
移动到固定放置点进行放置。
用户自定义部分¶
定义机器人 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 “路径规划”步骤获取规划路径，从而进行抓取，然后放置到固定的放置点。
移动机器人至 HOME 点。
移动到视觉拍照位置。
调用 MM_INIT_SKT 对通信进行初始化设置。
如需设置 Mech-Vision 工程配方，可以调用 MM_SET_MOD 设置 Mech-Vision 工程所使用的配方。
MM_START_VIS 触发 Mech-Vision 工程运行。
MM_GET_VISP 获取 Mech-Vision “路径规划”步骤输出的移动路径。
判断状态码是否正常，若状态码返回异常，机器人程序将暂停，用户需要进行处理。
根据规划的路径点数量，使用循环将获取的路径点位姿从 PR[60] 开始依次存放至位置寄存器。
使用运动指令使机器人沿规划路径移动，完成视觉抓取并离开视觉区域。
移动到抓取点和放置点之间的中间区域。
移动到固定放置点进行放置。
用户自定义部分¶
定义机器人 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] 位姿变量中。定义夹具动作
在移动到抓取点和放置点时，需添加夹具控制逻辑用来开关夹具。