样例程序19：MM_S19_Vis_PlanAllVision
程序简介
功能说明 |
机器人先触发Mech-Vision工程运行，然后通过循环获取所有规划路径，进而通过循环进行抓取与放置操作。在本样例中，相机拍照一次，Mech-Vision会规划出全部视觉结果的抓取路径。通常用于一拍多抓的场景。 |
||
文件路径 |
Mech-Vision和Mech-Viz软件安装目录下
|
||
所需工程 |
|||
使用前提 |
|
| 此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。 |
程序解读
以下为MM_S19_Vis_PlanAllVision样例程序的代码及相关解释说明。
| 与MM_S3_Vis_Path样例相比，本样例仅新增了通过循环获取所有规划路径并进行抓取与放置的功能（加粗部分的代码）。因此，下文不再重复解释与MM_S3_Vis_Path样例相同部分的代码（详情请参考MM_S3_Vis_Path样例说明）。 |
MODULE MM_S19_Vis_PlanAllVision
!----------------------------------------------------------
! FUNCTION: trigger Mech-Vision project, plan all vision
! results and get all planned paths
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num toolid{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,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 label{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 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{20}:=
[
[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[65.8654,94.6966,32.2878,-3.7036,-39.7668,-111.261],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[66.3351,103.661,20.3594,-3.9153,-36.8271,-110.505],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[65.8654,94.6966,32.2878,-3.7036,-39.7668,-111.261],[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]],
[[0,0,0,0,90,0],[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]],
[[-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]],
[[0,0,0,0,90,0],[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]],
[[28.8422,90.4027,-13.4878,13.7086,10.2751,-164.54],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[29.122,100.975,-26.3271,11.2142,12.4744,-161.72],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[28.8422,90.4027,-13.4878,13.7086,10.2751,-164.54],[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]],
[[0,0,0,0,90,0],[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]],
[[29.0915,100.961,-26.4037,12.003,12.8098,-162.514],[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_19()
!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 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 result
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_Jps count,jps{count},label{count},toolid{count};
ENDFOR
!parse pick cycle count, here suppose 5 points per planned path
pick_cnt:= pose_num DIV 5;
residual:= pose_num MOD 5;
!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)*5;
!follow the planned path to pick
MoveAbsJ jps{1+count},v1000,z50,gripper1;
MoveAbsJ jps{2+count},v1000,fine,gripper1;
MoveAbsJ jps{3+count},v1000,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
MoveAbsJ jps{4+count},v1000,fine,gripper1;
MoveAbsJ jps{5+count},v1000,z50,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上述样例程序代码对应的流程如下图所示。
下表为新增功能的逻辑解读。用户单击指令名称的超链接便可查看该指令的详细说明。
| 流程 | 代码及说明 |
|---|---|
通过循环转存规划路径 |
|
计算pick_cnt和residual |
本样例假设每次规划的抓取路径包含5个路径点，“pose_num DIV 5”表示pose_num整除5的商，“pose_num MOD 5”表示pose_num整除5的余数。pick_cnt表示规划的总抓取次数。如果residual不为0，则表示某次规划的抓取路径点个数少于5，即路径规划出现异常，需重新进行规划。 |
判断规划路径是否出错 |
如果抓取次数pick_cnt小于1，或residual不为0，则表示路径规划出现异常，用户需在此处添加处理逻辑，例如重新触发Mech-Vision工程并获取规划路径。 |
通过循环进行抓取和放置 |
上述代码表示，在FOR循环中，机器人移动到每次规划的5个路径点，从而完成抓取操作，然后接着执行放置操作。i用于控制循环的次数，即i从1开始，每次循环后自增1，直到大于抓取次数pick_cnt，结束循环。每当i增加1，则count增加5，从而使{1+count}~{5+count}表示每次规划的5个路径点在总规划路径中的编号。 |