样例程序16：MM_S16_Viz_GetDirection

程序简介

功能说明

机器人在获取规划路径并完成抓取后，可根据工件组朝向采取不同的放置策略。

文件路径

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

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

所需工程

Mech-Vision工程和Mech-Viz工程（末端工具类型为拆垛吸盘）

使用前提

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

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

程序解读

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

与MM_S15_Viz_GetDoList样例相比，本样例仅修改了如下加粗部分的代码。因此，下文不再重复解释与MM_S15_Viz_GetDoList样例相同部分的代码（详情请参考MM_S15_Viz_GetDoList样例说明）。 
MODULE MM_S16_Viz_GetDirection
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project, then get planned path
! and get box direction 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 box_direction:=0;
LOCAL VAR num movetype{5}:=[0,0,0,0,0];
LOCAL VAR num toolnum{5}:=[0,0,0,0,0];
LOCAL VAR num speed{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 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 drop_waypoint_1:=[[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_1:=[[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_2:=[[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_2:=[[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}:=
[
    [[1.1835,39.2938,-17.0883,0.1382,67.7901,176.701],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[1.2008,45.8522,-13.6729,0.1512,57.8163,176.689],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[1.1835,39.2938,-17.0883,0.1382,67.7901,176.701],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[36.2634,-36.6956,48.5019,-1.0197,78.1304,356.473],[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_16()
    !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 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
    !get gripper control signal list
    MM_Get_Dolist 0,0;
    !close socket connection
    MM_Close_Socket;
    !save waypoints of the planned path to local variables one by one
    FOR i FROM 1 TO pose_num DO
        count:=i;
        MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
    ENDFOR
    !follow the planned path to pick
    FOR j FROM 1 TO pose_num DO
        count:=j;
        MoveAbsJ jps{count},v1000,fine,gripper1;
        !set gripper control signal when current waypoint is picking waypoint
        IF count=vis_pose_num THEN
            !add object grasping logic here
            Stop;
            !set gripper control signal
            !MM_Set_DoList 0, 1, go16_1;
            !MM_Set_DoList 0, 2, go16_2;
            !MM_Set_DoList 0, 3, go16_3;
            !MM_Set_DoList 0, 4, go16_4;
        ENDIF
    ENDFOR
    !get box direction status from planned results
    box_direction:=MM_Plan_Results{17};
    !place the box according to its direction
    IF box_direction=0 THEN
        !move to intermediate waypoint of placing
        MoveJ drop_waypoint_1,v1000,z50,gripper1;
        !move to approach waypoint of placing
        MoveL RelTool(drop_1,0,0,-100),v1000,fine,gripper1;
        !move to placing waypoint
        MoveL drop_1,v300,fine,gripper1;
        !add object releasing logic here, such as "setdo DO_1, 0;"
        Stop;
        !move to departure waypoint of placing
        MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
    ELSE
        !move to intermediate waypoint of placing
        MoveJ drop_waypoint_2,v1000,z50,gripper1;
        !move to approach waypoint of placing
        MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
        !move to placing waypoint
        MoveL drop_2,v300,fine,gripper1;
        !add object releasing logic here, such as "setdo DO_1, 0;"
        Stop;
        !move to departure waypoint of placing
        MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
    ENDIF
    !move back to robot home position
    MoveAbsJ home\NoEOffs,v3000,fine,gripper1;
ENDPROC
ENDMODULE

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

sample16

下表是对加粗代码的逻辑解读。用户单击指令名称的超链接便可查看该指令的详细说明。

流程 代码及说明

获取工件组朝向

 
!get box direction status from planned results
box_direction:=MM_Plan_Results{17};
对于Mech-Viz工程而言，视觉移动规划数据是指“视觉移动”步骤规划的数据，包括被抓取工件的标签、已抓取的工件总数、本次抓取的工件数量、吸盘边角号、TCP偏移量、工件组朝向、单个工件朝向、工件组尺寸。

机器人通过MM_Get_PlanData指令获取路径点的视觉移动规划数据，然后再通过MM_Get_PlanJps指令将路径点的视觉移动规划数据从机器人内存中转存至MM_Plan_Results全局数组中。

在MM_Plan_Results全局数组中，MM_Plan_Results{17}表示工件组与吸盘长边的朝向关系，0表示平行，1表示垂直。

上述代码表示，将MM_Plan_Results{17}赋值给box_direction变量，则box_direction变量将表示工件组朝向。

根据工件组朝向采取不同的放置策略

 
IF box_direction=0 THEN
    !move to intermediate waypoint of placing
    MoveJ drop_waypoint_1,v1000,z50,gripper1;
    !move to approach waypoint of placing
    MoveL RelTool(drop_1,0,0,-100),v1000,fine,gripper1;
    !move to placing waypoint
    MoveL drop_1,v300,fine,gripper1;
    !add object releasing logic here, such as "setdo DO_1, 0;"
    Stop;
    !move to departure waypoint of placing
    MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
ELSE
    !move to intermediate waypoint of placing
    MoveJ drop_waypoint_2,v1000,z50,gripper1;
    !move to approach waypoint of placing
    MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
    !move to placing waypoint
    MoveL drop_2,v300,fine,gripper1;
    !add object releasing logic here, such as "setdo DO_1, 0;"
    Stop;
    !move to departure waypoint of placing
    MoveL RelTool(drop_2,0,0,-100),v1000,fine,gripper1;
ENDIF

上述代码表示，如果工件组与吸盘长边平行（即box_direction为0），则将该工件组放置在drop_1处；否则，将该工件组放置在drop_2处。

我们重视您的隐私

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

  • Preference
  • 同意
  • 不同意