Example Program 20: MM_S20_Viz_PlanAllVision

You are currently viewing the documentation for the latest version (2.1.0). To access a different version, click the "Switch version" button located in the upper-right corner of the page.

■ If you are not sure which version of the product you are currently using, please feel free to contact Mech-Mind Technical Support.

Program Introduction

Description

The robot triggers the Mech-Viz 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-Viz 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 Communication Component/Robot_Interface/KUKA/sample/MM_S20_Viz_PlanAllVision path.

Project

Mech-Vision and Mech-Viz projects

In the Mech-Viz project, the Reuse Vision Result parameter of the Vision Move Step must be enabled.

Prerequisites

You have set up the Standard Interface communication.
Automatic calibration is completed.

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_S20_Viz_PlanAllVision example program.

The only difference between the MM_S20_Viz_PlanAllVision example program and the MM_S2_Viz_Basic example program is that MM_S20_Viz_PlanAllVision can use for loops to obtain all planned paths and perform picking and placing (this code of this feature is bolded). As such, the features of the MM_S20_Viz_PlanAllVision program that are similar to those of MM_S2_Viz_Basic are not described in this part. For more information about these features, see Example Program 2: MM_S2_Viz_Basic.

DEF  MM_S20_Viz_PlanAllVision ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Viz project, plan all
; vision results and get all planned results using
; command 210
; 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_Viz(2,init_jps)
   ;get planned path
   MM_Get_PlanData(0,3,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
   FOR count=1 TO pos_num
      MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count])
   ENDFOR
   ;parse pick cycle count, here suppose 3 points per planned path
   pick_cnt = pos_num / 3
   residual = pos_num - pick_cnt*3
   ;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)*3
      Xpick_point1=pick_point[1+count]
      Xpick_point2=pick_point[2+count]
      Xpick_point3=pick_point[3+count]
      ;move to intermediate waypoint of picking
PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0]
      ;follow the planned path to pick
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
      ;add object grasping logic here, such as "$OUT[1]=TRUE"
      halt
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]
      ;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 to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   ENDFOR
   ;finish pick and-place cycle, and jump back to camera capturing
   GOTO RECAP
END

The 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

Feature	Code and description
Obtain the planned path	`;get planned path MM_Get_PlanData(0,3,pos_num,vis_pos_num,status)` MM_Get_PlanData: The command to obtain the planned path. The Vision Move waypoints obtained by this command contain Vision Move data and custom data (if any) in addition to poses, while Vision Move waypoints obtained by the MM_Get_VizData command do not contain Vision Move data or custom data. 0: Obtain the planned path from Mech-Viz. 3: The format of the data that is expected to be returned, which is pose (in joint positions), motion type, tool ID, velocity, Mech-Viz Vision Move data, element 1 in custom output data, ..., element N in custom output data. pose_num: The variable that stores the number of waypoints returned by the vision system. vis_pose_num: The variable that stores the sequence number of the last Vision Move waypoint (picking waypoint) in the total planned path. status: The variable that stores the command execution status code.
Store the planned path by looping	`FOR count=1 TO pos_num MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count]) ENDFOR` Line 1: FOR indicates a for loop. count is used to control the number of iterations in the loop (i.e., count starts from 1 and increments by 1 after each loop iteration until it exceeds the value of pose_num, at which point the loop ends). pose_num is the third parameter of the MM_Get_PlanData command, which represents the number of waypoints returned by the vision system. Line 2: The MM_Get_PlanJps command stores the joint positions, motion type, tool ID, and velocity of a specific waypoint in specific variables. The entire command stores the joint positions, motion type, tool ID, and velocity of the waypoint with an ID of count in the pick_point[count], move_type[count], tool_num[count], and speed[count] variables respectively. Line 3: The for loop ends.
Calculate pick_cnt and residual	`;parse pick cycle count, here suppose 3 points per planned path pick_cnt = pos_num / 3 residual = pos_num - pick_cnt3` This example program assumes that each planned picking path contains 3 waypoints. "pick_cnt = pos_num / 3" stands for the quotient of the pose_num value divided by 3, and "pos_num - pick_cnt3" stands for the remainder of the pose_num value divided by 3. 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 3 (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 (pick_cnt<1) OR (residual<>0) THEN halt GOTO RECAP ENDIF` 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-Viz project and then obtain the planned path.
Perform picking and placing by looping	FOR i=1 TO pick_cnt count=(i-1)*3 Xpick_point1=pick_point[1+count] Xpick_point2=pick_point[2+count] Xpick_point3=pick_point[3+count] ;move to intermediate waypoint of picking PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0] ;follow the planned path to pick PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0] PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0] ;add object grasping logic here, such as "$OUT[1]=TRUE" halt 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] ;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 to intermediate waypoint of placing PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0] ENDFOR The above code indicates that in the for loop, the robot moves to the 3 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 3. [1+count] to [3+count] denote the ID of the 3 waypoints planned each time in the total planned path.

Obtain the planned path

;get planned path
MM_Get_PlanData(0,3,pos_num,vis_pos_num,status)

MM_Get_PlanData: The command to obtain the planned path. The Vision Move waypoints obtained by this command contain Vision Move data and custom data (if any) in addition to poses, while Vision Move waypoints obtained by the MM_Get_VizData command do not contain Vision Move data or custom data.
0: Obtain the planned path from Mech-Viz.
3: The format of the data that is expected to be returned, which is pose (in joint positions), motion type, tool ID, velocity, Mech-Viz Vision Move data, element 1 in custom output data, ..., element N in custom output data.
pose_num: The variable that stores the number of waypoints returned by the vision system.
vis_pose_num: The variable that stores the sequence number of the last Vision Move waypoint (picking waypoint) in the total planned path.
status: The variable that stores the command execution status code.

Store the planned path by looping

FOR count=1 TO pos_num
MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count])
ENDFOR

Line 1: FOR indicates a for loop. count is used to control the number of iterations in the loop (i.e., count starts from 1 and increments by 1 after each loop iteration until it exceeds the value of pose_num, at which point the loop ends). pose_num is the third parameter of the MM_Get_PlanData command, which represents the number of waypoints returned by the vision system.
Line 2: The MM_Get_PlanJps command stores the joint positions, motion type, tool ID, and velocity of a specific waypoint in specific variables. The entire command stores the joint positions, motion type, tool ID, and velocity of the waypoint with an ID of count in the pick_point[count], move_type[count], tool_num[count], and speed[count] variables respectively.
Line 3: The for loop ends.

Calculate pick_cnt and residual

;parse pick cycle count, here suppose 3 points per planned path
pick_cnt = pos_num / 3
residual = pos_num - pick_cnt*3

This example program assumes that each planned picking path contains 3 waypoints. "pick_cnt = pos_num / 3" stands for the quotient of the pose_num value divided by 3, and "pos_num - pick_cnt*3" stands for the remainder of the pose_num value divided by 3. 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 3 (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 (pick_cnt<1) OR (residual<>0) THEN
   halt
   GOTO RECAP
ENDIF

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-Viz project and then obtain the planned path.

Perform picking and placing by looping

   FOR i=1 TO pick_cnt
      count=(i-1)*3
      Xpick_point1=pick_point[1+count]
      Xpick_point2=pick_point[2+count]
      Xpick_point3=pick_point[3+count]
      ;move to intermediate waypoint of picking
PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0]
      ;follow the planned path to pick
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
      ;add object grasping logic here, such as "$OUT[1]=TRUE"
      halt
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]
      ;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 to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   ENDFOR

The above code indicates that in the for loop, the robot moves to the 3 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 3. [1+count] to [3+count] denote the ID of the 3 waypoints planned each time in the total planned path.

Example Program 20: MM_S20_Viz_PlanAllVision

Program Introduction

Program Description

We Value Your Privacy