Example Program 3: MM_S3_Vis_Path

Program Introduction

Description

The robot triggers the Mech-Vision project to run, and then obtains the planned path for picking and placing.

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/FANUC/sample/MM_S3_Vis_Path path.

Project

Mech-Vision project

The Mech-Vision project should contain a Path Planning Step, and the Port Type parameter of the Output Step should be set to Predefined (robot path).

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

   1:  !-------------------------------- ;
   2:  !FUNCTION: trigger Mech-Vision ;
   3:  !project and get planned path ;
   4:  !Mech-Mind, 2023-12-25 ;
   5:  !-------------------------------- ;
   6:   ;
   7:  !set current uframe NO. to 0 ;
   8:  UFRAME_NUM=0 ;
   9:  !set current tool NO. to 1 ;
  10:  UTOOL_NUM=1 ;
  11:  !move to robot home position ;
  12:J P[1] 100% FINE    ;
  13:  !initialize communication ;
  14:  !parameters(initialization is ;
  15:  !required only once) ;
  16:  CALL MM_INIT_SKT('8','127.0.0.1',50000,5) ;
  17:  !move to image-capturing position ;
  18:L P[2] 1000mm/sec FINE    ;
  19:  !trigger NO.1 Mech-Vision project ;
  20:  CALL MM_START_VIS(1,0,2,10) ;
  21:  !get planned path from NO.1 ;
  22:  !Mech-Vision project; 2nd ;
  23:  !argument (1) means getting pose ;
  24:  !in JPs ;
  25:  CALL MM_GET_VISP(1,1,51,52,53) ;
  26:  !check whether planned path has ;
  27:  !been got from Mech-Vision ;
  28:  !successfully ;
  29:  IF R[53]<>1103,JMP LBL[99] ;
  30:  !save waypoints of the planned ;
  31:  !path to local variables one ;
  32:  !by one ;
  33:  CALL MM_GET_JPS(1,60,70,80) ;
  34:  CALL MM_GET_JPS(2,61,71,81) ;
  35:  CALL MM_GET_JPS(3,62,72,82) ;
  36:  !follow the planned path to pick ;
  37:  !move to approach waypoint ;
  38:  !of picking ;
  39:J PR[60] 50% FINE    ;
  40:  !move to picking waypoint ;
  41:J PR[61] 10% FINE    ;
  42:  !add object grasping logic here, ;
  43:  !such as "DO[1]=ON" ;
  44:  PAUSE ;
  45:  !move to departure waypoint ;
  46:  !of picking ;
  47:J PR[62] 50% FINE    ;
  48:  !move to intermediate waypoint ;
  49:  !of placing ;
  50:J P[3] 50% CNT100    ;
  51:  !move to approach waypoint ;
  52:  !of placing ;
  53:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;
  54:  !move to placing waypoint ;
  55:L P[4] 300mm/sec FINE    ;
  56:  !add object releasing logic here, ;
  57:  !such as "DO[1]=OFF" ;
  58:  PAUSE ;
  59:  !move to departure waypoint ;
  60:  !of placing ;
  61:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;
  62:  !move back to robot home position ;
  63:J P[1] 100% FINE    ;
  64:  END ;
  65:   ;
  66:  LBL[99:vision error] ;
  67:  !add error handling logic here ;
  68:  !according to different ;
  69:  !error codes ;
  70:  !e.g.: status=1003 means no ;
  71:  !point cloud in ROI ;
  72:  !e.g.: status=1002 means no ;
  73:  !vision results ;
  74:  PAUSE ;

The workflow corresponding to the above example program code is shown in the figure below.

The table below explains the above program. You can click the hyperlink to the command name to view its detailed description.

Feature Code and description

Set the reference frame

   7:  !set current uframe NO. to 0 ;
   8:  UFRAME_NUM=0 ;
   9:  !set current tool NO. to 1 ;
  10:  UTOOL_NUM=1 ;

UFRAME_NUM=0: Set the selected user reference frame to 0. 0 indicates the world reference frame.
UTOOL_NUM=1: Set the selected tool reference frame to 1. You must specify the tool reference frame represented by 1.

The above two statements set the selected user and tool reference frames.

Move to the home position

  11:  !move to robot home position ;
  12:J P[1] 100% FINE    ;

J: The joint position motion command of the robot, indicating that the robot moves along the arc to a specified position.
P[1]: Specify the target position to which the robot moves, i.e., the home position that is taught. The home position is usually a safe one where the robot is away from workobjects and the surrounding equipment.
100%: The percentage of relative maximum movement velocity.
FINE: The robot pauses briefly upon reaching a target position before moving to the next target.

The entire statement indicates that the robot moves to the taught home position by using joint position movement.

You should teach the home position (P[1]) in advance. For detailed instructions, see Teach Calibration Starting Point in the calibration document.

Initialize communication parameters

  13:  !initialize communication ;
  14:  !parameters(initialization is ;
  15:  !required only once) ;
  16:  CALL MM_INIT_SKT('8','127.0.0.1',50000,5) ;

MM_INIT_SKT: The command to initialize the communication.
'8': The port number of the robot.
'127.0.0.1': The IP address of the IPC.
50000: The port number of the IPC.
5: Specify the communication timeout period as 5 minutes.

The entire statement indicates that the robot establishes the connection with the vision system by specifying the IP address, port number, and timeout period of the communication target (the IPC) through the MM_INIT_SKT command.

Please modify the IP address and port number of the IPC according to the actual situation. The IP address and port number must be consistent with those set in the vision system.

Move to the image-capturing position

  17:  !move to image-capturing position ;
  18:L P[2] 1000mm/sec FINE    ;

L: The linear motion command of the robot. This command specifies to move the robot along a linear path to a specified position.
P[2]: Specify the target position to which the robot moves, i.e., the image-capturing position that is taught. The image-capturing position refers to the position of the robot where the camera on the robot captures images. At this position, the robot arm should not block the camera’s FOV.

The entire command indicates that the robot moves to the image-capturing position in linear motion, with a velocity of 1000 mm/sec.

You should teach the image-capturing position (P[2]) in advance. For detailed instructions, see Teach Calibration Start Point in the calibration document.

Trigger the Mech-Vision project to run

  19:  !trigger NO.1 Mech-Vision project ;
  20:  CALL MM_START_VIS(1,0,2,10) ;

MM_START_VIS: The command to trigger the Mech-Vision project to run.
1: The Mech-Vision project ID.
0: The Mech-Vision project is expected to return all waypoints.
2: Specify that the robot flange pose must be input to the Mech-Vision project.
10: The position register PR[10], which stores custom joint positions. In this example program, the joint positions have no practical use, but the position register ID must be specified.

The entire statement indicates that the robot triggers the vision system to run the Mech-Vision project with an ID of 1 and expects the Mech-Vision project to return all waypoints.

Obtain the planned path

  21:  !get planned path from NO.1 ;
  22:  !Mech-Vision project; 2nd ;
  23:  !argument (1) means getting pose ;
  24:  !in JPs ;
  25:  CALL MM_GET_VISP(1,1,51,52,53) ;

MM_GET_VISP: The command to obtain the path planned by Mech-Vision.
First 1: Specify the Mech-Vision project ID.
Second 1: Specify the pose type of obtained waypoints as joint positions.
51: The numeric register R[51], which stores the number of waypoints returned by the vision system.
52: The numeric register R[52], which stores the position of the Vision Move waypoint (picking waypoint) in the path.
53: The numeric register R[53], which stores the command execution status code.

The entire statement indicates that the robot obtains the planned path from the Mech-Vision project that has an ID of 1.

The returned planned path is saved to the robot memory and cannot be directly obtained. To access the planned path, you must store the planned path in a subsequent step.

  26:  !check whether planned path has ;
  27:  !been got from Mech-Vision ;
  28:  !successfully ;
  29:  IF R[53]<>1103,JMP LBL[99] ;

“IF A,JMP B”: If condition A is met, the program will jump to point B and execute the instructions there.
<>: Not equal.

The statement means that when the status code in R[53] is 1103, the robot has successfully obtained the planned path; otherwise, an exception has occurred in the vision system, and the program will jump to LBL[99] and execute the instructions there. The following is the code at LBL[99].

  66:  LBL[99:vision error] ;
  67:  !add error handling logic here ;
  68:  !according to different ;
  69:  !error codes ;
  70:  !e.g.: status=1003 means no ;
  71:  !point cloud in ROI ;
  72:  !e.g.: status=1002 means no ;
  73:  !vision results ;
  74:  PAUSE ;

You can perform the corresponding operation based on the specific error code. In this example program, all error codes are handled in the same way, by stopping the program execution using the PAUSE command.

Store the planned path

  30:  !save waypoints of the planned ;
  31:  !path to local variables one ;
  32:  !by one ;
  33:  CALL MM_GET_JPS(1,60,70,80) ;
  34:  CALL MM_GET_JPS(2,61,71,81) ;
  35:  CALL MM_GET_JPS(3,62,72,82) ;

MM_GET_JPS: The command to store the planned path.
1: Store the first waypoint.
60: The position register PR[60], which stores the joint positions of the first waypoint.
70: The numeric register R[70], which stores the label of the first waypoint.
80: The numeric register R[80], which stores the tool ID of the first waypoint.

The entire command “CALL MM_GET_JPS(1,60,70,80)” stores the joint positions, label, and tool ID of the first waypoint in the specified registers.

In this example, the path planned by Mech-Vision consists of three waypoints: the first waypoint is the approach waypoint of picking (PR[60]), the second waypoint is the picking waypoint (PR[61]), and the third waypoint is the departure waypoint of picking (PR[62]). Please store the planned path based on the actual Mech-Vision project.

Move to the approach waypoint of picking

  36:  !follow the planned path to pick ;
  37:  !move to approach waypoint ;
  38:  !of picking ;
  39:J PR[60] 50% FINE    ;

The robot moves to the approach waypoint of picking (the position represented by PR[60]).

Move to the picking waypoint

  40:  !move to picking waypoint ;
  41:J PR[61] 10% FINE    ;

The robot moves to the picking waypoint (the position represented by PR[61]).

Set DO commands to perform picking

  42:  !add object grasping logic here, ;
  43:  !such as "DO[1]=ON" ;
  44:  PAUSE ;

After the robot moves to the picking waypoint, you can set a DO command (such as “DO[1]=ON”) to control the robot to use the tool to perform picking. Please set DO commands based on the actual situation.

PAUSE indicates to stop the program execution. If you have added a statement to set a DO command, you can delete the PAUSE statement here.

Move to the departure waypoint of picking

  45:  !move to departure waypoint ;
  46:  !of picking ;
  47:J PR[62] 50% FINE    ;

The robot moves to the departure waypoint of picking (the position represented by PR[62]).

Move to the intermediate waypoint

  48:  !move to intermediate waypoint ;
  49:  !of placing ;
  50:J P[3] 50% CNT100    ;

P[3]: Specifies the target position to which the robot moves, which is the intermediate waypoint.
CNT: The robot approaches the target position but does not stop at that position; instead, it stops at the next position. The subsequent number indicates the degree to which the robot is approaching the target position; a larger value means it is further from the target position.

The entire command moves the robot in joint positions to a certain intermediate waypoint between the departure waypoint of picking and the approach waypoint of placing.

Adding intermediate waypoints can ensure smooth robot motion and avoid unnecessary collisions. You can add multiple intermediate waypoints according to the actual situation.
You need to teach the intermediate waypoint (P[3]) in advance. For information about how to teach the waypoint, see Teach Calibration Start Point in the calibration document.

Move the robot to the approach waypoint of placing

  51:  !move to approach waypoint ;
  52:  !of placing ;
  53:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;

P[4]: The placing waypoint. You need to teach the placing waypoint (P[4]) in advance. For information about how to teach the waypoint, see Teach Calibration Start Point in the calibration document.
“Tool_Offset,PR[2]”: The robot adjusts its position relative to the placing waypoint (P[4]) using the offset specified in PR[2]. In this case, you only need to set the Z-axis value of PR[2]. For instance, setting the Z-axis to -100 and all other values to 0 means the position is 100mm below the placing waypoint along the Z-axis (i.e., the robot will move 100mm above the placing waypoint to reach the approach waypoint of placing).

Adding approach waypoints of placing can prevent the robot from colliding with objects (such as bins) in the scene when moving. You can modify the PR[2] value based on the actual scenario to ensure that no collision occurs during the approaching process.

Move to the placing waypoint

  54:  !move to placing waypoint ;
  55:L P[4] 300mm/sec FINE    ;

The robot moves from the approach waypoint of placing to the placing waypoint.

Set DO commands to perform placing

  56:  !add object releasing logic here, ;
  57:  !such as "DO[1]=OFF" ;
  58:  PAUSE ;

After the robot moves to the placing waypoint, you can set a DO command (such as “DO[1]=OFF”) to control the robot to use the tool to perform placing. Please set DO commands based on the actual situation.

PAUSE indicates to stop the program execution. If you have added a statement to set a DO command, you can delete the PAUSE statement here.

Move the robot to the departure waypoint of placing

  59:  !move to departure waypoint ;
  60:  !of placing ;
  61:L P[4] 1000mm/sec FINE Tool_Offset,PR[2]    ;

The robot moves to a position above the placing waypoint and reaches the departure waypoint of placing.

Adding departure waypoints of placing can prevent the robot from colliding with objects (such as bins) in the scene when moving. You can modify the PR[2] value based on the actual scenario to ensure that no collision occurs during the departing process.

Move to the home position

  62:  !move back to robot home position ;
  63:J P[1] 100% FINE    ;

The robot moves from the departure waypoint of placing to the home position again.

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