样例程序10：MM_S10_Viz_Subtask
程序简介
功能说明 |
本样例由两个程序组成，其中子程序（后台程序）负责触发Mech-Viz工程运行并获取规划路径；主程序（前台程序）负责根据规划路径移动机器人，且在机器人离开抓取区域时触发子程序运行，从而提前规划下一轮路径，以缩短节拍。
|
||
文件路径 |
子程序：Mech-Vision和Mech-Viz软件安装目录下 主程序：Mech-Vision和Mech-Viz软件安装目录下 |
||
所需工程 |
Mech-Vision工程和Mech-Viz工程 |
||
使用前提 |
|
程序解读
以下为子程序的代码及相关解释说明。
| 在子程序中，触发Mech-Viz工程运行并获取规划路径的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 |
DEF MM_S10_Sub ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with
; MM_S10_Viz_Subtask)
; Mech-Mind, 2023-12-25
;---------------------------------------------------
$OUT[2001]=FALSE
;initialize communication parameters (initialization is required only once)
MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
LOOP
IF $OUT[2001]==TRUE THEN
;trigger Mech-Viz project
MM_Start_Viz(2,init_jps_b)
;get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData(1,pos_num_b,vis_pos_num_b,status_b)
;check whether planned path has been got from Mech-Viz successfully
IF status_b==2100 THEN
;save waypoints of the planned path to local variables one by one
MM_Get_Jps(1,pick_point_b[1],label_b[1],toolid_b[1])
MM_Get_Jps(2,pick_point_b[2],label_b[2],toolid_b[2])
MM_Get_Jps(3,pick_point_b[3],label_b[3],toolid_b[3])
ENDIF
$OUT[2001]=FALSE
ENDIF
ENDLOOP
END上述代码表示，子程序在运行时，首先将$OUT[2001]设置为FALSE，并初始化通信参数，然后通过LOOP循环持续监听$OUT[2001]值。
-
当$OUT[2001]为TRUE时，子程序发触发Mech-Viz工程运行并获取规划路径，然后将$OUT[2001]设置为FALSE。
-
当$OUT[2001]为FALSE时，子程序持续判断$OUT[2001]的值。
以下为主程序的代码及相关解释说明。
| 在主程序中，根据规划路径进行抓取与放置的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 |
DEF MM_S10_Viz_Subtask ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with MM_S10_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[2001]=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[2001]==FALSE)
;check whether planned path has been got from Mech-Viz successfully
IF status_b<> 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_b[1]
Xpick_point2=pick_point_b[2]
Xpick_point3=pick_point_b[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]
;trigger Mech-Viz project and get planned path in advance
TRIGGER WHEN DISTANCE = 1 DELAY = 0 DO $OUT[2001]=TRUE
;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]
;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工程运行并获取规划路径 |
上述代码表示，在主程序中，将$OUT[2001]设置为TRUE。此时子程序检测到$OUT[2001]为TRUE，便触发Mech-Viz工程运行并获取规划路径。 |
||
通过循环（抓取→触发下一轮规划→放置）提前规划下一轮路径 |
上述代码表示，主程序循环执行LOOP与ENDLOOP之间的代码。 上述代码表示，机器人移动到抓取前的一个等待点，等待子程序获取并转存规划路径（即($OUT[2001]从TRUE变为FALSE）。 上述代码表示，将子程序获取的规划路径（pick_point_b[1]、pick_point_b[2]和pick_point_b[3]）分别转存至Xpick_point1、Xpick_point2和Xpick_point3。 上述代码表示，机器人根据规划的路径先移动到抓取接近点（pick_point1），然后移动到抓取点（pick_point2），进行抓取（例如，$OUT[1]=TRUE），再移动到抓取离开点（pick_point3）。
上述代码表示，机器人在逼近目标点（drop_waypoint，即放置过程中的过渡点）时，触发$OUT[2001]=TRUE，即子程序再次触发Mech-Viz工程运行并获取规划路径。此处机器人已处于抓取区域外，因此机器人此时可以提前规划下一轮抓取路径，而不必等待放置完成后再规划下一轮抓取路径。
上述代码表示，机器人先依次移动到放置过渡点（drop_waypoint）、放置接近点（drop_app）、放置点（drop），然后进行放置操作（例如，$OUT[1]=FALSE），然后依次移动到放置离开点（drop_app）、Home点。 |