案例/模板说明 ============================ 机械臂主控主函数说明 ++++++++++++++++++++++++++++++++ 注意对工控机返回的 err_code 进行判断。 座标移动基础模板： ------------------------------------- .. code-block:: Include "api.pro"; Func run() # S1, S2 : Init and connect To IPC 初始化以及连接工控机 LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); EndWhile; gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# # String flow_name ="cart_basic.t"; # api.xyzSwitchFlow(flow_name, err_code); # If err_code <> 0 # Halt 3; # Goto L[1]; # EndIf; # S3 切换工件 Int ws_id = 0; String item_codename = "item1"; api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # io operation Set Out[1],ON; # S4: move To Home 回原点 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; Bool is_eye_in_hand = True; Int grasp_pose_token = 0; Double grasp_pose[6] = {0,0,0,0,0,0}; Int grasp_pose_num = 0; Int pipeline_num = 0; Int register_num = 0; While True # S5 If is_eye_in_hand # S6: Move To scan pose 移动到拍照位姿 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; # S7 发送当前位姿 api.xyzSendCurrentCartPose(err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; EndIf; # S8: req grasp pose(including cap image) 请求抓取位姿 grasp_pose_token = 0; ws_id = 0; api.xyzReqGraspPose(ws_id, err_code, grasp_pose_token); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S9 获取抓取位姿 grasp_pose_num = 0; pipeline_num = 0; register_num = 0; Int ints[6]; api.xyzGetGraspPose(grasp_pose_token, err_code, grasp_pose, grasp_pose_num, pipeline_num, register_num, ints); If err_code <> 0 Halt 3; Goto L[1]; EndIf; LP[2] = (grasp_pose[0],grasp_pose[1],grasp_pose[2],grasp_pose[3],grasp_pose[4],grasp_pose[5]),(0,0,0,1),(3,0,0); If grasp_pose_num < 1 Wait T[5]; Continue; EndIf; # S10: Move To grasp pose 移动到抓取位姿 # add more path pose, modify the offset PR[], unit: mm # 添加额外路径点 PR[1] = (0,0,100,0,0,0); Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0]; Movl LP[2], V[30], Z[0], Tool[0]; Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0]; # io operation Set Out[1],OFF; # S11: Move To place pose 移动到放置位姿 # add more path pose, modify the offset PR[], unit: mm # 添加额外路径点 PR[2] = (0,0,100,0,0,0); LP[3] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(3,0,0); Movj Offset(LP[2],PR[2]),V[30],Z[0],Tool[0]; Movl LP[3], V[30], Z[0], Tool[0]; Movj Offset(LP[2],PR[2]),V[30],Z[0],Tool[0]; # io operation Set Out[1],ON; EndWhile; L[1]: Close Socket, gMasterPort; EndFunc; 座标移动二次定位模板 -------------------------------------- .. code-block:: Include "api.pro"; Func run() # S1, S2 : Init and connect To IPC 初始化以及连接工控机 LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); EndWhile; gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# # String flow_name ="cart_repo.t"; # api.xyzSwitchFlow(flow_name, err_code); # If err_code <> 0 # Halt 3; # Goto L[1]; # EndIf; # S3 切换工件 Int ws_id = 0; String item_codename = "item1"; api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S4: Move To Home 回原点 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; # io operation Set Out[1],ON; Int rough_grasp_pose_token = 0; Double rough_grasp_pose[6] = {0,0,0,0,0,0}; Int rough_grasp_pose_num = 0; Int rough_pipeline_num = 0; Int rough_register_num = 0; String board_name; Int board_grasp_pose_token = 0; Double board_grasp_pose[6] = {0,0,0,0,0,0}; Int board_grasp_pose_num = 0; Int board_pipeline_num = 0; Int board_register_num = 0; Int fine_grasp_pose_token = 0; Double fine_grasp_pose[6] = {0,0,0,0,0,0}; Int fine_grasp_pose_num = 0; Int fine_pipeline_num = 0; Int fine_register_num = 0; Int ints[6]; While True # S5 请求抓取位姿 rough_grasp_pose_token = 0; ws_id = 0; api.xyzReqGraspPose(ws_id, err_code, rough_grasp_pose_token); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S6: camera1 get rough grasp pose 获取粗抓取位姿 api.xyzGetGraspPose(rough_grasp_pose_token, err_code, rough_grasp_pose, rough_grasp_pose_num, rough_pipeline_num, rough_register_num, ints); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S7: grasp board if no object exists on board 如果没有物体在隔板上，抓取隔板 If (rough_grasp_pose_num < 1) # S15: switch board; 切换抓取物体为隔板 ws_id = 0; board_name = "board"; api.xyzSwitchItem(ws_id, board_name, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S16 请求隔板抓取位姿 ws_id = 0; api.xyzReqGraspPose(ws_id, err_code, board_grasp_pose_token); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S17 获取隔板抓取位姿 api.xyzGetGraspPose(board_grasp_pose_token, err_code, board_grasp_pose, board_grasp_pose_num, board_pipeline_num, board_register_num, ints); If err_code <> 0 Halt 3; Goto L[1]; EndIf; LP[2] = (board_grasp_pose[0],board_grasp_pose[1],board_grasp_pose[2],board_grasp_pose[3],board_grasp_pose[4],board_grasp_pose[5]),(0,0,0,1),(3,0,0); If (board_grasp_pose_num < 1) Halt 3; Goto L[1]; EndIf; # S18: Move To board grasp pose 移动到隔板抓取位姿 PR[1] = (0,0,100,0,0,0); # add more path pose, modify the offset PR[], unit: mm # 添加额外路径点 Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0]; Movl LP[2], V[30], Z[0], Tool[0]; Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0]; # io operation Set Out[1],OFF; # S19: Move To board place pose 移动到隔板放置位姿 # add more path pose, modify the offset PR[], unit: mm # 添加额外路径点 PR[2] = (0,0,100,0,0,0); LP[3] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(3,0,0); Movj Offset(LP[3],PR[2]),V[30],Z[0],Tool[0]; Movl LP[3], V[30], Z[0], Tool[0]; Movj Offset(LP[3],PR[2]),V[30],Z[0],Tool[0]; # io operation Set Out[1],ON; # S20 切换工件 item_codename = "item1"; ws_id = 0; api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; Continue; EndIf; # S9: move To rough_grasp_pose 移动到粗抓取位姿 LP[4] = (rough_grasp_pose[0],rough_grasp_pose[1],rough_grasp_pose[2],rough_grasp_pose[3],rough_grasp_pose[4],rough_grasp_pose[5]),(0,0,0,1),(2,0,0); Movj LP[4], V[30], Z[0], Tool[0]; # S9: Send cart pose with eye In hand api.xyzSendCurrentCartPose(err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S10 使用机械臂上相机识别工件 ws_id = 1; item_codename = "item1"; api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S11 请求精确抓取位姿 fine_grasp_pose_token = 0; ws_id = 0; api.xyzReqGraspPose(ws_id, err_code, fine_grasp_pose_token); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S12 获取精确抓取位姿 api.xyzGetGraspPose(fine_grasp_pose_token, err_code, fine_grasp_pose, fine_grasp_pose_num, fine_pipeline_num, fine_register_num); If err_code <> 0 Halt 3; Goto L[1]; EndIf; If (fine_grasp_pose_num < 1) Halt 3; Goto L[1]; EndIf; LP[5] = (fine_grasp_pose[0],fine_grasp_pose[1],fine_grasp_pose[2],fine_grasp_pose[3],fine_grasp_pose[4],fine_grasp_pose[5]),(0,0,0,1),(3,0,0); # S13: Move To Fine grasp pose 移动到精确抓取位姿 # add more path pose, modify the offset PR[], unit: mm # 添加额外路径点 PR[3] = (0,0,100,0,0,0); Movj Offset(LP[5],PR[3]),V[30],Z[0],Tool[0]; Movj LP[5], V[30], Z[0], Tool[0]; Movj Offset(LP[5],PR[3]),V[30],Z[0],Tool[0]; # io operation Set Out[1],OFF; # S14: Move To place pose 移动到放置位姿 # add more path pose, modify the offset PR[], unit: mm # 添加额外路径点 PR[4] = (0,0,100,0,0,0); LP[6] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj Offset(LP[6],PR[4]),V[30],Z[0],Tool[0]; Movj LP[6], V[30], Z[0], Tool[0]; Movj Offset(LP[6],PR[4]),V[30],Z[0],Tool[0]; # io operation Set Out[1],ON; EndWhile; L[1]: Close Socket, gMasterPort; EndFunc; 轨迹移动同步模板： ---------------------------------- .. code-block:: Include "api.pro"; Func run() # S1,S2: Init And connect To IPC 初始化以及连接工控机 LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); EndWhile; gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# # String flow_name ="traj_sync.t"; # 切换任务 # api.xyzSwitchFlow(flow_name, err_code); # If err_code <> 0 # Halt 3; # Goto L[1]; # EndIf; # S3 Int ws_id = 0; String item_codename = "item1"; 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # io operation Set Out[1],ON; # S4: move To Home 回原点 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; Int pipeline_num = 0; Int register_num = 0; Int num = 0; Int wp_type[30]={}; Double pos[30][6]={}; While True # S5 请求计算抓取放置 ws_id = 0; api.xyzReqPickPlace(ws_id,err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S6 获取抓取入筐轨迹 ws_id = 0; api.xyzGetPickin(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S7 If num < 1 # S15 如果没有可以抓取的工件 Goto L[1]; EndIf; # S8 执行抓取入筐轨迹 api.xyzExecutePickInTraj(num, wp_type, pos); # io operation Set Out[1],OFF; # S9 获取抓取出筐轨迹 ws_id = 0; api.xyzGetPickout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S10 执行抓取出筐轨迹 api.xyzExecutePickOutTraj(num, wp_type, pos); # S11 获取放置入筐轨迹 ws_id = 0; api.xyzGetPlacein(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S12 执行放置入筐轨迹 api.xyzExecuteTraj(num, wp_type, pos); # io operation Set Out[1],ON; # S13 获取放置出筐轨迹 ws_id = 0; api.xyzGetPlaceout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S14 执行放置出筐轨迹 api.xyzExecuteTraj(num, wp_type, pos); EndWhile; L[1]: Close Socket, gMasterPort; EndFunc; 轨迹移动异步模板 ------------------------------------- .. code-block:: Include "api.pro"; Func run() # S1,S2: Init And connect To IPC 初始化以及连接工控机 LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); EndWhile; gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# # String flow_name ="traj_async.t"; # api.xyzSwitchFlow(flow_name, err_code); # If err_code <> 0 # Halt 3; # Goto L[1]; # EndIf; # S3 切换工件 Int ws_id = 0; String item_codename = "item1"; api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # io operation Set Out[1],ON; # S4: move To Home 回原点 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; Int pipeline_num = 0; Int register_num = 0; Int num = 0; Int wp_type[30]={}; Double pos[30][6]={}; While True # S5 请求计算抓取放置 ws_id = 0; api.xyzReqPickPlace(ws_id,err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S6 获取抓取入筐轨迹 ws_id = 0; api.xyzGetPickin(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S7 If num < 1 # S16 如果没有可以抓取的工件 Goto L[1]; EndIf; # S8 执行抓取入筐轨迹 api.xyzExecutePickInTraj(num, wp_type, pos); # io operation Set Out[1],OFF; # S9 请求抓取出筐轨迹 ws_id = 0; api.xyzGetPickout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S10 执行抓取出筐轨迹 api.xyzExecutePickOutTraj(num, wp_type, pos); # S11: request next pick and place plan in advance 请求下一次抓取放置 ws_id = 0; api.xyzReqPickPlace(ws_id,err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S12 获取放置入筐轨迹 ws_id = 0; api.xyzGetPlacein(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S13 执行放置入筐轨迹 api.xyzExecuteTraj(num, wp_type, pos); # io operation Set Out[1],ON; # S14 获取放置出筐轨迹 ws_id = 0; api.xyzGetPlaceout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # S15 执行放置出筐轨迹 api.xyzExecuteTraj(num, wp_type, pos); EndWhile; L[1]: Close Socket, gMasterPort; EndFunc;