样例程序3：MM_S3_Vis_Path
程序简介
功能说明 |
机器人触发Mech-Vision工程运行，然后获取规划路径，进而执行抓取和放置操作。 |
||
文件路径 |
Mech-Vision和Mech-Viz软件安装目录下
|
||
所需工程 |
|||
使用前提 |
|
| 此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。 |
程序解读
以下为MM_S3_Vis_Path样例程序的代码及相关解释说明。
MODULE MM_S3_Vis_Path
!----------------------------------------------------------
! FUNCTION: trigger Mech-Vision project and get planned path
! 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 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}:=
[
[[-9.7932,85.483,6.0459,-20.5518,-3.0126,-169.245],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-9.653,95.4782,-4.3661,-23.6568,-2.6275,-165.996],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-9.653,95.4782,-4.3661,-23.6568,-2.6275,-165.996],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-9.653,95.4782,-4.3661,-23.6568,-2.6275,-165.996],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-9.7932,85.483,6.0459,-20.5518,-3.0126,-169.245],[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_3()
!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;
!trigger NO.1 Mech-Vision project
MM_Start_Vis 1,0,2,snap_jps;
!get planned path from NO.1 Mech-Vision project; 2nd argument (1) means getting pose in JPs
MM_Get_VisPath 1,1,pose_num,vis_pose_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
Stop;
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;
ENDPROC
ENDMODULE上述样例程序代码对应的流程如下图所示。
下表为上述程序的逻辑解读。用户单击指令名称的超链接便可查看该指令的详细说明。
| 流程 | 代码及说明 | ||
|---|---|---|---|
定义变量 |
定义局部变量。上述局部变量仅在本程序中生效。 用户需提前示教Home点（home变量）、传入Mech-Vision工程的位姿（snap_jps变量）、拍照点（camera_capture变量）、放置的过渡点（drop_waypoint变量）、放置点（drop变量），并设置工具数据（gripper1变量）。
|
||
设置加速度和速度 |
AccSet 50, 50：将加速度限制在正常值的50%，加速度增减率限制在正常值的50%。 VelSet 50, 1000：将速率设置为编程速率的50%，将最大TCP速率设置为1000mm/s。 |
||
移动到Home点 |
因此，整条指令表示机器人准确移动到示教的Home点（不受外部轴影响），速度为3000mm/s。 |
||
初始化通信参数 |
机器人通过MM_Init_Socket指令指定通信对象（工控机）的IP地址（127.0.0.1）、端口号（50000）以及超时等待时间（300秒）。
|
||
移动到拍照点 |
因此，整条指令表示机器人以直线移动方式准确移动至示教的拍照点，速度为1000mm/s。 |
||
建立通信 |
机器人通过MM_Open_Socket指令与视觉系统之间建立TCP协议通信连接。 |
||
触发运行Mech-Vision工程 |
因此，整条指令表示机器人触发视觉系统运行编号为1的Mech-Vision工程，且期望Mech-Vision工程返回所有路径点。 |
||
获取规划路径 |
因此，整条指令表示机器人获取Mech-Vision工程1返回的规划路径。
当状态码status为1103时，则机器人成功获取到规划路径；否则视觉系统发生异常。用户可根据具体异常状态码做相应的处理。 |
||
关闭通信 |
机器人通过MM_Close_Socket指令断开与视觉系统之间的TCP协议通信连接。 |
||
转存规划路径 |
因此，“MM_Get_JPS 1,jps{1},label{1},toolid{1};”整条指令表示将第一个路径点的关节角、标签和末端工具编号分别转存至指定变量。
|
||
移动到抓取接近点 |
机器人移动到抓取接近点（即jps{1}所表示的位置）。 |
||
移动到抓取点 |
机器人移动到抓取点（即jps{2}所表示的位置）。 |
||
设置DO执行抓取 |
机器人移动到抓取点后，通过设置DO指令（例如“setdo DO_1, 1;”），控制末端工具进行抓取。用户需根据实际场景增加设置DO的操作。
|
||
移动到抓取离开点 |
机器人移动到抓取离开点（即jps{3}所表示的位置）。 |
||
移动到中间过渡点 |
因此，整条指令表示机器人以关节角移动方式移动至抓取离开点与放置接近点之间的某个过渡点，速度为1000mm/s，转弯半径为50mm。
|
||
移动到放置接近点 |
因此，整条指令表示机器人移动到放置点上方的100mm处，即到达放置接近点。
|
||
移动到放置点 |
机器人从放置接近点移动到放置点。 |
||
设置DO执行放置 |
机器人移动到放置点后，通过设置DO指令（例如“setdo DO_1, 0;”），控制末端工具进行放置。用户需根据实际场景增加设置DO的操作。
|
||
移动到放置离开点 |
机器人移动到放置点上方的100mm处，即到达放置离开点。
|
||
移动到Home点 |
机器人从放置离开点重新移动到Home点。 |