样例程序7：MM_S7_Viz_SwitchTCP
程序简介
功能说明 |
机器人触发Mech-Viz工程运行，并获取路径规划结果，然后根据抓取点的末端工具编号切换相应的末端工具，进而执行抓取和放置操作。 |
||
文件路径 |
Mech-Vision和Mech-Viz软件安装目录下
|
||
所需工程 |
Mech-Vision工程和Mech-Viz工程 |
||
使用前提 |
|
| 此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。 |
程序解读
以下为MM_S7_Viz_SwitchTCP样例程序的代码及相关解释说明。
| 与MM_S2_Viz_Basic样例相比，本样例主要新增了根据末端工具编号切换工具的功能（加粗部分的代码）。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 |
MODULE MM_S7_Viz_SwitchTCP
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project and get planned path,
! switch TCP according to the label
! 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 robtarget pos{5}:=
[
[[502.502,99.5586,399.968],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[500,100,300],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[500,100,300],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[500,100,300],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[502.502,99.5586,399.968],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]]
];
!define LOCAL tooldata variables
LOCAL PERS tooldata cur_gripper:=[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];
LOCAL PERS tooldata gripper{5}:=,[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]]];
PROC Sample_7()
!set the acceleration parameters
AccSet 50, 50;
!set the velocity parameters
VelSet 50, 1000;
!move to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,cur_gripper;
!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,cur_gripper;
!open socket connection
MM_Open_Socket;
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps;
!get planned path, 1st argument (2) means getting pose in TCP
MM_Get_VizData 2, 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 waypoints of the planned path to local variables one by one
MM_Get_Pose 1,pos{1},label{1},toolid{1};
MM_Get_Pose 2,pos{2},label{2},toolid{2};
MM_Get_Pose 3,pos{3},label{3},toolid{3};
!reset tool signals according to received toolId
IF toolid{vis_pose_num}=1 Then
cur_gripper:=gripper{toolid{vis_pose_num}};
!reset tool signal;
ELSEIF toolid{vis_pose_num}=2 Then
cur_gripper:=gripper{toolid{vis_pose_num}};
!reset tool signal;
ELSE
Stop;
!reset tool signal;
ENDIF
!follow the planned path to pick
!move to approach waypoint of picking
MoveL pos{1},v1000,fine,cur_gripper;
!move to picking waypoint
MoveL pos{2},v300,fine,cur_gripper;
!add object grasping logic here
IF toolid{vis_pose_num}=1 Then
Stop;
!open tool signal;
ELSEIF toolid{vis_pose_num}=2 Then
Stop;
!open tool signal;
ELSE
Stop;
!open tool signal;
ENDIF
!move to departure waypoint of picking
MoveL pos{3},v1000,fine,cur_gripper;
!move to intermediate waypoint of placing
MoveJ drop_waypoint,v1000,z50,cur_gripper;
!move to approach waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,cur_gripper;
!move to placing waypoint
MoveL drop,v300,fine,cur_gripper;
!add object releasing logic here
IF toolid{vis_pose_num}=1 Then
Stop;
!reset tool signal;
ELSEIF toolid{vis_pose_num}=2 Then
Stop;
!reset tool signal;
ELSE
Stop;
!reset tool signal;
ENDIF
!move to departure waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,cur_gripper;
!move back to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,cur_gripper;
ENDPROC
ENDMODULE上述样例程序代码对应的流程如下图所示。
下表为新增代码的逻辑解读。
| 流程 | 代码及说明 | ||
|---|---|---|---|
获取规划路径 |
因此，整条指令表示机器人获取Mech-Viz工程返回的规划路径。
|
||
转存规划路径 |
因此，“MM_Get_Pose 1,pos{1},label{1},toolid{1}”整条指令表示将第一个路径点的工具位姿、标签和末端工具编号分别转存至指定变量。
|
||
根据末端工具编号切换末端工具 |
toolid{vis_pose_num}表示抓取点的末端工具编号值。用户可根据toolid{vis_pose_num}变量值，决定机器人移动到对应位姿（抓取点）时所使用的末端工具，即根据末端工具编号切换末端工具。上述代码的逻辑说明如下所示。
当机器人移动至抓取点时，用户可根据末端工具编号打开相应工具对应的DO信号，例如样例中如下代码。 当机器人移动至放置点时，用户可根据末端工具编号关闭并重置相应工具对应的DO信号，例如样例中如下代码。 |