样例程序19：MM_S19_Vis_PlanAllVision

您正在查看最新版本（V2.1.0）的文档。如果您想查阅其他版本的文档，可以点击页面右上角“切换版本”按钮进行切换。

■ 如果您不确定当前使用的产品是哪个版本，请随时联系梅卡曼德技术支持。

程序简介

功能说明

机器人先触发Mech-Vision工程运行，然后通过循环获取所有规划路径，进而通过循环进行抓取与放置操作。在本样例中，相机拍照一次，Mech-Vision会规划出全部视觉结果的抓取路径。通常用于一拍多抓的场景。

文件路径

Mech-Vision和Mech-Viz软件安装目录下Communication Component/Robot_Interface/FANUC/sample/MM_S19_Vis_PlanAllVision。

所需工程

Mech-Vision工程

该工程需满足以下配置：在路径规划工具的工作流程标签页下，全局配置步骤的规划全部视觉结果参数需处于打开状态。

使用前提

已完成标准接口通信配置。
已完成自动标定。

此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。

程序解读

以下为MM_S19_Vis_PlanAllVision样例程序的代码及相关解释说明。

与MM_S3_Vis_Path样例相比，本样例仅新增了通过循环获取所有规划路径并进行抓取与放置的功能（加粗部分的代码）。因此，下文不再重复解释与MM_S3_Vis_Path样例相同部分的代码（详情请参考MM_S3_Vis_Path样例说明）。

   1:  !-------------------------------- ;
   2:  !FUNCTION: trigger Mech-Vision ;
   3:  !project, plan all vision results ;
   4:  !and get all planned paths ;
   5:  !Mech-Mind, 2023-12-25 ;
   6:  !-------------------------------- ;
   7:   ;
   8:  !set current uframe NO. to 0 ;
   9:  UFRAME_NUM=0 ;
  10:  !set current tool NO. to 1 ;
  11:  UTOOL_NUM=1 ;
  12:  !move to robot home position ;
  13:J P[1] 100% FINE    ;
  14:  !initialize communication ;
  15:  !parameters(initialization is ;
  16:  !required only once) ;
  17:  CALL MM_INIT_SKT('8','127.0.0.1',50000,5) ;
  18:  LBL[1:recap] ;
  19:  !move to image-capturing position ;
  20:L P[2] 1000mm/sec FINE    ;
  21:  !trigger NO.1 Mech-Vision project ;
  22:  CALL MM_START_VIS(1,0,2,10) ;
  23:  !get planned path from NO.1 ;
  24:  !Mech-Vision project; 2nd ;
  25:  !argument (1) means getting pose ;
  26:  !in JPs ;
  27:  CALL MM_GET_VISP(1,1,51,52,53) ;
  28:  !check whether planned path has ;
  29:  !been got from Mech-Vision ;
  30:  !successfully ;
  31:  IF R[53]<>1103,JMP LBL[99] ;
  32:  !save all waypoint data to local ;
  33:  !variables using for-loop, a ;
  34:  !maximum of 50 points are support ;
  35:  !supported ;
  36:  FOR R[10]=1 TO R[51] ;
  37:  R[11]=59+R[10]    ;
  38:  R[12]=69+R[10]    ;
  39:  R[13]=99+R[10]    ;
  40:  CALL MM_GET_JPS(R[10],R[11],R[12],R[13]) ;
  41:  ENDFOR ;
  42:  !parse pick cycle count, here ;
  43:  !suppose 5 points per planned ;
  44:  !path ;
  45:  R[30]=R[51] DIV 5    ;
  46:  R[29]=R[51] MOD 5    ;
  47:  !check if parsed data is valid; ;
  48:  !if not, retry to get planned ;
  49:  !path or add some error handling ;
  50:  !logic ;
  51:  IF R[30]<1) OR (R[29]<>0 THEN ;
  52:  PAUSE ;
  53:  JMP LBL[1] ;
  54:  ENDIF ;
  55:  !repeatedly run pick-and-place ;
  56:  !cycle using for-loop ;
  57:  FOR R[10]=1 TO R[30] ;
  58:  R[20]=R[10]-1    ;
  59:  R[21]=R[20]*5    ;
  60:  R[31]=60+R[21]    ;
  61:  R[32]=61+R[21]    ;
  62:  R[33]=62+R[21]    ;
  63:  R[34]=63+R[21]    ;
  64:  R[35]=64+R[21]    ;
  65:  !follow the planned path to pick ;
  66:J PR[R[31]] 50% CNT100    ;
  67:J PR[R[32]] 50% FINE    ;
  68:J PR[R[33]] 10% FINE    ;
  69:  !add object grasping logic here, ;
  70:  !such as "DO[1]=ON" ;
  71:  PAUSE ;
  72:J PR[R[34]] 50% FINE    ;
  73:J PR[R[35]] 50% CNT100    ;
  74:  !move to intermediate waypoint ;
  75:  !of placing ;
  76:J P[3] 50% CNT100    ;
  77:  !move to approach waypoint ;
  78:  !of placing ;
  79:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;
  80:  !move to placing waypoint ;
  81:L P[4] 300mm/sec FINE    ;
  82:  !add object releasing logic here, ;
  83:  !such as "DO[1]=OFF" ;
  84:  PAUSE ;
  85:  !move to departure waypoint ;
  86:  !of placing ;
  87:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;
  88:  !move to intermediate waypoint ;
  89:  !of placing ;
  90:J P[3] 50% CNT100    ;
  91:  ENDFOR ;
  92:  !finish pick-and-place cycle, and ;
  93:  !jump back to camera capturing ;
  94:  JMP LBL[1] ;
  95:  END ;
  96:   ;
  97:  LBL[99:vision error] ;
  98:  !add error handling logic here ;
  99:  !according to different ;
 100:  !error codes ;
 101:  !e.g.: status=1003 means no ;
 102:  !point cloud in ROI ;
 103:  !e.g.: status=1002 means no ;
 104:  !vision results ;
 105:  PAUSE ;

上述样例程序代码对应的流程如下图所示。

下表为新增功能的逻辑解读。用户单击指令名称的超链接便可查看该指令的详细说明。

流程代码及说明

流程	代码及说明
通过循环转存规划路径	`32: !save all waypoint data to local ; 33: !variables using for-loop, a ; 34: !maximum of 50 points are support ; 35: !supported ; 36: FOR R[10]=1 TO R[51] ; 37: R[11]=59+R[10] ; 38: R[12]=69+R[10] ; 39: R[13]=99+R[10] ; 40: CALL MM_GET_JPS(R[10],R[11],R[12],R[13]) ; 41: ENDFOR ;` 第36行：FOR表示以下为循环程序片段；R[10]用于控制循环的次数（即R[10]从1开始，每次循环后自增1，直到大于R[51]，结束循环）；R[51]为MM_GET_VISP指令中第三个参数所表示的数值寄存器，该寄存器保存视觉系统返回的路径点个数。因此，R[10]可以表示当前路径点在规划路径中的编号。第37~39行：R[11]、R[12]、R[13]分别表示当前路径点对应的关节角、标签、末端工具编号所使用的寄存器号码。第40行：MM_GET_JPS指令表示将某个路径点的关节角、标签和末端工具编号分别转存到指定寄存器，因此整条指令表示将第R[10]编号的路径点的关节角、标签和末端工具编号依次转存到R[11]、R[12]和R[13]所表示的寄存器中。
计算R[30]和R[29]	`42: !parse pick cycle count, here ; 43: !suppose 5 points per planned ; 44: !path ; 45: R[30]=R[51] DIV 5 ; 46: R[29]=R[51] MOD 5 ;` 本样例假设每次规划的抓取路径包含5个路径点，“R[51] DIV 5”表示R[51]整除5的商，“R[51] MOD 5”表示R[51]整除5的余数。R[30]表示规划的总抓取次数。如果R[29]不为0，则表示某次规划的抓取路径点个数少于5，即路径规划出现异常，需重新进行规划。
判断规划路径是否出错	`47: !check if parsed data is valid; ; 48: !if not, retry to get planned ; 49: !path or add some error handling ; 50: !logic ; 51: IF ((R[30]<1) OR (R[29]<>0)) THEN ; 52: PAUSE ; 53: JMP LBL[1] ; 54: ENDIF ;` 如果抓取次数R[30]小于1，或R[29]不为0，则表示路径规划出现异常，用户需在此处添加处理逻辑，例如重新触发Mech-Vision工程并获取规划路径。
通过循环进行抓取和放置	55: !repeatedly run pick-and-place ; 56: !cycle using for-loop ; 57: FOR R[10]=1 TO R[30] ; 58: R[20]=R[10]-1 ; 59: R[21]=R[20]*5 ; 60: R[31]=60+R[21] ; 61: R[32]=61+R[21] ; 62: R[33]=62+R[21] ; 63: R[34]=63+R[21] ; 64: R[35]=64+R[21] ; 65: !follow the planned path to pick ; 66:J PR[R[31]] 50% CNT100 ; 67:J PR[R[32]] 50% FINE ; 68:J PR[R[33]] 10% FINE ; 69: !add object grasping logic here, ; 70: !such as "DO[1]=ON" ; 71: PAUSE ; 72:J PR[R[34]] 50% FINE ; 73:J PR[R[35]] 50% CNT100 ; 74: !move to intermediate waypoint ; 75: !of placing ; 76:J P[3] 50% CNT100 ; 77: !move to approach waypoint ; 78: !of placing ; 79:L P[4] 1000mm/sec FINE Tool_Offset,PR[2] ; 80: !move to placing waypoint ; 81:L P[4] 300mm/sec FINE ; 82: !add object releasing logic here, ; 83: !such as "DO[1]=OFF" ; 84: PAUSE ; 85: !move to departure waypoint ; 86: !of placing ; 87:L P[4] 1000mm/sec FINE Tool_Offset,PR[2] ; 88: !move to intermediate waypoint ; 89: !of placing ; 90:J P[3] 50% CNT100 ; 91: ENDFOR ; 上述代码表示，在FOR循环中，机器人移动到每次规划的5个路径点，从而完成抓取操作，然后接着执行放置操作。R[10]用于控制循环的次数，即R[10]从1开始，每次循环后自增1，直到大于抓取次数R[30]，结束循环。每当R[10]增加1，则R[21]增加5，60+R_{64+R[21]（即R[31]\}R[35]）表示每次规划的5个路径点将要存放的5个寄存器号码。

通过循环转存规划路径

  32:  !save all waypoint data to local ;
  33:  !variables using for-loop, a ;
  34:  !maximum of 50 points are support ;
  35:  !supported ;
  36:  FOR R[10]=1 TO R[51] ;
  37:  R[11]=59+R[10]    ;
  38:  R[12]=69+R[10]    ;
  39:  R[13]=99+R[10]    ;
  40:  CALL MM_GET_JPS(R[10],R[11],R[12],R[13]) ;
  41:  ENDFOR ;

第36行：FOR表示以下为循环程序片段；R[10]用于控制循环的次数（即R[10]从1开始，每次循环后自增1，直到大于R[51]，结束循环）；R[51]为MM_GET_VISP指令中第三个参数所表示的数值寄存器，该寄存器保存视觉系统返回的路径点个数。因此，R[10]可以表示当前路径点在规划路径中的编号。
第37~39行：R[11]、R[12]、R[13]分别表示当前路径点对应的关节角、标签、末端工具编号所使用的寄存器号码。
第40行：MM_GET_JPS指令表示将某个路径点的关节角、标签和末端工具编号分别转存到指定寄存器，因此整条指令表示将第R[10]编号的路径点的关节角、标签和末端工具编号依次转存到R[11]、R[12]和R[13]所表示的寄存器中。

计算R[30]和R[29]

  42:  !parse pick cycle count, here ;
  43:  !suppose 5 points per planned ;
  44:  !path ;
  45:  R[30]=R[51] DIV 5    ;
  46:  R[29]=R[51] MOD 5    ;

本样例假设每次规划的抓取路径包含5个路径点，“R[51] DIV 5”表示R[51]整除5的商，“R[51] MOD 5”表示R[51]整除5的余数。R[30]表示规划的总抓取次数。如果R[29]不为0，则表示某次规划的抓取路径点个数少于5，即路径规划出现异常，需重新进行规划。

判断规划路径是否出错

  47:  !check if parsed data is valid; ;
  48:  !if not, retry to get planned ;
  49:  !path or add some error handling ;
  50:  !logic ;
  51:  IF ((R[30]<1) OR (R[29]<>0)) THEN ;
  52:  PAUSE ;
  53:  JMP LBL[1] ;
  54:  ENDIF ;

如果抓取次数R[30]小于1，或R[29]不为0，则表示路径规划出现异常，用户需在此处添加处理逻辑，例如重新触发Mech-Vision工程并获取规划路径。

通过循环进行抓取和放置

  55:  !repeatedly run pick-and-place ;
  56:  !cycle using for-loop ;
  57:  FOR R[10]=1 TO R[30] ;
  58:  R[20]=R[10]-1    ;
  59:  R[21]=R[20]*5    ;
  60:  R[31]=60+R[21]    ;
  61:  R[32]=61+R[21]    ;
  62:  R[33]=62+R[21]    ;
  63:  R[34]=63+R[21]    ;
  64:  R[35]=64+R[21]    ;
  65:  !follow the planned path to pick ;
  66:J PR[R[31]] 50% CNT100    ;
  67:J PR[R[32]] 50% FINE    ;
  68:J PR[R[33]] 10% FINE    ;
  69:  !add object grasping logic here, ;
  70:  !such as "DO[1]=ON" ;
  71:  PAUSE ;
  72:J PR[R[34]] 50% FINE    ;
  73:J PR[R[35]] 50% CNT100    ;
  74:  !move to intermediate waypoint ;
  75:  !of placing ;
  76:J P[3] 50% CNT100    ;
  77:  !move to approach waypoint ;
  78:  !of placing ;
  79:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;
  80:  !move to placing waypoint ;
  81:L P[4] 300mm/sec FINE    ;
  82:  !add object releasing logic here, ;
  83:  !such as "DO[1]=OFF" ;
  84:  PAUSE ;
  85:  !move to departure waypoint ;
  86:  !of placing ;
  87:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;
  88:  !move to intermediate waypoint ;
  89:  !of placing ;
  90:J P[3] 50% CNT100    ;
  91:  ENDFOR ;

上述代码表示，在FOR循环中，机器人移动到每次规划的5个路径点，从而完成抓取操作，然后接着执行放置操作。R[10]用于控制循环的次数，即R[10]从1开始，每次循环后自增1，直到大于抓取次数R[30]，结束循环。每当R[10]增加1，则R[21]增加5，60+R_{64+R[21]（即R[31]\}R[35]）表示每次规划的5个路径点将要存放的5个寄存器号码。

样例程序19：MM_S19_Vis_PlanAllVision

程序简介

程序解读

我们重视您的隐私