样例程序9：MM_S9_Viz_RunInAdvance
程序解读
以下为MM_S9_Viz_RunInAdvance样例程序的代码及相关解释说明。
| 与MM_S5_Viz_SetBranch样例相比，本样例仅新增了提前规划下一轮路径的功能（加粗部分的代码）。因此，下文不再重复解释与MM_S5_Viz_SetBranch样例相同部分的代码（详情请参考MM_S5_Viz_SetBranch样例说明）。 |
DEF MM_S9_Viz_RunInAdvance ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Viz project then set
; branch and get planned path, trigger Mech-Viz
; project and set branch in advance during
; pick&place process
; 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
;initialize communication parameters (initialization is required only once)
MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
;trigger Mech-Viz project
MM_Start_Viz(2,init_jps)
;set branch exit port
MM_Set_Branch(1,1)
LOOP
;get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData(1,pos_num,vis_pos_num,status)
;check whether planned path has been got from Mech-Viz successfully
IF status<> 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 local variables one by one
MM_Get_Jps(1,Xpick_point1,label[1],toolid[1])
MM_Get_Jps(2,Xpick_point2,label[2],toolid[2])
MM_Get_Jps(3,Xpick_point3,label[3],toolid[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
;trigger Mech-Viz project but not to trigger camera capturing
MM_Start_Viz(2,init_jps)
;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]
;set branch exit port and trigger camera capturing when robot moves out of camera’s field of view
MM_Set_Branch(1,1)
;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上述样例程序代码对应的流程如下图所示。
以下为提前规划下一轮路径的代码及相关解释说明。
| 流程 | 代码及说明 | ||
|---|---|---|---|
通过循环（抓取→触发下一轮规划→放置）提前规划下一轮路径 |
上述代码表示，循环执行LOOP与ENDLOOP之间的代码。 上述代码表示，机器人通过MM_Get_VizData指令获取Mech-Viz规划的路径。 上述代码表示，机器人通过MM_Get_Jps指令将规划路径转存至指定变量。本样例假设Xpick_point1、Xpick_point2、Xpick_point3分别为抓取接近点、抓取点、抓取离开点。 上述代码表示，机器人依次移动到抓取接近点、抓取点，然后设置DO指令（例如“$OUT[1]=TRUE”），控制末端工具进行抓取。
上述代码表示，机器人通过MM_Start_Viz指令再次触发Mech-Viz工程运行。此时机器人已获取到本轮规划的抓取路径，且已移动至抓取点，因此可以提前触发Mech-Viz工程运行以规划下一轮路径，而不必等待放置完成后再触发Mech-Viz工程运行。
上述代码表示，机器人依次移动到抓取离开点（pick_point3）、中间过渡点（drop_waypoint）、放置接近点（drop_app）。 上述代码表示，机器人通过MM_Set_Branch指令设置“消息分支”步骤的出口。在该出口分支上，“视觉识别”步骤将被执行，Mech-Viz根据视觉识别结果提前规划下一轮抓取路径。 上述代码表示，机器人先移动到放置点（drop），然后进行放置操作（例如，$OUT[1]=FALSE），然后依次移动到放置离开点（drop_app）、Home点。 |