案例/模板说明

机械臂主控程序说明

以下为机械臂主控的四种应用形式的模板代码，注意对工控机返回的 err_code 进行判断。

座标移动基础模板

-------------------------------CartMove Basic Template-------------------------------------

V_HOME_POSE = {user=0,tool=1,coordinate={240, 560, 90, -153, -14, 16}, joint={80, -50, -100, 90, 90, -30}}
V_PLACE_POSE = {user=0,tool=1,coordinate={-230, 540, -120, 175, 30, 0}}
V_SCAN_POSE = {user=0,tool=1,coordinate={-100, 650, 240, -180, 0, 0}}

function xyzMain()
-- S1: 初始化
local err_code = 0
local return_token = 0
local ws_id = 0
local flow_name = "cart_basic.t"
local item_codename = "item1"

--IO operation
DO(1, OFF)

-- 切换任务流图 ，默认被注释掉，如果需求可以取消注释
-- do
--   err_code = xyzSwitchFlow(flow_name)
--   xyzCheckErrorCode(err_code)
-- end

-- S3: 切换成当前工件
do
    err_code = xyzSwitchItem(ws_id, item_codename)
    xyzCheckErrorCode(err_code)
end

-- S4: 运动到 home 位
-- 首先移动到 home 点。注意需要先定义 V_HOME_POSE 的关节角度值
-- 或者用使用其他点来替代 V_HOME_POSE
do
    --Speed(V_J_SPEED)
    --Accel(V_J_ACC)
    --MoveJ(V_HOME_POSE)
    --Sync()

    SpeedS(V_TCP_SPEED)
    AccelS(V_TCP_ACC)
    Move(V_HOME_POSE)
    Sync()
end

-- 如果是眼在机械臂上的应用形式，请把 eye_on_hand 赋值为 true
eye_on_hand = false

while(true)
do
    -- S5: 眼在机械臂上
    if eye_on_hand then
    -- S6: 运动到拍照位姿
    -- 注意: 需要先定义拍照位姿，
    -- 可以直接修改 V_SCAN_POSE 或者使用其他点位来替换
    --走到眼在手上的拍照点，并发送当前拍照点位姿给上位机
    SpeedS(V_TCP_SPEED)
    AccelS(V_TCP_ACC)
    Move(V_SCAN_POSE)
    Sync()

    -- S7: 发送拍照位姿
    err_code = xyzSendCurrentCartPose()
    xyzCheckErrorCode(err_code)
    end

    -- S8: 请求抓取点位
    --请求计算抓取点（自带拍照功能）
    err_code, return_token = xyzReqGraspPose(ws_id)
    xyzCheckErrorCode(err_code)

    local grasp_pose = {}
    local grasp_pose_num = 0
    local pipeline_num = 0
    local register_num = 0

    -- S9: 获取抓取点位
    err_code, grasp_pose, grasp_pose_num, pipeline_num, register_num = xyzGetGraspPose(return_token)
    xyzCheckErrorCode(err_code)

    if grasp_pose_num < 1 then
        --当前无可抓取工件，进行下一次拍照请求
        print("No grasp_pose, continue requesting")
        Sleep(5000) --5S
        goto Continue
    end

    -- S10: 运动到抓取点并抓取工件
    -- 注意: 可能需要添加其他路径点作为过渡
    do
    Pick = {user=0,tool=1,coordinate=grasp_pose}
    SpeedS(V_TCP_SPEED)
    AccelS(V_TCP_ACC)
    -- 移动到预抓取点，偏移值可以根据实际需求修改
    Move(RP(Pick, {0, 0, 100}))
    -- 运动到抓取点
    Move(Pick)
    Sync()
    DO(1, ON)
    -- 移回预抓取点，偏移值可以根据实际需求修改
    Move(RP(Pick, {0, 0, 100}))
    end

    -- S11: 运动到放置点并放置工件
    -- 注意: 可能需要添加其他路径点作为过渡
    -- 注意: 首先需要定义放置点位姿,
    -- 可以直接修改 V_PLACE_POSE 或者使用其他点来替换
    do
    SpeedS(V_TCP_SPEED)
    AccelS(V_TCP_ACC)
    Move(V_PLACE_POSE)
    Sync()
    DO(1, OFF)
    end
end
end


-- S2: 连接到工控机
xyzMasterConnect()
xyzMain()
xyzMasterClose()

座标移动二次定位模板：

-------------------------------CartMove Reposition Template-------------------------------------

V_HOME_POSE = {user=0,tool=1,coordinate={240, 560, 90, -153, -14, 16}, joint={80, -50, -100, 90, 90, -30}}
V_PLACE_POSE = {user=0,tool=1,coordinate={-230, 540, -120, 175, 30, 0}}
V_BOARD_PLACE_POSE = {user=0,tool=1,coordinate={-100, 650, 240, -180, 0, 0}}

function xyzMain()
-- S1: 初始化
local err_code = 0
local return_token = 0

local flow_name = "cart_repo.t"
local item_codename = "item1"

local ws_id = 0   --相机1
local ws_id_2 = 1 --相机2

--IO operation
DO(1,OFF)

-- 切换任务流图 ，默认被注释掉，如果需求可以取消注释
-- do
--   err_code = xyzSwitchFlow(flow_name)
--   xyzCheckErrorCode(err_code)
-- end

-- S3: 机械臂外的相机切换成识别当前工件
do
    err_code = xyzSwitchItem(ws_id, item_codename)
    xyzCheckErrorCode(err_code)
end

--IO operation
DO(1,OFF)

-- S4: 运动到 home 位
-- 首先移动到 home 点。注意需要先定义 V_HOME_POSE 的关节角度值
-- 或者用使用其他点来替代 V_HOME_POSE
do
    --Speed(V_J_SPEED)
    --Accel(V_J_ACC)
    --MoveJ(V_HOME_POSE)
    --Sync()

    SpeedS(V_TCP_SPEED)
    AccelS(V_TCP_ACC)
    Move(V_HOME_POSE)
    Sync()
end

while(true)
do
    local rough_grasp_pose = {}
    local rough_grasp_pose_num = 0
    local rough_pipeline_num = 0
    local rough_register_num = 0

    -- S5: 请求工件的抓取位姿
    err_code, return_token = xyzReqGraspPose(ws_id)
    xyzCheckErrorCode(err_code)

    -- S6: 获取工件的抓取位姿
    --相机1获取工件粗定位抓取点
    err_code, rough_grasp_pose, rough_grasp_pose_num, rough_pipeline_num, rough_register_num = xyzGetGraspPose(return_token)
    xyzCheckErrorCode(err_code)

    -- S7: 处理是否识别到工件
    --无工件，抓隔板
    if rough_grasp_pose_num < 1 then
        local board_grasp_pose = {}
        local board_grasp_pose_num = 0
        local board_pipeline_num = 0
        local board_register_num = 0

        -- S15: 没有识别到工件，机械臂外的相机切换识别隔板
        --相机1拍隔板
        err_code = xyzSwitchItem(ws_id, "board")
        xyzCheckErrorCode(err_code)

        -- S16: 请求隔板抓取位姿
        err_code, return_token = xyzReqGraspPose(ws_id)
        xyzCheckErrorCode(err_code)

        -- S17: 获取隔板的抓取位姿
        --相机1获取隔板抓取点
        err_code, board_grasp_pose, board_grasp_pose_num, board_pipeline_num, board_register_num = xyzGetGraspPose(return_token)
        xyzCheckErrorCode(err_code)
        if board_grasp_pose_num < 1 then
        -- no board
        --无隔板，此时也可以换料框
        goto err_exit
        end

        -- S18: 运动到隔板抓取位姿，并抓取隔板
        -- 注意: 可能需要添加其他路径点作为过渡
        --抓隔板
        local P = {user=0,tool=1,coordinate=board_grasp_pose}
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        -- 移动到隔板预抓取点，偏移值可以根据实际需求修改
        Move(RP(P, {0, 0, 100}))
        -- move to board grasp pose
        Move(P)
        Sync()
        DO(1,ON)
        -- 移回隔板预抓取点，偏移值可以根据实际需求修改
        Move(RP(P, {0, 0, 100}))

        -- S19: 运动到隔板的放置位姿
        -- 注意: 可能需要添加其他路径点作为过渡
        --放置隔板
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(V_BOARD_PLACE_POSE)
        Sync()
        DO(1,OFF)

        -- S20: 机械臂外的相机切换成识别当前工件
        err_code = xyzSwitchItem(ws_id, item_codename)
        xyzCheckErrorCode(err_code)

    else --有工件，二次定位
        -- S8: 运动到拍照位姿
        -- 注意: 需要实现定义拍照位，或者根据机械臂外相机提供的抓取点位来确定,
        -- 可以直接修改 rough_grasp_pose 或者使用其他点位替换
        --走到工件粗定位点
        local P = {user=0,tool=1,coordinate=rough_grasp_pose}
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(P)
        Sync()

        -- S9: 发送拍照位姿
        --发送当前眼在手上的拍照点位姿给上位机
        err_code = xyzSendCurrentCartPose()
        xyzCheckErrorCode(err_code)

        -- S10: 机械臂上的相机切换成识别当前工件
        --相机2对工件进行精定位拍照
        err_code = xyzSwitchItem(ws_id_2, item_codename)
        xyzCheckErrorCode(err_code)

        -- S11: 请求机械臂上的相机获取工件抓取位姿
        err_code, return_token = xyzReqGraspPose(ws_id_2)
        xyzCheckErrorCode(err_code)

        local fine_grasp_pose = {}
        local fine_grasp_pose_num = 0
        local fine_pipeline_num = 0
        local fine_register_num = 0

        -- S12: 获取工件的抓取位姿
        --获取工件精定位抓取点
        err_code, fine_grasp_pose, fine_grasp_pose_num, fine_pipeline_num, fine_register_num = xyzGetGraspPose(return_token)
        xyzCheckErrorCode(err_code)

        if fine_grasp_pose_num < 1 then
        -- no object
        goto err_exit
        end

        -- S13: 运动到工件的抓取位姿并进行抓取
        -- 注意: 可能需要添加其他路径点作为过渡
        --抓工件
        local P = {user=0,tool=1,coordinate=fine_grasp_pose}
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        -- 移动到工件预抓取点，偏移值可以根据实际需求修改
        Move(RP(P, {0, 0, 100}))
        -- move to grasp pose
        Move(P)
        Sync()
        DO(2, ON)
        -- 移回工件预抓取点，偏移值可以根据实际需求修改
        Move(RP(P, {0, 0, 100}))

        -- S14: 运动到工件的放置位姿
        -- 注意: 可能需要添加其他路径点作为过渡
        --放置工件
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(V_PLACE_POSE)
        Sync()

        DO(2, OFF)
    end
end

::err_exit:: do
    print("Error_Exit. Please Check")
    return
end
end

-- S2: 连接到工控机
xyzMasterConnect()
xyzMain()
xyzMasterClose()

轨迹移动同步模板：

-------------------------------TrajMove Sync Template-------------------------------------

-- 注意: 使用 Sync() 函数可以使程序等待机械臂完成运动指令
-- 注意: 使用 Sync() 函数可以使程序等待机械臂完成运动指令
-- 注意: 使用 Sync() 函数可以使程序等待机械臂完成运动指令

function xyzMain()
    -- S1: 初始化
    local err_code = 0
    local ws_id = 0
    local flow_name = "traj_sync.t"
    local item_codename = "item1"

    --IO operation
    DO(1,OFF)

    -- 切换任务流图 ，默认被注释掉，如果需求可以取消注释
    -- do
    --     err_code = xyzSwitchFlow(flow_name)
    --     xyzCheckErrorCode(err_code)
    -- end

    -- S3: 切换成当前工件
    do
        err_code = xyzSwitchItem(ws_id, item_codename)
        xyzCheckErrorCode(err_code)
    end

    -- S4: 运动到 home 位
    -- 首先移动到 home 点。注意需要先定义 home 的关节角度值
    -- 或者用使用其他点来替代 home
    do
    --local home = {joint={80, -50, -100, 90, 90, -30}}
    --Speed(V_J_SPEED)
    --Accel(V_J_ACC)
    --MoveJ(home)
    --Sync()

    local home = {coordinate={240, 560, 90, -153, -14, 16}}
    SpeedS(V_TCP_SPEED)
    AccelS(V_TCP_ACC)
    Move(home)
    Sync()
    end

    while(true)
    do
        local pipeline_num = 0
        local register_num = 0
        local wp_num = 0
        local wp_type = {}
        local wp = {}

        -- S5: 请求抓取和放置规划
        err_code = xyzReqPickPlace(ws_id)
        xyzCheckErrorCode(err_code)

        -- S6: 获取 pick-in 轨迹
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickin(ws_id)
        xyzCheckErrorCode(err_code)

        -- S7: 判断是否识别到工件
        --if tote cleared, exit
        if wp_num == 0 then
            print("tote clear")
            goto clear_tore_exit
        end

        -- S8: 执行 pick-in 轨迹
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute pick in error")
            goto err_exit
        end
        Sync()
        --IO operation
        DO(1, ON)

        -- S9: 获取 pick-out 轨迹
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickout(ws_id)
        xyzCheckErrorCode(err_code)

        -- S10: 执行 pick-out 轨迹
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute pick out error")
            goto err_exit
        end

        -- S11: 获取 place-in 轨迹
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlacein(ws_id)
        xyzCheckErrorCode(err_code)

        -- S12: 执行 place-in 轨迹
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute place in error")
            goto err_exit
        end
        Sync()
        --IO operation
        DO(1, OFF)

        -- S13: 获取 place-out 轨迹
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlaceout(ws_id)
        xyzCheckErrorCode(err_code)

        -- S14: 执行 place-out 轨迹
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute place out error")
            goto err_exit
        end
    end

    -- 处理其他异常情况
    ::err_exit:: do
        print("Error_Exit. Please Check the error code")
        return
    end

    ::clear_tore_exit:: do
        print("work done: tote has been cleared")
        return
    end

end

-- S2: 连接到工控机
xyzMasterConnect()
xyzMain()
xyzMasterClose()

轨迹移动异步模板：

-------------------------------TrajMove ASync Template-------------------------------------

-- 注意: 使用 Sync() 函数可以使程序等待机械臂完成运动指令
-- 注意: 使用 Sync() 函数可以使程序等待机械臂完成运动指令
-- 注意: 使用 Sync() 函数可以使程序等待机械臂完成运动指令

function xyzMain()
    -- S1: 初始化
    local err_code = 0
    local ws_id = 0
    local flow_name = "traj_async.t"
    local item_codename = "item1"

    --IO operation
    DO(1,OFF)

    -- 切换任务流图 ，默认被注释掉，如果需求可以取消注释
    -- do
    --   err_code = xyzSwitchFlow(flow_name)
    --   xyzCheckErrorCode(err_code)
    -- end

    -- S3: 切换成当前工件
    do
    err_code = xyzSwitchItem(ws_id, item_codename)
    xyzCheckErrorCode(err_code)
    end



    -- S4: 运动到 home 位
    -- 首先移动到 home 点。注意需要先定义 home 的关节角度值
    -- 或者用使用其他点来替代 home
    do
    --local home = {joint={80, -50, -100, 90, 90, -30}}
    --Speed(V_J_SPEED)
    --Accel(V_J_ACC)
    --MoveJ(home)
    --Sync()

    local home = {coordinate={240, 560, 90, -153, -14, 16}}
    SpeedS(V_TCP_SPEED)
    AccelS(V_TCP_ACC)
    Move(home)
    Sync()
    end

    -- S5: 请求抓取和放置规划
    err_code = xyzReqPickPlace(ws_id)
    xyzCheckErrorCode(err_code)

while(true)
    do
    local pipeline_num = 0
    local register_num = 0
    local wp_num = 0
    local wp_type = {}
    local wp = {}

    -- S6: 获取 pick-in 轨迹
    err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickin(ws_id)
    xyzCheckErrorCode(err_code)

    -- S7: 判断是否识别到工件
    --if tote cleared, exit
    if wp_num == 0 then
        print("tote clear")
        goto clear_tore_exit
    end

    -- S8: 执行 pick-in 轨迹
    if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
        print("Execute pick in error")
        goto err_exit
    end
    Sync()
    --IO operation
    DO(1, ON)

    -- S9: 获取 pick-out 轨迹
    err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickout(ws_id)
    xyzCheckErrorCode(err_code)

    -- S10: 执行 pick-out 轨迹
    if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
        print("Execute pick out error")
        goto err_exit
    end
    Sync()

    -- S11: 请求下一次的抓取和放置规划
    -- request next pick and place plan in advance
    err_code = xyzReqPickPlace(ws_id)
    xyzCheckErrorCode(err_code)

    -- S12: 获取本次 place-in 轨迹
    err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlacein(ws_id)
    xyzCheckErrorCode(err_code)

    -- S13: 执行本次 place-in 轨迹
    if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
        print("Execute place in error")
        goto err_exit
    end
    Sync()
    --IO operation
    DO(1, OFF)

    -- S14: 获取本次 place-out 轨迹
    err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlaceout(ws_id)
    xyzCheckErrorCode(err_code)

    -- S15: 执行本次 place-out 轨迹
    if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
        print("Execute place out error")
        goto err_exit
    end
    end

    -- 处理其他异常情况
    ::err_exit:: do
    print("Error_Exit. Please Check the error code")
    return
    end

    ::clear_tore_exit:: do
    print("work done: tote has been cleared")
    return
    end
end

-- S2: 连接到工控机
xyzMasterConnect()
xyzMain()
xyzMasterClose()