案例/模板说明
============================

机械臂主控程序说明
++++++++++++++++++++++++++++++++
以下为机械臂主控模板代码，注意对工控机返回的 err_code 进行判断。


座标移动基础模板
+++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_CartMove_template()
        ; S1: Initialization
        ; S1: 初始化
        INT err_code ; 错误码
        INT grasp_pose_token ; 抓取位姿 token
        E6POS grasp_pose ; 抓取位姿
        E6POS home_pose ; home 位姿
        E6POS scan_pose ; 拍照位姿
        E6POS place_pose ; 放置位姿
        INT grasp_pose_num ; 抓取位姿数量
        BOOL is_eye_in_hand ; 是否为眼在手上
        INT obj_pipeline_num ; 物体运动流程编号
        INT obj_register_num ；物体抓取序号
        INT ints[6]
        INT vs_id ; 视觉编号
        CHAR item_codename[20] ; 物体代号
        CHAR task_codename[20] ；任务代号

        grasp_pose_token = 0
        gMasterFlag = TRUE
        err_code = 0
        grasp_pose_num = 0
        obj_pipeline_num = 0
        obj_register_num = 0

        ; 以下为中断处理等代码，不能删除
        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)
        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; S2: Connect to IPC
        ; S2: 连接 IPC
        xyzOpenSocket("MasterClient") ; 连接 IPC

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 必须先清空字符串    
        ; task_codename[] = "cart_move.t"  
        ; err_code = xyzSwitchTask(task_codename[]) ; 切换任务
        ; IF (err_code <> 0) THEN  ;
        ;     GOTO END_PROGRAM;
        ; ENDIF

        ; S3: Switch object
        ; S3: 切换物体
        vs_id = 0
        gStrOpSuccess = STRCLEAR(item_codename[]) ; 必须先清空字符串          
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 切换工件
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO END_PROGRAM;
        ENDIF

        ; S4: Move to home pose
        ; S4: 移动到 home 位姿
        ; you should use WorkVisual to set the home_pose,
        ; or use pendant to set the home_pose
        ; 使用 WorkVisual 设置 home 位姿，或者使用示教器设置 home 位姿
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数: 速度、加速度、减速度
        PTP home_pose ; 关节移动到 home 位姿
        ; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数: 速度、加速度、减速度
        ; LIN home_pose ; 直线移动到 home 位姿
        ; $OUT[500] = TRUE; IO 操作

        ; if application is eye_on_hand, please make eye_on_hand := true;
        ; 如果是眼在手上的应用，请将 eye_on_hand := true;
        is_eye_in_hand = false;

        BEGIN_WHILE:
        WHILE TRUE
            ; S5: eye on hand application
            ; S5: 眼在手上的应用
            IF is_eye_in_hand THEN
                ; S6: Move to scan pose
                ; S6: 移动到拍照位姿
                ; you should use WorkVisual to set the scan_pose,
                ; or use pendant to set the scan_pose
                ; 使用 WorkVisual 设置拍照位姿，或者使用示教器设置拍照位姿
                ; set movel or moveJ parameters(vel, acc, zone)
                scan_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
                xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数: 速度、加速度、减速度
                PTP scan_pose ; 关节移动到拍照位姿
                ; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数: 速度、加速度、减速度
                ; LIN scan_pose ; 直线移动到拍照位姿

                ; S7: Send scan pose
                ; S7: 发送拍照位姿
                err_code = xyzSendCurrentCartPose()
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF
            ENDIF

            ; S8: Request grasp pose(including cap image)
            ; S8: 请求抓取位姿
            err_code = xyzReqGraspPose(0,grasp_pose_token)
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; S9: Get grasp pose
            ; S9: 获取抓取位姿
            err_code = xyzGetGraspPose(grasp_pose_token, grasp_pose, grasp_pose_num, obj_pipeline_num, obj_register_num, ints[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; 如果没有可以抓取的物体
            IF (grasp_pose_num < 1) THEN
                WAIT SEC 5
                GOTO BEGIN_WHILE;
            ENDIF

            ; S10: Move to grasp pose and grasp object
            ; S10: 移动到抓取位姿并抓取物体
            ; Attention: Additional waypoints maybe need added
            ; 注意：可能需要添加额外的路径点
            xyzSetMoveJParameters(30,20,20)
            PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:grasp_pose C_DIS ;pre_pick 移动到预抓取点
            xyzSetMoveLParameters(10,20,0)
            LIN grasp_pose ; 直线移动到抓取位姿
            ;$OUT[500] = TRUE; grasp object
            WAIT SEC 0.5
            xyzSetMoveLParameters(10,20,20)   
            LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:grasp_pose C_DIS ;pose_pick 直线移动到后抓取点

            ; S11: Move to place pose and place object
            ; S11: 移动到放置位姿并放置物体
            ; you should use WorkVisual to set the place_pose,
            ; or use pendant to set the place_pose
            ; 使用 WorkVisual 设置放置位姿，或者使用示教器设置放置位姿
            place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
            xyzSetMoveJParameters(10,20,20)        
            PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;pre_place 移动到预放置点
            xyzSetMoveLParameters(10,20,0)
            LIN place_pose ; 直线移动到放置位姿
            ;$OUT[500] = FALSE; place object 放置物体
            WAIT SEC 0.5        
            ;Attention: Additional waypoints should be added        
            xyzSetMoveLParameters(10,20,20)      
            LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;post_place 直线移动到后放置点      

        ENDWHILE

        END_PROGRAM:
    END

座标移动二次定位模板
+++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_CartMove_reposition()
        ; S1: Initialization
        ; S1: 初始化
        INT err_code ; 错误码
        INT  rough_grasp_pose_token ; 粗定位抓取位姿 token
        E6POS  rough_grasp_pose ; 粗定位抓取位姿
        INT  rough_grasp_pose_num ; 粗定位抓取位姿数量
        INT  board_grasp_pose_token ; 隔板精定位抓取位姿 token
        E6POS  board_grasp_pose ; 隔板精定位抓取位姿
        INT  board_grasp_pose_num ; 隔板精定位抓取位姿数量
        INT  fine_grasp_pose_token ; 精定位抓取位姿 token
        E6POS  fine_grasp_pose ; 精定位抓取位姿
        INT  fine_grasp_pose_num ; 精定位抓取位姿数量
        E6POS home_pose ; home 位姿
        INT obj_pipeline_num ; 物体运动流程编号
        INT obj_register_num ; 物体抓取序号
        INT board_pipeline_num ; board运动流程编号
        INT board_register_num ; board 抓取序号
        INT ints[6]
        INT ints[6] 
        INT vs_id ; 视觉编号
        CHAR item_codename[20] ; 物体代号
        CHAR task_codename[20] ; 任务代号
        E6POS place_pose, scan_pose  ; 放置位姿、拍照位姿  

        gMasterFlag = TRUE
        err_code = 0
        obj_pipeline_num = 0
        obj_register_num = 0
        board_pipeline_num = 0
        board_register_num = 0
        fine_grasp_pose_token = 0

        ; 以下为中断处理等代码，不能删除
        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)
        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; $OUT[500] = TRUE; io initialization IO 初始化

        ; S2: Connect to IPC
        ; S2: 连接 IPC
        xyzOpenSocket("MasterClient") ; 连接 IPC

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 必须先清空字符串
        ; task_codename[] = "cart_repo.t"
        ; err_code = xyzSwitchTask(task_codename[]) ; 切换任务
        ; IF (err_code <> 0) THEN  ;
        ;    HALT;
        ;    GOTO END_PROGRAM;
        ; ENDIF

        ; S3: The camera outside the robotic arm is switched to recognize the current workpiece
        ; S3: 切换机械臂外部的相机，识别当前工件
        ; camera 1 rough location, 0: vs_id, "item1": item_codename
        ; change item_codename here!
        vs_id = 0
        gStrOpSuccess = STRCLEAR(task_codename[]) ; 必须先清空字符串        
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 切换工件
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO END_PROGRAM;
        ENDIF

        ; S4: Move to home pose
        ; S4: 移动到 home 位姿
        ; Additional: should define home_pose first,
        ; you can modify home_pose with WorkVisual,
        ; or use pendant to set the home_pose
        ; 需要先定义 home 位姿，
        ; 可以使用 WorkVisual 修改 home 位姿，或者使用示教器设置 home 位姿
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; vel, acc, zone 设置关节移动参数：速度、加速度、减速度
        PTP home_pose ; 关节移动到 home 位姿
        ; xyzSetMoveLParameters(10,20,0) ; vel, acc, zone 设置直线移动参数：速度、加速度、减速度
        ; LIN home_pose ; 直线移动到 home 位姿

        BEGIN_WHILE:
        WHILE TRUE
            ; S5: Request grasp pose
            ; S5: 请求抓取位姿
            err_code = xyzReqGraspPose(0,rough_grasp_pose_token) ; 请求抓取位姿
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; S6: get grasp pose
            ; S6: 获取抓取位姿
            err_code = xyzGetGraspPose(rough_grasp_pose_token, rough_grasp_pose,  rough_grasp_pose_num,  obj_pipeline_num, obj_register_num, ints[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; S7: grasp board if no object exists on board
            ; S7: 如果board上没有物体，就抓取board
            IF (rough_grasp_pose_num < 1) THEN
                ; S15: The camera outside the robotic arm is switched to recognize the board
                ; S15: 切换机械臂外部的相机，识别board
                ; no object in workspace, should remove board first
                ; 没有物体在工作区，应该先移除board
                ; switch board; O:vs_id, board: board_name
                vs_id = 0
                gStrOpSuccess = STRCLEAR(item_codename[]) ; 必须先清空字符串                
                item_codename[] = "board"
                err_code = xyzSwitchItem(vs_id, item_codename[]) ;切换工件为 board
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S16: Request board grasp pose
                ; S16: 请求 board 抓取位姿
                err_code = xyzReqGraspPose(0,board_grasp_pose_token)
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S17: Get board grasp pose
                ; S17: 获取 board 抓取位姿
                err_code = xyzGetGraspPose(board_grasp_pose_token, board_grasp_pose,  board_grasp_pose_num,  board_pipeline_num, board_register_num, ints[])
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; 如果没有可以抓取的 board
                IF (board_grasp_pose_num < 1) THEN
                    ; no board exists, you can change tote
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S18: Move to board grasp pose and grasp board
                ; S18: 移动到 board 抓取位姿并抓取 board
                xyzSetMoveJParameters(30,20,20)
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:board_grasp_pose C_DIS ;pre_pick  移动到预抓取点    
                xyzSetMoveLParameters(10,20,0)
                LIN board_grasp_pose
                ; $OUT[500] = TRUE; grasp board ; 抓取 board
                WAIT SEC 0.5             
                xyzSetMoveLParameters(10,20,20)
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:board_grasp_pose C_DIS ;post_pick  直线移动到后抓取点           

                ; S19: Move to board place pose
                ; S19: 移动到 board 放置位姿
                ; you should use WorkVisual to set the place_pose,
                ; or use pendant to set the place_pose
                ; 需要先定义 place 位姿，使用 WorkVisual 或者示教器设置
                place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88} ; 放置位姿
                xyzSetMoveJParameters(30,20,20)        
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;pre_place    移动到预放置点      
                xyzSetMoveLParameters(10,20,0)
                ; $OUT[500] = FALSE; place board ; 放置 board                
                LIN place_pose ; 直线移动到放置位姿
                xyzSetMoveLParameters(10,20,20)     
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;post_place  直线移动到后放置点      


                ; S20: The camera outside the robotic arm is switched to recognize the current workpiece
                ; S20: 切换机械臂外部的相机，识别当前工件
                ; 0: vs_id, "item1": item_codename
                ; change the item_codename here!
                vs_id = 0
                gStrOpSuccess = STRCLEAR(item_codename[]) ; 必须先清空字符串                   
                item_codename[] = "item1"
                err_code = xyzSwitchItem(vs_id, item_codename[]); 切换工件为 item1
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                GOTO BEGIN_WHILE;
            ELSE

                ; S8: Move to scan pose
                ; S8: 移动到拍照位姿
                ; Additional: should define scan_pose first,
                ; you can modify scan_pose with WorkVisual or
                ; use pendant to set scan_pose
                ; 需要先定义 scan 位姿，使用 WorkVisual 或者示教器设置
                scan_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
                xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数：速度、加速度、减速度
                PTP scan_pose ; 关节移动到拍照位姿
                ; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数：速度、加速度、减速度
                ; LIN scan_pose ; 直线移动到拍照位姿

                ; S9: Send scan pose
                ; S9: 发送拍照位姿
                err_code = xyzSendCurrentCartPose()
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S10: The camera on the robotic arm is switched to recognize the current workpiece
                ; S10: 切换机械臂上的相机，识别当前工件
                ; camera2 cap precise image, 1: vs_id, "item1": item_codename
                ; change the item_codename here!
                vs_id = 1
                gStrOpSuccess = STRCLEAR(item_codename[]) ; 必须先清空字符串                   
                item_codename[] = "item1"
                err_code = xyzSwitchItem(vs_id, item_codename[]) ; 切换工件为 item1
                IF (err_code <> 0) THEN  ;
                    GOTO END_PROGRAM;
                ENDIF

                ; S11: Request grasp pose
                ; S11: 请求抓取位姿
                err_code = xyzReqGraspPose(1,fine_grasp_pose_token)
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S12: Get grasp pose
                ; S12: 获取抓取位姿
                err_code = xyzGetGraspPose(fine_grasp_pose_token ,fine_grasp_pose, fine_grasp_pose_num, obj_pipeline_num, obj_register_num, ints[])
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; 如果没有可以抓取的工件
                IF (fine_grasp_pose_num < 1) THEN
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S13: Move to grasp pose and grasp workpiece
                ; S13: 移动到抓取位姿并抓取工件
                xyzSetMoveJParameters(10,20,20)
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:fine_grasp_pose C_DIS ;pre_pick 移动到预抓取点
                xyzSetMoveLParameters(10,20,0)
                LIN fine_grasp_pose ; 直线移动到抓取位姿
                ;$OUT[500] = TRUE;
                WAIT SEC 0.5
                xyzSetMoveLParameters(10,20,20)
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:fine_grasp_pose C_DIS ;post_pick 直线移动到后抓取点   

                ; S14: Move to place pose and place workpiece
                ; S14: 移动到放置位姿并放置工件
                ; you should use WorkVisual to define place_pose,
                ; or use pendant to set place_pose
                ; 需要先定义 place 位姿，使用 WorkVisual 或者示教器设置
                place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88} ; 放置位姿
                xyzSetMoveJParameters(10,20,20)
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;pre_place  移动到预放置点              
                xyzSetMoveLParameters(10,20,0)
                LIN place_pose ; 关节移动到放置位姿
                ; $OUT[500] = FALSE;
                xyzSetMoveLParameters(10,20,20)
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;post_place 直线移动到后放置点  
            ENDIF
        ENDWHILE

        END_PROGRAM:
    END


3D轨迹移动同步程序
+++++++++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_TrajMove_Sync()

        ; S1: Initialization
        ; S1: 初始化
        INT err_code
        CHAR task_codename[20]
        INT vs_id
        CHAR item_codename[20]
        E6POS home_pose
        INT pipeline_num
        INT register_num
        INT way_point_num
        INT wp_type[30]
        E6AXIS joints[50]
        E6POS carts[50]

        gMasterFlag = TRUE

        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)

        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; S2: Connect to IPC
        ; S2: 连接工控机
        xyzOpenSocket("MasterClient")
        gStrOpSuccess = STRCLEAR(gSendgMotMsg[])

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 必须先清空字符串
        ; task_codename[] = "traj_sync.t"
        ; err_code = xyzSwitchTask(task_codename[]) ; 切换任务
        ; IF (err_code <> 0) THEN  ;
        ;    HALT;
        ;    GOTO err_exit;
        ; ENDIF

        ; S3: Switch to the current item
        ; S3: 切换到当前工件
        vs_id = 0
        gStrOpSuccess = STRCLEAR(item_codename[]) ; 必须先清空字符串    
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 切换工件
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        ; IO operation
        ; $OUT[500] = False
        WAIT SEC 0.5

        ; S4: Move to home pose
        ; S4: 移动到 home 位姿
        ; you should use WorkVisual to set the home pose,
        ; or use pendant to set the home pose
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; vel, acc, zone
        PTP home_pose
        ; xyzSetMoveLParameters(10,20,0)
        ; LIN home_pose

        WHILE True
        ; S5: Request Pick and place trajectory
        ; S5: 请求抓取放置轨迹
        vs_id = 0
        err_code = xyzReqPickPlace(vs_id)
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        ; S6: Get Pick in trajectory
        ; S6: 获取抓取入筐轨迹
        err_code = xyzGetPickin(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN
            ; S15
            HALT;
            GOTO err_exit;
        ENDIF

        ; if tote cleared, exit
        ; 如果没有可以抓取的物体，退出
        IF (way_point_num < 1) THEN
            GOTO clear_tore_exit
        ENDIF
        ; S8 Execute Pick in trajectory
        ; S8: 执行抓取入筐轨迹
        xyzExecutePickInTraj(way_point_num, wp_type[], joints[], carts[])
        ; IO operation IO 操作
        ; $OUT[500] = False

        ; S9: Get Pick out trajectory
        ; S9: 获取抓取出筐轨迹
        err_code = xyzGetPickout(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF
        ; S10: Execute Pick out trajectory
        ; S10: 执行抓取出筐轨迹
        xyzExecutePickOutTraj(way_point_num, wp_type[], joints[], carts[])

        ; S11: Get Place in trajectory
        ; S11: 获取放置入筐轨迹
        err_code = xyzGetPlacein(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF
        ; S12: Execute Place in trajectory
        ; S12: 执行放置入筐轨迹
        xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

        ; IO operation IO 操作
        ; $OUT[500] = True

        ; S13: Get Place out trajectory
        ; S13: 获取放置出筐轨迹
        err_code = xyzGetPlaceout(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF
        ; S14: Execute Place out trajectory
        ; S14: 执行放置出筐轨迹
        xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

        ENDWHILE

        err_exit:

        clear_tore_exit:

        END


3D轨迹移动异步运行程序
+++++++++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_TrajMove_Async()

        ; S1: Initialization
        ; S1: 初始化
        INT err_code
        CHAR task_codename[20]
        INT vs_id
        CHAR item_codename[20]
        E6POS home_pose
        INT pipeline_num
        INT register_num
        INT way_point_num
        INT wp_type[30]
        E6AXIS joints[50]
        E6POS carts[50]

        gMasterFlag = TRUE ; 表示正在运行机械臂主控

        ; 以下为中断处理等代码，不能删除
        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)

        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; S2: Connect to IPC
        ; S2: 连接工控机
        xyzOpenSocket("MasterClient")
        gStrOpSuccess = STRCLEAR(gSendgMotMsg[])

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 必须先清空字符串
        ; task_codename[] = "traj_async.t"
        ; err_code = xyzSwitchTask(task_codename[]) ; 切换任务
        ; IF (err_code <> 0) THEN  ;
        ;    HALT;
        ;    GOTO err_exit;
        ; ENDIF

        ; S3: Switch to the current item
        ; S3: 切换到当前工件
        vs_id = 0
        gStrOpSuccess = STRCLEAR(item_codename[]) ; 必须先清空字符串
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 切换工件
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        ; IO operation IO 操作
        ; $OUT[500] = False

        ; S4: Move to home pose
        ; S4: 移动到 home 位姿
        ; you should use WorkVisual to set the home_pose,
        ; or use pendant to set the home_pose
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; vel, acc, zone
        PTP home_pose
        ; xyzSetMoveLParameters(10,20,0)
        ; LIN home_pose

        ; S5: Request Pick and place trajectory
        ; S5: 请求抓取放置轨迹
        vs_id = 0
        err_code = xyzReqPickPlace(vs_id)
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        WHILE True

            ; S6: Get Pick in trajectory
            ; S6: 获取抓取入筐轨迹
            vs_id = 0
            err_code = xyzGetPickin(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF

            ; if tote cleared, exit
            ; 如果没有可以抓取的物体，退出
            IF (way_point_num < 1) THEN
                GOTO clear_tore_exit
            ENDIF
            ; S8 Execute Pick in trajectory
            ; S8: 执行抓取入筐轨迹
            xyzExecutePickInTraj(way_point_num, wp_type[], joints[], carts[])
            ; IO operation IO 操作
            ; $OUT[500] = False

            ; S9: Get Pick out trajectory
            ; S9: 获取抓取出筐轨迹
            vs_id = 0
            err_code = xyzGetPickout(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF
            ; S10: Execute Pick out trajectory
            ; S10: 执行抓取出筐轨迹
            xyzExecutePickOutTraj(way_point_num, wp_type[], joints[], carts[])

            ; S11: request next pick and place plan in advance
            ; S11: 提前请求下一次抓取放置轨迹
            vs_id = 0
            err_code = xyzReqPickPlace(vs_id)
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF

            ; S12: Get Place in trajectory
            ; S12: 获取放置入筐轨迹
            vs_id = 0
            err_code = xyzGetPlacein(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF
            ; S13: Execute Place in trajectory
            ; S13: 执行放置入筐轨迹
            xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

            ; IO operation IO 操作
            ; $OUT[500] = True

            ; S14: Get Place out trajectory
            ; S14: 获取放置出筐轨迹
            vs_id = 0
            err_code = xyzGetPlaceout(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF
            ; S15: Execute Place out trajectory
            ; S15: 执行放置出筐轨迹
            xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

        ENDWHILE

        err_exit:

        clear_tore_exit:

    END