样例程序8：MM_S8_Viz_Subtask

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

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

程序简介

功能说明

本样例由两个程序组成，其中子程序（后台程序）负责触发Mech-Viz工程运行并获取规划路径；主程序（前台程序）负责根据规划路径移动机器人，且在机器人放置时触发子程序运行，从而提前规划下一轮路径，以缩短节拍。

本样例与MM_S10_Viz_Subtask样例十分相似，仅是主程序触发子程序的时机不同，因此，建议用户对比学习这两个样例，以更好地理解它们之间的细微差别。

文件路径

子程序：Mech-Vision和Mech-Viz软件安装目录下Communication Component/Robot_Interface/KUKA/sample/sub_prog/MM_S8_Sub。

主程序：Mech-Vision和Mech-Viz软件安装目录下Communication Component/Robot_Interface/KUKA/sample/MM_S8_Viz_Subtask。

所需工程

Mech-Vision工程和Mech-Viz工程

使用前提

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

此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。
主程序属于前台程序。子程序属于后台程序，需要在机器人系统开机后自动运行。

程序解读

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

在子程序中，触发Mech-Viz工程运行并获取规划路径的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。

DEF MM_S8_Sub ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with MM_S8_Viz_Subtask)
; Mech-Mind, 2023-12-25
;---------------------------------------------------
   $OUT[2000]=FALSE
   ;initialize communication parameters (initialization is required only once)
   MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
   LOOP
      IF $OUT[2000]==TRUE THEN
         ;trigger Mech-Viz project
         MM_Start_Viz(2,init_jps_a)
         ;get planned path, 1st argument (1) means getting pose in JPs
         MM_Get_VizData(1,pos_num_a,vis_pos_num_a,status_a)
         ;check whether planned path has been got from Mech-Viz successfully
         IF status_a==2100 THEN
            ;save waypoints of the planned path to local variables one by one
            MM_Get_Jps(1,pick_point_a[1],label_a[1],toolid_a[1])
            MM_Get_Jps(2,pick_point_a[2],label_a[2],toolid_a[2])
            MM_Get_Jps(3,pick_point_a[3],label_a[3],toolid_a[3])
         ENDIF
         $OUT[2000]=FALSE
      ENDIF
   ENDLOOP
END

上述代码表示，子程序在运行时，首先将$OUT[2000]设置为FALSE，并初始化通信参数，然后通过LOOP循环持续监听$OUT[2000]值。

当$OUT[2000]为TRUE时，子程序发触发Mech-Viz工程运行并获取规划路径，然后将$OUT[2000]设置为FALSE。
当$OUT[2000]为FALSE时，子程序持续判断$OUT[2000]的值。

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

在主程序中，根据规划路径进行抓取与放置的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。

DEF  MM_S8_Viz_Subtask ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with MM_S8_Sub)
; Mech-Mind, 2023-12-25
;---------------------------------------------------
   ;set current tool no. to 1
   BAS(#TOOL,1)
   ;set current base no. to 0
   BAS(#BASE,0)
   ;move to robot home position
PTP HOME Vel=100 % DEFAULT
   ;trigger Mech-Viz project and get planned path
   $OUT[2000]=TRUE
LOOP
   ;move to wait position for picking
LIN pick_wait_point Vel=1 m/s CPDAT6 Tool[1] Base[0]
   ;wait until subtask program finished
   Wait for ($OUT[2000]==FALSE)
   ;check whether planned path has been got from Mech-Viz successfully
   IF status_a<> 2100 THEN
      ;add error handling logic here according to different error codes
      ;e.g.: status=2038 means no point cloud in ROI
      halt
   ENDIF
   ;save waypoints of the planned path to jps variables one by one
   Xpick_point1=pick_point_a[1]
   Xpick_point2=pick_point_a[2]
   Xpick_point3=pick_point_a[3]
   ;follow the planned path to pick
   ;move to approach waypoint of picking
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
   ;move to picking waypoint
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
   ;add object grasping logic here, such as "$OUT[1]=TRUE"
   halt
   ;move to departure waypoint of picking
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
   ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
   ;trigger Mech-Viz project and get planned path in advance
   $OUT_C[2000]=TRUE
   ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
   ;add object releasing logic here, such as "$OUT[1]=FALSE"
   halt
   ;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
   ;move back to robot home position
PTP HOME Vel=100 % DEFAULT
ENDLOOP
END

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

下表为主程序核心代码的逻辑解读。

流程代码及说明

流程	代码及说明
触发Mech-Viz工程运行并获取规划路径	`;trigger Mech-Viz project and get planned path $OUT[2000]=TRUE` 上述代码表示，在主程序中，将$OUT[2000]设置为TRUE。此时子程序检测到$OUT[2000]为TRUE，便触发Mech-Viz工程运行并获取规划路径。
通过循环（抓取→触发下一轮规划→放置）提前规划下一轮路径	`LOOP ... ENDLOOP` 上述代码表示，主程序循环执行LOOP与ENDLOOP之间的代码。 `;move to wait position for picking LIN pick_wait_point Vel=1 m/s CPDAT6 Tool[1] Base[0] ;wait until subtask program finished Wait for ($OUT[2000]==FALSE)` 上述代码表示，机器人移动到抓取前的一个等待点，等待子程序获取并转存规划路径（即$OUT[2000]从TRUE变为FALSE）。 `;save waypoints of the planned path to jps variables one by one Xpick_point1=pick_point_a[1] Xpick_point2=pick_point_a[2] Xpick_point3=pick_point_a[3]` 上述代码表示，将子程序获取的规划路径（pick_point_a[1]、pick_point_a[2]和pick_point_a[3]）分别转存至Xpick_point1、Xpick_point2和Xpick_point3。 ;follow the planned path to pick ;move to approach waypoint of picking PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0] ;move to picking waypoint PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0] ;add object grasping logic here, such as "$OUT[1]=TRUE" halt ;move to departure waypoint of picking PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0] ;move to intermediate waypoint of placing PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0] ;move to approach waypoint of placing LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0] 上述代码表示，机器人根据规划的路径先移动到抓取接近点（pick_point1），然后移动到抓取点（pick_point2），进行抓取（例如，$OUT[1]=TRUE），再依次移动到抓取离开点（pick_point3）、放置过渡点（drop_waypoint）、放置接近点（drop_app）。 `;trigger Mech-Viz project and get planned path in advance $OUT_C[2000]=TRUE` 上述代码表示，主程序程序再次将$OUT_C[2000]设置为TRUE，即再次触发Mech-Viz工程运行并获取规划路径。此处机器人已处于放置区域，因此机器人此时可以提前规划下一轮抓取路径，而不必等待放置完成后再规划下一轮抓取路径。 `;move to placing waypoint LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0] ;add object releasing logic here, such as "$OUT[1]=FALSE" halt ;move to departure waypoint of placing LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0] ;move back to robot home position PTP HOME Vel=100 % DEFAULT` 上述代码表示，机器人先移动到放置点（drop），然后进行放置操作（例如，$OUT[1]=FALSE），然后依次移动到放置离开点（drop_app）、Home点。

触发Mech-Viz工程运行并获取规划路径

;trigger Mech-Viz project and get planned path
$OUT[2000]=TRUE

上述代码表示，在主程序中，将$OUT[2000]设置为TRUE。此时子程序检测到$OUT[2000]为TRUE，便触发Mech-Viz工程运行并获取规划路径。

通过循环（抓取→触发下一轮规划→放置）提前规划下一轮路径

LOOP
    ...
ENDLOOP

上述代码表示，主程序循环执行LOOP与ENDLOOP之间的代码。

   ;move to wait position for picking
LIN pick_wait_point Vel=1 m/s CPDAT6 Tool[1] Base[0]
   ;wait until subtask program finished
   Wait for ($OUT[2000]==FALSE)

上述代码表示，机器人移动到抓取前的一个等待点，等待子程序获取并转存规划路径（即$OUT[2000]从TRUE变为FALSE）。

;save waypoints of the planned path to jps variables one by one
Xpick_point1=pick_point_a[1]
Xpick_point2=pick_point_a[2]
Xpick_point3=pick_point_a[3]

上述代码表示，将子程序获取的规划路径（pick_point_a[1]、pick_point_a[2]和pick_point_a[3]）分别转存至Xpick_point1、Xpick_point2和Xpick_point3。

   ;follow the planned path to pick
   ;move to approach waypoint of picking
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
   ;move to picking waypoint
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
   ;add object grasping logic here, such as "$OUT[1]=TRUE"
   halt
   ;move to departure waypoint of picking
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
   ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]

上述代码表示，机器人根据规划的路径先移动到抓取接近点（pick_point1），然后移动到抓取点（pick_point2），进行抓取（例如，$OUT[1]=TRUE），再依次移动到抓取离开点（pick_point3）、放置过渡点（drop_waypoint）、放置接近点（drop_app）。

;trigger Mech-Viz project and get planned path in advance
$OUT_C[2000]=TRUE

上述代码表示，主程序程序再次将$OUT_C[2000]设置为TRUE，即再次触发Mech-Viz工程运行并获取规划路径。此处机器人已处于放置区域，因此机器人此时可以提前规划下一轮抓取路径，而不必等待放置完成后再规划下一轮抓取路径。

   ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
   ;add object releasing logic here, such as "$OUT[1]=FALSE"
   halt
   ;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
   ;move back to robot home position
PTP HOME Vel=100 % DEFAULT

上述代码表示，机器人先移动到放置点（drop），然后进行放置操作（例如，$OUT[1]=FALSE），然后依次移动到放置离开点（drop_app）、Home点。

样例程序8：MM_S8_Viz_Subtask

程序简介

程序解读

我们重视您的隐私