样例程序8：MM_S8_Viz_Subtask

程序简介

功能说明

本样例由两个程序组成，其中子任务负责触发Mech-Viz工程运行并获取规划路径；主任务负责根据规划路径移动机器人，且在机器人放置时触发子任务运行，从而提前规划下一轮路径，以缩短节拍。

本样例与MM_S10_Viz_Subtask样例十分相似，仅是主任务触发子任务的时机不同，因此，建议用户对比学习这两个样例，以更好地理解它们之间的细微差别。

文件路径

子任务：Mech-Vision和Mech-Viz软件安装目录下Communication Component/Robot_Interface/ABB/sample/sub_prog/MM_S8_Sub。

主任务：Mech-Vision和Mech-Viz软件安装目录下Communication Component/Robot_Interface/ABB/sample/MM_S8_Viz_Subtask。

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

所需工程

Mech-Vision工程和Mech-Viz工程

使用前提

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

  2. 已完成自动标定。

  • 此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。

  • 主任务属于前台程序。子任务属于后台程序，需要在机器人系统开机后自动运行。

程序解读

以下为子任务的代码及相关解释说明。

在子任务中，触发Mech-Viz工程运行并获取规划路径的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 
MODULE MM_S8_Sub
!----------------------------------------------------------
! FUNCTION: run Mech-Viz project and get planned path
! in subtask (run together with MM_S8_Viz_Subtask)
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define variables
PERS num pose_num_a:=5;
PERS num vis_pose_num_a:=3;
PERS num toolid_a{5}:=[0,0,0,0,0];
PERS num label_a{5}:=[0,0,0,0,0];
PERS num status_a:=2100;
PERS bool flag_vis_a:=FALSE;
CONST jointtarget snap_jps_a:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS jointtarget jps_a{5}:=
[
    [[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]],
    [[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[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]]
];

PROC main()
    flag_vis_a:=FALSE;
    !initialize communication parameters (initialization is required only once)
    MM_Init_Socket "127.0.0.1",50000,300;
    WHILE TRUE DO
        IF flag_vis_a=TRUE THEN
            MM_Open_Socket;
            !trigger Mech-Viz project
            MM_Start_Viz 2,snap_jps_a;
            !get planned path, 1st argument (1) means getting pose in JPs
            MM_Get_VizData 1, pose_num_a, vis_pose_num_a, status_a;
            !check whether planned path has been got from Mech-Viz successfully
            IF status_a=2100 THEN
                !save waypoints of the planned path to local variables one by one
                MM_Get_Jps 1,jps_a{1},label_a{1},toolid_a{1};
                MM_Get_JPS 2,jps_a{2},label_a{2},toolid_a{2};
                MM_Get_JPS 3,jps_a{3},label_a{3},toolid_a{3};
            ENDIF
            MM_Close_Socket;
            flag_vis_a:=FALSE;
        ENDIF
    ENDWHILE
ENDPROC
ENDMODULE

上述代码表示，子任务在运行时，首先将flag_vis_a设置为FALSE，并初始化通信参数，然后通过WHILE循环持续监听flag_vis_a值。

  • 当flag_vis_a为TRUE时，子任务发触发Mech-Viz工程运行并获取规划路径，然后将flag_vis_a设置为FALSE。

  • 当flag_vis_a为FALSE时，子任务持续判断flag_vis_a的值。

以下为主任务的代码及相关解释说明。

在主任务中，根据规划路径进行抓取与放置的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 
MODULE MM_S8_Viz_Subtask
!----------------------------------------------------------
! FUNCTION: run Mech-Viz project and get planned path
! in subtask (run together with MM_S8_Sub)
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
PERS bool flag_vis_a:=FALSE;
PERS num status_a:=2100;
PERS num pose_num_a:=5;
PERS num toolid_a{5}:=[0,0,0,0,0];
PERS num label_a{5}:=[0,0,0,0,0];
PERS num vis_pose_num_a:=3;
LOCAL VAR num count:=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 PERS robtarget pick_wait_point:=[[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]];
PERS jointtarget jps_a{5}:=
[
    [[0,0,0,0,89,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0,0,0,0,89,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[0,0,0,0,89,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
    [[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[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_8()
    !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;
    !trigger Mech-Viz project and get planned path
    trigger_vis_a;
LOOP:
    !move to wait position for picking
    MoveL pick_wait_point,v1000,fine,gripper1;
    !wait until subtask program finished
    WaitUntil(flag_vis_a=FALSE);
    !check whether planned path has been got from Mech-Viz successfully
    IF status_a <> 2100 THEN
       !add error handling logic here according to different error codes
       !e.g.: status=2038 means no point cloud in ROI
       Stop;
    ENDIF
    !follow the planned path to pick
    !move to approach waypoint of picking
    MoveAbsJ jps_a{1},v1000,fine,gripper1;
    !move to picking waypoint
    MoveAbsJ jps_a{2},v300,fine,gripper1;
    !add object grasping logic here, such as "setdo DO_1, 1;"
    Stop;
    !move to departure waypoint of picking
    MoveAbsJ jps_a{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;
    !trigger Mech-Viz project and get planned path in advance
    trigger_vis_a;
    !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;
    GOTO LOOP;
ENDPROC
PROC trigger_vis_a()
    flag_vis_a:=TRUE;
ENDPROC
ENDMODULE

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

sample8

下表为主任务核心代码的逻辑解读。

流程 代码及说明

触发Mech-Viz工程运行并获取规划路径

 
!trigger Mech-Viz project and get planned path
trigger_vis_a;

上述代码表示，主任务调用trigger_vis_a()函数。该函数的代码如下所示。

PROC trigger_vis_a()
    flag_vis_a:=TRUE;

上述代码表示，在trigger_vis_a()函数中将flag_vis_a设置为TRUE，此时子任务检测到flag_vis_a为TRUE，便触发Mech-Viz工程运行并获取规划路径。

通过循环（抓取→触发下一轮规划→放置）提前规划下一轮路径

 
LOOP:
    ...
    GOTO LOOP;

上述代码表示，主任务循环执行LOOP与GOTO LOOP之间的代码。

!move to wait position for picking
MoveL pick_wait_point,v1000,fine,gripper1;
!wait until subtask program finished
WaitUntil(flag_vis_a=FALSE);

上述代码表示，机器人移动到抓取前的一个等待点，等待子任务运行完毕（即flag_vis_a从TRUE变为FALSE），从而确保已获取并转存规划路径。

!follow the planned path to pick
!move to approach waypoint of picking
MoveAbsJ jps_a{1},v1000,fine,gripper1;
!move to picking waypoint
MoveAbsJ jps_a{2},v300,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!move to departure waypoint of picking
MoveAbsJ jps_a{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;

上述代码表示，机器人根据规划的路径先移动到抓取接近点（jps_a{1}），然后移动到抓取点（jps_a{2}），进行抓取（例如，setdo DO_1, 1;），再依次移动到抓取离开点（jps_a{3}）、放置过渡点（drop_waypoint）、放置接近点（RelTool(drop,0,0,-100)）。

!trigger Mech-Viz project and get planned path in advance
trigger_vis_a;

上述代码表示，主任务程序再次调用trigger_vis_a()函数，即再次触发Mech-Viz工程运行并获取规划路径。此处机器人已处于放置区域，因此机器人此时可以提前规划下一轮抓取路径，而不必等待放置完成后再规划下一轮抓取路径。

!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;

上述代码表示，机器人先移动到放置点（drop），然后进行放置操作（例如，setdo DO_1, 0;），然后依次移动到放置离开点（RelTool(drop,0,0,-100)）、Home点。

我们重视您的隐私

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

  • Preference
  • 同意
  • 不同意