ROKAE 抓取样例程序

本节将介绍用于完成简单识别并移动的机器人样例程序。

样例程序介绍

机器人简单抓取和放置的样例程序位于 MM_Project 文件夹中的 main.mod 文件中。

使用 Mech-Vision 进行视觉识别并获取结果

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
// **********************************************************
// FUNCTION: Eye to Hand simple pick and place with Vision
// **********************************************************
LOCAL PROC Vision_Sample_1()
        AccSet 100,100
        MoveAbsJ home_point,v3000,fine,tool0 //move robot home position
        MoveL camera_capture,v500,fine,tool0
        Pos_Num = 0

        // Set ip address of IPC
        IP_Address = "192.168.2.222"
        Server_Port = 50000
        MM_Timeout = 60
        MM_Init_Socket()
        Wait 0.1

        // Set vision recipe
        Job = 1
        Model_Number = 1
        MM_Switch_Model()

        // Run vision project
        Job = 1
        Pos_Num_Need = 0
        SendPos_Type = 1
        MM_Start_Vis()
        Wait 1
        Job = 1
            MM_Get_VisData()
        IF(MM_Status != 1100)
                EXIT
        ENDIF
        Serial = 1
        MM_Get_Pose()
            robtarget _P = MM_P
        MoveL RelTool(_P,0,0,-100,0,0,0),v1000,fine,tool01
        MoveL MM_P,v300,fine,tool01
        Pause

        // Add object grasping logic here.
        MoveL Offs(_P,0,0,100), v1000, fine, tool01
        MoveL way_point1,v1000,z50,tool01
        MoveL RelTool(drop,0,0,-100,0,0,0),v1000,fine,tool01
        MoveL drop,v300,fine,tool01
        Pause

        // Add object releasing logic here.
        MoveL Offs(drop,0,0,100), v1000, fine, tool01
        MoveAbsJ home_point,v3000,fine,tool0
        RETURN

ENDPROC

程序逻辑解读

  • 第 6 行：移动机器人至 Home 点。

  • 第 7 行：移动到相机拍照位置。

  • 第 11 行：IP_Address 表示工控机的 IP 地址。

  • 第 12 行：Server_Port 表示工控机的端口号。

  • 第 13 行：MM_Timeout 表示通信超时等待时间。

  • 第 14 行：调用 MM_Init_Socket 对通信进行初始化设置。

  • 第 18 行：Job 表示 Mech-Vision 工程编号。

  • 第 19 行：Model_Number 表示 Mech-Vision 工程中配方模板的编号。

  • 第 20 行：如需设置 Mech-Vision 工程配方，可以在此处调用 MM_Switch_Model 设置工程配方。

  • 第 24 行：Pos_Num_Need 表示期望从 Mech-Vision 得到的视觉点数量；其值为 0，表示从 Mech-Vision 工程取得识别结果中所有的视觉点。

  • 第 25 行：SendPos_Type 表示以“机器人当前关节角+当前法兰”形式向视觉系统传入机器人位姿。

  • 第 26 行：MM_Start_Vis 触发 Mech-Vision 工程运行。

  • 第 27 行：等待1秒。Eye To Hand 模式下，若在 MM_Start_Vis 和 MM_Get_VisData 之间有移动步骤（MoveL 或 MoveJ），则不需要在此设置等待；Eye In Hand模式下则必须等待一秒，在相机拍照完成之前保持相机静止。

  • 第 29 行：获取 Mech-Vision 工程计算出的位姿。

  • 第 30 行：判断获取位姿的状态码是否正常，若状态码返回异常，用户需要进行处理，例如停止机器人。

  • 第 34 行：MM_Get_Pose 获取抓取点位。由于ROAKE自定义函数不支持传入形参，所以指定全局变量 MM_P 存储 MM_Get_Pose 计算结果。使用局部变量 _P = MM_P，将获取的位姿数据转存到用户定义的位置变量。

  • 第 37 行：移动到抓取点。

  • 第 42 行：移动到抓取点和放置点之间的安全区域。

  • 第 44 行：移动到放置点。

用户自定义部分

  • 定义工具中心点 TCP

    tool01: 用户可在示教器的标定界面中修改 tool01 的值来调整 TCP。

  • 示教 Home 点

    MoveAbsJ home_point, 用户需将 Home 点位姿记录在 home_point 位姿变量中。

  • 示教拍照点

    MoveL camera_capture, 用户需将拍照点位姿记录在 camera_capture 位姿变量中。

  • 示教过渡点

    MoveL way_point1, 用户可添加一个或几个中间点，使机器人从抓取点移动至放置点过程中，无碰撞。

  • 示教放置点

    MoveL drop，用户需将放置点位姿记录在 drop 位姿变量中。

  • 定义抓取前后，放置前后的接近距离

    • 接近抓取点

      MoveL RelTool(_P,0,0,-100,0,0,0),v1000,fine,tool01
      抓取点沿工具 Z 轴负方向移动 100mm，用户需要修改偏移量，保证接近过程无碰撞。

    • 离开抓取点

      MoveL Offs(_P,0,0,100), v1000, fine, tool01
      从抓取点沿物体坐标 Z 轴正方向移动 100mm，用户需要修改偏移量，保证离开过程无碰撞。

    • 接近放置点

      MoveL RelTool(drop,0,0,-100,0,0,0),v1000,fine,tool01
      放置点沿工具 Z 轴负方向移动 100mm，用户需要修改偏移量，保证接近过程无碰撞。

    • 离开放置点

      MoveL Offs(drop,0,0,100), v1000, fine, tool01
      从放置点沿物体坐标 Z 轴正方向移动 100mm，用户需要修改偏移量，保证离开过程无碰撞。

  • 定义夹具动作

    在移动到抓取点和放置点时，需添加夹具控制逻辑用来开关夹具。

使用 Mech-Viz 规划路径

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
// **********************************************************
// FUNCTION: Eye to Hand simple pick and place with Viz
// **********************************************************
    LOCAL PROC Vision_Sample_2()
            AccSet 100, 100
            MoveAbsJ home_point,v3000,fine,tool0    //move robot home position
            MoveL camera_capture,v500,fine,tool0
            Pos_Num = 0

            // Set ip address of IPC
            IP_Address = "192.168.2.222"
            Server_Port = 50000
            MM_Timeout = 60
            MM_Init_Socket()
            Wait 0.1

            // Set vision recipe
            Job = 1
            Model_Number = 1
            MM_Switch_Model()

            // Run Viz project
            SendPos_Type = 1
            MM_Start_Viz()
            Wait 0.1

            // set branch exitport
            Branch_Num = 1
            Export_Num = 1
            MM_Set_Branch()

            // get planned path
            Jps_Pos = 2
            MM_Get_VizData()
            IF(MM_Status != 2100)
                    EXIT
            ENDIF
            robtarget _P[20]
            int _Label[20]
            int _Speed[20]
            Serial = 0
            while(Serial < Pos_Num)
                    Serial += 1
                    MM_Get_Pose()
                    _P[Serial] = MM_P
                    _Label[Serial] = MM_Label
                    _Speed[Serial] = MM_Speed
            endwhile

            // follow the planned path to pick
            int count = 0
            while(count < Pos_Num)
                    count += 1
                    MoveL _P[count],v1000,fine,tool01
                    IF(count == VisPos_Num)
                            Pause
                            // add object grasping logic here
                    ENDIF
            endwhile

            // go to drop location
            MoveL RelTool(drop,0,0,-100,0,0,0),v1000,z50,tool01
            MoveL drop,v500,fine,tool01     // drop point
            Pause

            // add object releasing logic here
            MoveL Offs(drop,0,0,100), v1000, fine, tool01
            MoveAbsJ home_point,v3000,fine,tool0
            RETURN

    ENDPROC

程序逻辑解读

  • 第 6 行：移动机器人至 Home 点。

  • 第 7 行：移动到相机拍照位置。

  • 第 11 行：IP_Address 表示工控机的 IP 地址。

  • 第 12 行：Server_Port 表示工控机的端口号。

  • 第 13 行：MM_Timeout 表示通信超时等待时间。

  • 第 14 行：调用 MM_Init_Socket 对通信进行初始化设置。

  • 第 18 行：Job 表示 Mech-Vision 工程编号。

  • 第 19 行：Model_Number 表示 Mech-Vision 工程中配方模板的编号。

  • 第 20 行：如需设置 Mech-Vision 工程配方，可以在此处调用 MM_Switch_Model 设置工程配方。

  • 第 23 行：SendPos_Type 表示以“机器人当前关节角+当前法兰”形式向 Mech-Viz 传入机器人位姿。

  • 第 24 行：触发 Mech-Viz 工程运行。

  • 第 28 行：Branch_Num 表示分支步骤编号。

  • 第 29 行：Export_Num 表示分支步骤的出口号。

  • 第 30 行：如需设置 Mech-Viz 工程中的分支，可在此处调用 MM_Set_Branch() 设置分支步骤的出口。

  • 第 33 行：Jps_Pos 表示路径点将以机器人工具位姿（TCP）的形式返回。

  • 第 34 行：获取 Mech-Viz 工程规划的移动路径。

  • 第 35 行：判断状态码是否正常，若状态码返回异常，用户需要进行处理，例如暂停机器人。

  • 第 42~48 行：通过循环调用 MM_Get_Pose 获取位姿数据并转存到 _P[20]、_Label[20]和_Speed[20] 对应的单元中。由于ROAKE自定义函数不支持传入形参，所以指定全局变量 MM_P、MM_Label和MM_Speed 存储 MM_Get_Pose 计算结果。

  • 第 52~59 行：使用循环使机器人沿 Mech-Viz 规划的路径移动到抓取点。

  • 第 63 行：移动到放置点进行放置。

用户自定义部分

  • 定义工具中心点 TCP

    tool01: 用户可在示教器的标定界面中修改 tool01 的值来调整 TCP。

  • 示教 Home 点

    MoveAbsJ home_point, 用户需将 Home 点位姿记录在 home_point 位姿变量中。

  • 示教拍照点

    MoveL camera_capture, 用户需将拍照点位姿记录在 camera_capture 位姿变量中。

  • 示教放置点

    MoveL drop，用户需将放置点位姿记录在 drop 位姿变量中。

  • 定义夹具动作

    在移动到抓取点和放置点时，需添加夹具控制逻辑用来开关夹具。

  • 定义放置前后的接近距离

    • 接近放置点

    MoveL RelTool(drop,0,0,-100,0,0,0),v1000,fine,tool01
    放置点沿工具 Z 轴负方向移动 100mm，用户需要修改偏移量，保证接近过程无碰撞。

    • 离开放置点

      MoveL Offs(drop,0,0,100), v1000, fine, tool01
      从放置点沿物体坐标 Z 轴正方向移动 100mm，用户需要修改偏移量，保证离开过程无碰撞。

使用 Mech-Vision “路径规划”步骤获取规划路径

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
// ****************************************************************************
// FUNCTION: Eye to Hand simple pick and place with Vision Path Planning Step
// ****************************************************************************
    LOCAL PROC Vision_Sample_3()
            AccSet 100, 100
            MoveAbsJ home_point,v3000,fine,tool0    //move robot home position
            MoveL camera_capture,v500,fine,tool0
            Pos_Num = 0

            // Set ip address of IPC
            IP_Address = "192.168.2.222"
            Server_Port = 50000
            MM_Timeout = 60
            MM_Init_Socket()
            Wait 0.1

            // Set vision recipe
            Job = 1
            Model_Number = 1
            MM_Switch_Model()

            // Run vision project
            Job = 1
            Pos_Num_Need = 0
            SendPos_Type = 1
            MM_Start_Vis()
            Wait 1

            // get planned path from Mech-Vision Path Planning Step
            Job = 1
            Jps_Pos = 2
            MM_Get_VisPath()
            IF(MM_Status != 1103)
                    EXIT
            ENDIF
            robtarget _P[20]
            int _Label[20]
            int _Speed[20]
            Serial = 0
            while(Serial < Pos_Num)
                    Serial += 1
                    MM_Get_Pose()
                    _P[Serial] = MM_P
                    _Label[Serial] = MM_Label
                    _Speed[Serial] = MM_Speed
            endwhile

            // follow the planned path to pick
            int count = 0
            while(count < Pos_Num)
                    count += 1
                    MoveL _P[count],v1000,fine,tool01
                    IF(count == VisPos_Num)
                            Pause
                            // add object grasping logic here
                    ENDIF
            endwhile

            // go to drop location
            MoveL RelTool(drop,0,0,-100,0,0,0),v1000,z50,tool01
            MoveL drop,v500,fine,tool01     // drop point
            Pause

            // add object releasing logic here
            MoveL Offs(drop,0,0,100), v1000, fine, tool01
            MoveAbsJ home_point,v3000,fine,tool0
            RETURN

    ENDPROC

程序逻辑解读

  • 第 6 行：移动机器人至 Home 点。

  • 第 7 行：移动到相机拍照位置。

  • 第 11 行：IP_Address 表示工控机的 IP 地址。

  • 第 12 行：Server_Port 表示工控机的端口号。

  • 第 13 行：MM_Timeout 表示通信超时等待时间。

  • 第 14 行：调用 MM_Init_Socket 对通信进行初始化设置。

  • 第 18 行：Job 表示 Mech-Vision 工程编号。

  • 第 19 行：Model_Number 表示 Mech-Vision 工程中配方模板的编号。

  • 第 20 行：如需设置 Mech-Vision 工程配方，可以在此处调用 MM_Switch_Model 设置工程配方。

  • 第 24 行：Pos_Num_Need 表示期望从 Mech-Vision 得到的视觉点数量；其值为 0，表示从 Mech-Vision 工程取得识别结果中所有的视觉点。

  • 第 25 行：SendPos_Type 表示以“机器人当前关节角+当前法兰”形式向视觉系统传入机器人位姿。

  • 第 26 行：MM_Start_Vis 触发 Mech-Vision 工程运行。

  • 第 27 行：等待1秒。Eye To Hand 模式下，若在 MM_Start_Vis 和 MM_Get_VisData 之间有移动步骤（MoveL 或 MoveJ），则不需要在此设置等待；Eye In Hand模式下则必须等待一秒，在相机拍照完成之前保持相机静止。

  • 第 31 行：Jps_Pos 表示路径点将以机器人工具位姿（TCP）的形式返回。

  • 第 32 行：MM_Get_VisPath 获取 Mech-Vision “路径规划”步骤输出的移动路径。

  • 第 33 行：判断获取路径的状态码是否正常，若状态码返回异常，用户需要进行处理，例如停止机器人。

  • 第 40~46 行：循环使用 MM_Get_Pose 获取位姿数据并转存到 _P[20]、_Label[20]和_Speed[20] 对应的单元中。由于ROAKE自定义函数不支持传入形参，所以指定全局变量 MM_P、MM_Label和MM_Speed 存储 MM_Get_Pose 计算结果。

  • 第 50~57 行：使用循环使机器人沿规划路径移动，完成视觉抓取。

  • 第 61 行：移动到固定的放置点。

用户自定义部分

  • 定义工具中心点 TCP

    tool01: 用户可在示教器的标定界面中修改 tool01 的值来调整 TCP。

  • 示教 Home 点

    MoveAbsJ home_point, 用户需将 Home 点位姿记录在 home_point 位姿变量中。

  • 示教拍照点

    MoveL camera_capture, 用户需将拍照点位姿记录在 camera_capture 位姿变量中。

  • 示教放置点

    MoveL drop，用户需将放置点位姿记录在 drop 位姿变量中。

  • 定义夹具动作

    在移动到抓取点和放置点时，需添加夹具控制逻辑用来开关夹具。

  • 定义放置前后的接近距离

    • 接近放置点

      MoveL RelTool(drop,0,0,-100,0,0,0),v1000,fine,tool01
      放置点沿工具 Z 轴负方向移动 100mm，用户需要修改偏移量，保证接近过程无碰撞。

    • 离开放置点

      MoveL Offs(drop,0,0,100), v1000, fine, tool01
      从放置点沿物体坐标 Z 轴正方向移动 100mm，用户需要修改偏移量，保证离开过程无碰撞。

运行样例程序

运行样例程序之前，需满足以下前提：

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

  2. 已参照 ROKAE 标定操作流程 完成手眼标定。

  3. 已搭建 Mech-Vision 方案 及 工程 ，并 自动加载当前方案 。

  4. 若运行“使用 Mech-Viz 规划路径”样例，则搭建 Mech-Viz 工程，并在 Mech-Viz 工程资源面板中，右键单击工程名称，勾选 设为自动加载 。

  5. Home 点、相机拍照点、机器人工具中心点、中间路径点已示教位姿数据。

满足上述前提后，请参见 此处 运行程序。

执行到以下步骤时，根据具体样例打开注释。例如，运行第一个样例，选中 “// Vision_Sample_1()” 行后，依次点击 更多 ‣ 注释/反注释 。

../../../../_images/rokae_sample_1.png