样例程序20：MM_S20_Viz_PlanAllVision

程序简介

功能说明

机器人先触发Mech-Viz工程运行，然后通过循环获取所有规划路径，进而通过循环进行抓取与放置操作。在本样例中，相机拍照一次，Mech-Viz会规划出全部视觉结果的抓取路径。通常用于一拍多抓的场景。

文件路径

Mech-Vision和Mech-Viz软件安装目录下Communication Component/Robot_Interface/ABB/sample/MM_S20_Viz_PlanAllVision。

对于RobotWare6系统，文件后缀为.mod。对于RobotWare7系统，用户需将.mod修改为.modx。

所需工程

Mech-Vision工程和Mech-Viz工程

Mech-Viz工程需满足以下配置：视觉移动步骤的复用视觉结果参数需处于勾选状态。
sample20 1

使用前提

  1. 已完成标准接口通信配置。

  2. 已完成自动标定。
此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。

程序解读

以下为MM_S20_Viz_PlanAllVision样例程序的代码及相关解释说明。

与MM_S2_Viz_Basic样例相比，本样例仅新增了通过循环获取所有规划路径并进行抓取与放置的功能（加粗部分的代码）。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 
MODULE MM_S20_Viz_PlanAllVision
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project, plan all vision
! results and get all planned results using command 210
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=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 movetype{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num toolnum{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num speed{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 VAR robtarget pickpoint:=[[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 VAR robtarget pick_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 VAR 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 VAR 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}:=
[
    [[-4.5438,26.6029,0.467,-0.1719,62.9165,-5.0819],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-4.5664,32.0439,7.6047,-0.1988,50.3379,-5.0558],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-4.5363,25.4009,-2.7016,-0.1659,67.2872,-5.0885],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.7719,15.0701,16.7232,0.1053,57.9831,-111.82],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.7567,21.7711,23.8763,0.1281,44.1289,-111.841],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-20.777,13.5249,13.6202,0.1005,62.6312,-111.816],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3363,10.0698,23.0021,0.0017,56.8211,89.2201],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3365,17.2854,30.3891,0.0022,42.2185,89.2197],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0.3362,8.4182,19.8363,0.0017,61.6386,89.2202],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.6715,8.5325,24.7355,0.0089,56.6276,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.6701,15.8879,32.2202,0.0112,41.7875,265.897],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[-2.672,6.8572,21.5409,0.0085,61.4976,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.387,12.8326,19.5238,-0.0095,57.6221,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.3856,19.7174,26.8272,-0.0117,43.434,110.351],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[21.3874,11.2544,16.3788,-0.0091,62.3455,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.5881,41.5724,-23.6476,-0.3127,72.1244,189.431],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.5502,45.3582,-15.4893,-0.3431,60.1803,189.468],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[10.6008,40.9927,-27.4632,-0.306,76.5197,189.419],[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_20()
    !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 Mech-Viz project
    MM_Start_Viz 2,snap_jps;
    !get planned path
    MM_Get_PlanData 0, 3, 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 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_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
    ENDFOR
    !parse pick cycle count, here suppose 3 points per planned path
    pick_cnt:= pose_num DIV 3;
    residual:= pose_num MOD 3;
    !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)*3;
        !move to intermediate waypoint of picking
        MoveJ pick_waypoint,v1000,z50,gripper1;
        !follow the planned path to pick
        MoveAbsJ jps{1+count},v1000,fine,gripper1;
        MoveAbsJ jps{2+count},v1000,fine,gripper1;
        !add object grasping logic here, such as "setdo DO_1, 1;";
        Stop;
        MoveAbsJ jps{3+count},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 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

上述样例程序代码对应的流程如下图所示。

sample20

下表为新增功能的逻辑解读。用户单击指令名称的超链接便可查看该指令的详细说明。

流程 代码及说明

获取规划路径

 
!get planned path
MM_Get_PlanData 0, 3, pose_num, vis_pose_num, status;

  • MM_Get_PlanData：获取规划路径的指令。通过该指令获取的视觉移动路径点，除了包含位姿外，还将包含视觉移动规划数据或自定义数据（如有），而通过MM_Get_VizData指令获取的视觉移动路径点将不包含视觉移动规划数据或自定义数据。

  • 0：从Mech-Viz获取规划路径。

  • 3：预期返回的数据格式，即以“位姿（关节角形式）,移动类型,末端工具编号,速度,Mech-Viz视觉移动规划数据,自定义数据项1,…,自定义数据项N”格式返回数据。

  • pose_num：该变量保存视觉系统返回的路径点个数。

  • vis_pose_num：该变量保存最后一个视觉移动路径点（抓取点）在总规划路径中的位置编号。

  • status：该变量保存指令执行的状态码。

通过循环转存规划路径

 
FOR i FROM 1 TO pose_num DO
    count:=i;
    MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
ENDFOR

  • 第1行：FOR表示以下为循环程序片段；i用于控制循环的次数（即i从1开始，每次循环后自增1，直到大于pose_num，结束循环）；pose_num为MM_Get_PlanData指令的第三个参数，该参数表示视觉系统返回的路径点个数。

  • 第2行：将i赋值给count，count表示当前路径点在总规划路径中的编号。

  • 第3行：MM_Get_PlanJps指令表示将某个路径点的关节角、移动类型、末端工具编号和速度分别转存到指定变量，因此整条指令表示将第count编号的路径点的关节角、移动类型、末端工具编号和速度依次转存到JPS{count}、movetype{count}、toolnum{count}和speed{count}变量。

计算pick_cnt和residual

 
!parse pick cycle count, here suppose 3 points per planned path
pick_cnt:= pose_num DIV 3;
residual:= pose_num MOD 3;

本样例假设每次规划的抓取路径包含3个路径点，“pose_num DIV 3”表示pose_num整除3的商，“pose_num MOD 3”表示pose_num整除3的余数。pick_cnt表示规划的总抓取次数。如果residual不为0，则表示某次规划的抓取路径点个数少于3，即路径规划出现异常，需重新进行规划。

判断规划路径是否出错

 
IF (pick_cnt<1) OR (residual<>0) THEN
    Stop;
    GOTO RECAP;
ENDIF

如果抓取次数pick_cnt小于1，或residual不为0，则表示路径规划出现异常，用户需在此处添加处理逻辑，例如重新触发Mech-Viz工程并获取规划路径。

通过循环进行抓取和放置

 
FOR i FROM 1 TO pick_cnt DO
    count:=(i-1)*3;
    !move to intermediate waypoint of picking
    MoveJ pick_waypoint,v1000,z50,gripper1;
    !follow the planned path to pick
    MoveAbsJ jps{1+count},v1000,fine,gripper1;
    MoveAbsJ jps{2+count},v1000,fine,gripper1;
    !add object grasping logic here, such as "setdo DO_1, 1;";
    Stop;
    MoveAbsJ jps{3+count},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 to intermediate waypoint of placing
    MoveJ drop_waypoint,v1000,z50,gripper1;
ENDFOR

上述代码表示，在FOR循环中，机器人移动到每次规划的3个路径点，从而完成抓取操作，然后接着执行放置操作。i用于控制循环的次数，即i从1开始，每次循环后自增1，直到大于抓取次数pick_cnt，结束循环。每当i增加1，则count增加3，从而使{1+count}~{3+count}表示每次规划的3个路径点在总规划路径中的编号。

我们重视您的隐私

我们使用 cookie 为您在我们的网站上提供最佳体验。继续使用该网站即表示您同意使用 cookie。如果您拒绝，将使用一个单独的 cookie 来确保您在访问本网站时不会被跟踪或记住。

  • Preference
  • 同意
  • 不同意