Example Program 20: MM_S20_Viz_PlanAllVision

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/ABB/sample/MM_S20_Viz_PlanAllVision path.

For RobotWare6, the file extension is .mod. For RobotWare7, please modify the file extension from .mod to .modx.

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, only the feature of using for loops to obtain all planned paths and perform picking and placing is described in the following section. For information about the parts of MM_S20_Viz_PlanAllVision that are consistent with those of MM_S2_Viz_Basic, see Example Program: MM_S2_Viz_Basic.

MODULE 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
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num vis_pose_num:=0;
LOCAL VAR num count:=0;
LOCAL VAR num pick_cnt:=0;
LOCAL VAR num residual:=0;
LOCAL VAR num movetype{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num toolnum{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num speed{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
!define local joint&pose variables
LOCAL CONST jointtarget home:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL CONST jointtarget snap_jps:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget camera_capture:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL VAR robtarget pickpoint:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL VAR robtarget pick_waypoint:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL VAR robtarget drop_waypoint:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL VAR robtarget drop:=[[302.00,0.00,558.00],[0,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS jointtarget jps{20}:=
[
    [[-4.5438,26.6029,0.467,-0.1719,62.9165,-5.0819],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-4.5664,32.0439,7.6047,-0.1988,50.3379,-5.0558],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-4.5363,25.4009,-2.7016,-0.1659,67.2872,-5.0885],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.7719,15.0701,16.7232,0.1053,57.9831,-111.82],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.7567,21.7711,23.8763,0.1281,44.1289,-111.841],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.777,13.5249,13.6202,0.1005,62.6312,-111.816],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3363,10.0698,23.0021,0.0017,56.8211,89.2201],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3365,17.2854,30.3891,0.0022,42.2185,89.2197],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3362,8.4182,19.8363,0.0017,61.6386,89.2202],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.6715,8.5325,24.7355,0.0089,56.6276,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.6701,15.8879,32.2202,0.0112,41.7875,265.897],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.672,6.8572,21.5409,0.0085,61.4976,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.387,12.8326,19.5238,-0.0095,57.6221,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.3856,19.7174,26.8272,-0.0117,43.434,110.351],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.3874,11.2544,16.3788,-0.0091,62.3455,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.5881,41.5724,-23.6476,-0.3127,72.1244,189.431],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.5502,45.3582,-15.4893,-0.3431,60.1803,189.468],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.6008,40.9927,-27.4632,-0.306,76.5197,189.419],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[28.7848,90.399,-13.628,14.5039,10.6719,-165.371],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]]
];
!define local tooldata variables
LOCAL PERS tooldata gripper1:=[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];

PROC Sample_20()
    !set the acceleration parameters
    AccSet 50, 50;
    !set the velocity parameters
    VelSet 50, 1000;
    !move to robot home position
    MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
    !initialize communication parameters (initialization is required only once)
    MM_Init_Socket "127.0.0.1",50000,300;
RECAP:
    !move to image-capturing position
    MoveL camera_capture,v1000,fine,gripper1;
    !open socket connection
    MM_Open_Socket;
    !trigger Mech-Viz project
    MM_Start_Viz 2,snap_jps;
    !get planned path
    MM_Get_PlanData 0, 3, pose_num, vis_pose_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
        Stop;
    ENDIF
    !close socket connection
    MM_Close_Socket;
    !save all waypoint data to local variables using for-loop, a maximum of 50 points are supported
    FOR i FROM 1 TO pose_num DO
        count:=i;
        MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
    ENDFOR
    !parse pick cycle count, here suppose 3 points per planned path
    pick_cnt:= pose_num DIV 3;
    residual:= pose_num MOD 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
        Stop;
        GOTO RECAP;
    ENDIF
    !repeatedly run pick-and-place cycle using for-loop
    FOR i FROM 1 TO pick_cnt DO
        count:=(i-1)*3;
        !move to intermediate waypoint of picking
        MoveJ pick_waypoint,v1000,z50,gripper1;
        !follow the planned path to pick
        MoveAbsJ jps{1+count},v1000,fine,gripper1;
        MoveAbsJ jps{2+count},v1000,fine,gripper1;
        !add object grasping logic here, such as "setdo DO_1, 1;";
        Stop;
        MoveAbsJ jps{3+count},v1000,fine,gripper1;
        !move to intermediate waypoint of placing
        MoveJ drop_waypoint,v1000,z50,gripper1;
        !move to approach waypoint of placing
        MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
        !move to placing waypoint
        MoveL drop,v300,fine,gripper1;
        !add object releasing logic here, such as "setdo DO_1, 0;"
        Stop;
        !move to departure waypoint of placing
        MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
        !move to intermediate waypoint of placing
        MoveJ drop_waypoint,v1000,z50,gripper1;
    ENDFOR
    !finish pick and-place cycle, and jump back to camera capturing
    GOTO RECAP;
ENDPROC
ENDMODULE

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, pose_num, vis_pose_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 position 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 i FROM 1 TO pose_num DO count:=i; MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count}; ENDFOR` Line 1: FOR indicates a for loop. 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 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: Assign the value of i to count, where count represents the ID of the current waypoint in the total planned path. Line 3: 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 JPS{count}, movetype{count}, toolnum{count}, and speed{count} variables respectively.
Calculate pick_cnt and residual	`!parse pick cycle count, here suppose 3 points per planned path pick_cnt:= pose_num DIV 3; residual:= pose_num MOD 3;` This example program assumes that each planned picking path contains 3 waypoints. pose_num DIV 3 stands for the quotient of the pose_num value divided by 3, and pose_num MOD 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 Stop; 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 FROM 1 TO pick_cnt DO count:=(i-1)*3; !move to intermediate waypoint of picking MoveJ pick_waypoint,v1000,z50,gripper1; !follow the planned path to pick MoveAbsJ jps{1+count},v1000,fine,gripper1; MoveAbsJ jps{2+count},v1000,fine,gripper1; !add object grasping logic here, such as "setdo DO_1, 1;"; Stop; MoveAbsJ jps{3+count},v1000,fine,gripper1; !move to intermediate waypoint of placing MoveJ drop_waypoint,v1000,z50,gripper1; !move to approach waypoint of placing MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1; !move to placing waypoint MoveL drop,v300,fine,gripper1; !add object releasing logic here, such as "setdo DO_1, 0;" Stop; !move to departure waypoint of placing MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1; !move to intermediate waypoint of placing MoveJ drop_waypoint,v1000,z50,gripper1; 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, pose_num, vis_pose_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 position 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 i FROM 1 TO pose_num DO
    count:=i;
    MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
ENDFOR

Line 1: FOR indicates a for loop. 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 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: Assign the value of i to count, where count represents the ID of the current waypoint in the total planned path.
Line 3: 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 JPS{count}, movetype{count}, toolnum{count}, and speed{count} variables respectively.

Calculate pick_cnt and residual

!parse pick cycle count, here suppose 3 points per planned path
pick_cnt:= pose_num DIV 3;
residual:= pose_num MOD 3;

This example program assumes that each planned picking path contains 3 waypoints. pose_num DIV 3 stands for the quotient of the pose_num value divided by 3, and pose_num MOD 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
    Stop;
    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 FROM 1 TO pick_cnt DO
    count:=(i-1)*3;
    !move to intermediate waypoint of picking
    MoveJ pick_waypoint,v1000,z50,gripper1;
    !follow the planned path to pick
    MoveAbsJ jps{1+count},v1000,fine,gripper1;
    MoveAbsJ jps{2+count},v1000,fine,gripper1;
    !add object grasping logic here, such as "setdo DO_1, 1;";
    Stop;
    MoveAbsJ jps{3+count},v1000,fine,gripper1;
    !move to intermediate waypoint of placing
    MoveJ drop_waypoint,v1000,z50,gripper1;
    !move to approach waypoint of placing
    MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
    !move to placing waypoint
    MoveL drop,v300,fine,gripper1;
    !add object releasing logic here, such as "setdo DO_1, 0;"
    Stop;
    !move to departure waypoint of placing
    MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
    !move to intermediate waypoint of placing
    MoveJ drop_waypoint,v1000,z50,gripper1;
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.

We Value Your Privacy

We use cookies to provide you with the best possible experience on our website. By continuing to use the site, you acknowledge that you agree to the use of cookies. If you decline, a single cookie will be used to ensure you're not tracked or remembered when you visit this website.

I accept

I don't accept