Example Program 6: MM_S6_Viz_ErrorHandle
Program Introduction
Description |
The robot starts the Mech-Viz project, obtains the planned path, and then determines whether the planned path is obtained successfully according to the status code. If the planned path is obtained successfully, the robot will perform picking and placing; otherwise, the robot will stop. |
||
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 and Mech-Viz projects |
||
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_S6_Viz_ErrorHandle example program.
| The only difference between the MM_S6_Viz_ErrorHandle example program and the MM_S2_Viz_Basic example program is that MM_S6_Viz_ErrorHandle can handle errors based on the different error codes (this code of this feature is bolded). As such, only the feature of handling errors based on the specific error codes is described in the following section. For information about the parts of MM_S6_Viz_ErrorHandle that are consistent with those of MM_S2_Viz_Basic, see Example Program 2: MM_S2_Viz_Basic. |
MODULE MM_S6_Viz_ErrorHandle
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project and get planned path,
! handle errors according to status codes (if no point cloud
! in ROI, retry several times before exit loop)
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num toolid{5}:=[0,0,0,0,0];
LOCAL VAR num vis_pose_num:=0;
LOCAL VAR num count:=0;
LOCAL VAR num retry_cnt:=0;
LOCAL VAR num label{5}:=[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 PERS 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 PERS 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{5}:=
[
[[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[11.2355,52.1281,-23.3996,0.5938,62.6295,-169.548],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[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_6()
!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;
!move to image-capturing position
MoveL camera_capture,v1000,fine,gripper1;
!open socket connection
MM_Open_Socket;
RECAP:
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps;
!get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData 1, pose_num, vis_pose_num, status;
!check whether planned path has been got from Mech-Viz successfully
IF status <> 2100 THEN
IF status = 2038 THEN
!no point cloud in ROI, add handling logic here
!self-adding then check retry counter
retry_cnt:=retry_cnt+1;
IF retry_cnt<3 THEN
!jump back to vision retry label if the number of retry times is less than 3
GOTO RECAP;
ELSE
!reset counter and exit loop if the number of retry times has reached 3
retry_cnt:=0;
GOTO END_LOOP;
ENDIF
ELSE
!add other error handling logic here
Stop;
GOTO END_LOOP;
ENDIF
ENDIF
!close socket connection
MM_Close_Socket;
!save waypoints of the planned path to local variables one by one
MM_Get_Jps 1,jps{1},label{1},toolid{1};
MM_Get_JPS 2,jps{2},label{2},toolid{2};
MM_Get_JPS 3,jps{3},label{3},toolid{3};
!follow the planned path to pick
!move to approach waypoint of picking
MoveAbsJ jps{1},v1000,fine,gripper1;
!move to picking waypoint
MoveAbsJ jps{2},v1000,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!move to departure waypoint of picking
MoveAbsJ jps{3},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 back to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
RETURN;
END_LOOP:
Stop;
ENDPROC
ENDMODULEThe workflow corresponding to the above example program code is shown in the figure below.
The table below describes the feature to process different status codes.
| Feature | Code and description | ||
|---|---|---|---|
Process different status codes |
After MM_Get_VizData is executed, the robot stores the received status code in the status variable. You can perform the corresponding operation based on the specific received error code.
|