Example Program 19: MM_S19_Vis_PlanAllVision
Program Introduction
Description |
The robot triggers the Mech-Vision project to run. Then, the robot uses for loops to obtain all planned paths and perform picking and placing. In this example, once the camera captures an image, Mech-Vision will plan picking paths for all vision results. This program is applicable to scenarios where one image is used to perform picking for multiple times. |
||
File path |
You can navigate to the installation directory of Mech-Vision and Mech-Viz and find the file by using the |
||
Project |
Mech-Vision project
|
||
Prerequisites |
|
| This example program is provided for reference only. Before using the program, please modify the program according to the actual scenario. |
Program Description
This part describes the MM_S19_Vis_PlanAllVision example program.
| The only difference between the MM_S19_Vis_PlanAllVision example program and the MM_S3_Vis_Path example program is that MM_S19_Vis_PlanAllVision can use for loops to obtain all planned paths and perform picking and placing (this code of this feature is bolded). As such, only the feature of using for loops to obtain all planned paths and perform picking and placing is described in the following part. For information about the parts of MM_S19_Vis_PlanAllVision that are consistent with those of MM_S3_Vis_Path, see Example Program: MM_S3_Vis_Path. |
DEF MM_S19_Vis_PlanAllVision ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Vision project, plan all
; vision results and get all planned paths
; 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)
RECAP:
;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
;trigger Mech-Viz project
MM_Start_Vis(1,0,2,init_jps)
;get planned path from NO.1 Mech-Vision project; 2nd argument (1) means getting pose in JPs
MM_Get_Vispath(1,1,pos_num,vis_pos_num,status)
;check whether planned path has been got from Mech-Vision successfully
IF status<> 1103 THEN
;add error handling logic here according to different error codes
;e.g.: status=1003 means no point cloud in ROI
;e.g.: status=1002 means no vision results
halt
ENDIF
;save waypoints of the planned path to local variables one by one
FOR count=1 TO pos_num
MM_Get_Jps(count,pick_point[count],label[count],toolid[count])
ENDFOR
;parse pick cycle count, here suppose 5 points per planned path
pick_cnt = pos_num / 5
residual = pos_num - pick_cnt*5
;check if parsed data is valid; if not, retry to get planned path or add some error handling logic
IF (pick_cnt<1) OR (residual<>0) THEN
halt
GOTO RECAP
ENDIF
;repeatedly run pick-and-place cycle using for-loop
FOR i=1 TO pick_cnt
count=(i-1)*5
Xpick_point1=pick_point[1+count]
Xpick_point2=pick_point[2+count]
Xpick_point3=pick_point[3+count]
Xpick_point4=pick_point[4+count]
Xpick_point5=pick_point[5+count]
;follow the planned path to pick
;move to approach waypoint of picking
PTP pick_point1 CONT Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=50 % PDAT2 Tool[1] Base[0]
PTP pick_point3 Vel=10 % PDAT3 Tool[1] Base[0]
;add object grasping logic here, such as "$OUT[1]=TRUE"
halt
PTP pick_point4 Vel=50 % PDAT5 Tool[1] Base[0]
PTP pick_point5 CONT Vel=50 % PDAT6 Tool[1] Base[0]
;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT4 Tool[1] Base[0]
;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT2 Tool[1] Base[0]
;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT3 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 CPDAT2 Tool[1] Base[0]
;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT4 Tool[1] Base[0]
ENDFOR
;finish pick and-place cycle, and jump back to camera capturing
GOTO RECAP
ENDThe workflow corresponding to the above example program code is shown in the figure below.
The table below describes the feature of using for loops to obtain all planned paths and perform picking and placing. You can click the hyperlink to the command name to view its detailed description.
| Feature | Code and description |
|---|---|
Store the planned path by looping |
|
Calculate pick_cnt and residual |
This example program assumes that each planned picking path contains 5 waypoints. "pos_num / 5" stands for the quotient of the pose_num value divided by 5, and "pos_num - pick_cnt*5" stands for the remainder of the pose_num value divided by 5. pick_cnt is the total number of picking times planned. If residual is not set to 0, the planned number of picking waypoints is less than 5 (i.e., an error has occurred during path planning and a re-planning operation is needed). |
Determine whether an error has occurred during path planning |
If the number of picking times (pick_cnt) is less than 1 or the value of residual is not 0, an error has occurred during path planning. You need to add processing code here, such as the code to restart the Mech-Vision project and then obtain the planned path. |
Perform picking and placing by looping |
The above code indicates that in the for loop, the robot moves to the 5 waypoints planned each time to complete the picking operation and then performs the placing operation. i is used to control the number of iterations in the loop (i.e., i starts from 1 and increments by 1 after each loop iteration until it exceeds the value of pick_cnt, at which point the loop ends). When i increments by 1, count increments by 5. [1+count] to [5+count] denote the ID of the 5 waypoints planned each time in the total planned path. |