샘플 프로그램10：MM_S10_Viz_Subtask
프로그램 소개
기능 설명 |
이 샘플은 두 개의 프로그램으로 구성되어 있습니다. 그 중, 서브 프로그램(백그라운드 프로그램)은 Mech-Viz프로젝트 실행을 트리거하고 계획된 경로를 가져오는 역할을 합니다. 메인 프로그램(포어그라운드 프로그램)은 계획된 경로에 따라 로봇을 이동시키며, 로봇이 피킹 영역에 떠날 때 서브 프로그램을 실행하여 다음 라운드 경로를 미리 계획하여 사이클 타임을 단축합니다.
|
||
파일 경로 |
서브 프로그램: Mech-Vision 및 Mech-Viz의 설치 디렉토리로 이동하여 메인 프로그램: Mech-Vision 및 Mech-Viz의 설치 디렉토리로 이동하여
|
||
필요한 프로젝트 |
Mech-Vision와 Mech-Viz 프로젝트 |
||
사용 전제 조건 |
|
|
프로그램 설명
아래는 서브 프로그램의 코드와 관련된 설명입니다.
| 서브 프로그램에서 Mech-Viz 프로젝트 실행을 트리거하고 계획 경로의 코드를 획득합니다. MM_S2_Viz_Basic 샘플과 비슷합니다. 따라서, MM_S10_Viz_Subtask의 MM_S2_Viz_Basic과 일치하는 부분은 다시 설명하지 않습니다. (일치하는 부분에 대한 정보는 MM_S2_Viz_Basic 샘플 프로그램 설명을 참조하십시오). |
MODULE MM_S10_Sub
!----------------------------------------------------------
! FUNCTION: run Mech-Viz project and get planned path
! in subtask(run together with MM_S10_Viz_Subtask)
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define variables
PERS num pose_num_b:=5;
PERS num vis_pose_num_b:=3;
PERS num toolid_b{5}:=[0,0,0,0,0];
PERS num label_b{5}:=[0,0,0,0,0];
PERS num status_b:=2100;
PERS bool flag_vis_b:=FALSE;
CONST jointtarget snap_jps_b:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS jointtarget jps_b{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_b:=FALSE;
!initialize communication parameters (initialization is required only once)
MM_Init_Socket "127.0.0.1",50000,300;
WHILE TRUE DO
IF flag_vis_b=TRUE THEN
MM_Open_Socket;
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps_b;
!get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData 1, pose_num_b, vis_pose_num_b, status_b;
!check whether planned path has been got from Mech-Viz successfully
IF status_b=2100 THEN
!save waypoints of the planned path to local variables one by one
MM_Get_Jps 1,jps_b{1},label_b{1},toolid_b{1};
MM_Get_JPS 2,jps_b{2},label_b{2},toolid_b{2};
MM_Get_JPS 3,jps_b{3},label_b{3},toolid_b{3};
ENDIF
MM_Close_Socket;
flag_vis_b:=FALSE;
ENDIF
ENDWHILE
ENDPROC
ENDMODULE위 코드는 서브 프로그램은 실행할 때 먼저 flag_vis_b이 FALSE로 설정되고 통신 파라미터를 초기화한 후 WHILE 순환을 통해 flag_vis_b의 값을 계속 모니터링합니다.
-
flag_vis_b가 TRUE로 설정된 경우, 서브 프로그램은 Mech-Viz 프로젝트 실행을 트리거하고 계획 경로를 획득한 후 flag_vis_b가 FALSE로 설정됩니다.
-
flag_vis_b가 FALSE일 때 서브 프로그램은 flag_vis_b의 값을 계속 모니터링합니다.
아래는 메인 프로그램의 코드와 관련된 설명입니다.
| 메인 프로그램에서 계획된 경로에 따라 피킹 및 배치의 코드를 실행합니다. MM_S2_Viz_Basic 샘플과 비슷합니다. 따라서, MM_S10_Viz_Subtask의 MM_S2_Viz_Basic과 일치하는 부분은 다시 설명하지 않습니다. (일치하는 부분에 대한 정보는 MM_S2_Viz_Basic 샘플 프로그램 설명을 참조하십시오). |
MODULE MM_S10_Viz_Subtask
!----------------------------------------------------------
! FUNCTION: run Mech-Viz project and get planned path
! in subtask (run together with MM_S10_Sub)
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
PERS bool flag_vis_b:=FALSE;
PERS num status_b:=2100;
PERS num pose_num_b:=5;
PERS num toolid_b{5}:=[0,0,0,0,0];
PERS num label_b{5}:=[0,0,0,0,0];
PERS num vis_pose_num_b:=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_b{5}:=
[
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0,0,0,0,0,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_10()
!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_b;
LOOP:
!move to wait position for picking
MoveL pick_wait_point,v1000,fine,gripper1;
!wait until subtask program finished
WaitUntil(flag_vis_b=FALSE);
!check whether planned path has been got from Mech-Viz successfully
IF status_b <> 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_b{1},v1000,fine,gripper1;
!move to picking waypoint
MoveAbsJ jps_b{2},v300,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!move to departure waypoint of picking
MoveAbsJ jps_b{3},v1000,fine,gripper1;
!move to intermediate waypoint of placing, and trigger Mech-Viz project and get planned path in advance
MoveJSync drop_waypoint,v1000,z50,gripper1,"trigger_vis_b";
!move to approach waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
!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_b()
flag_vis_b:=TRUE;
ENDPROC
ENDMODULE위 샘플 프로그램 코드에 해당하는 워크플로는 아래 그림에 표시되어 있습니다.
아래 표는 메인 프로그램 주요 코드의 설명입니다.
| 워크플로 | 코드와 설명 | ||
|---|---|---|---|
Mech-Viz 프로젝트 실행를 트리거하여 경로 계획을 획득하기 |
위의 코드는 메인 프로그램은 trigger_vis_b() 함수를 호출하는 것입니다. 이 함수의 코드는 아래와 같습니다. 위 코드는 trigger_vis_b() 함수에서 flag_vis_b 값이 TRUE로 설정되면, 서브 프로그램이 이 TRUE 값을 감지하고 Mech-Viz 프로젝트 실행를 트리거하여 경로 계획을 획득합니다. |
||
루프(피킹→다음 경로 계획을 트리거하기→배치)를 통해 다음 경로를 미리 계획하기 |
위 코드는 메인 프로그램이 LOOP와 GOTO LOOP 사이의 코드를 순환 실행함을 나타냅니다. 위 코드는 로봇이 피킹 사전 대기 웨이포인트로 이동한 후, 서브 프로그램이 계획된 경로를 획득하여 저장할 때까지 대기하는 것을 나타냅니다. (즉, flag_vis_b가 TRUE에서 FALSE로 변경됨). 위 코드는 로봇이 계획된 경로에 따라 피킹 접근 웨이포인트(jps_b{1})로 이동한 후 피킹 웨이포인트(jps_b{2})로 이동하여 피킹(예를 들어 setdo DO_1, 1;)하고, 피킹 출발 웨이포인트(jps_b{3})로 이동하는 것을 나타낸다. 위 코드는 로봇이 가장 빠른 방식으로 목표점(drop_waypoint, 즉 배치 과정의 중간 웨이포인트)으로 이동하며, 목표점에서 trigger_vis_b() 함수를 호출함을 나타냅니다. 메인 프로그램은 trigger_vis_b() 함수를 다시 호출합니다. 즉, Mech-Viz 프로젝트 실행를 다시 트리거하여 경로 계획을 획득합니다. 여기는 로봇이 이미 피킹 영역을 벗어나 있으므로, 배치 완성을 기다릴 필요 없고, 미리 다음 피킹 경로를 계획할 수 있습니다.
위 코드는 로봇이 먼저 순서대로 배치 접근 웨이포인트(RelTool(drop,0,0,-100)), 배치 웨이포인트(drop)로 이동한 후, 배치 작업(예: setdo DO_1, 0;)을 수행하고, 그 후 배치 출발 웨이포인트(RelTool(drop,0,0,-100))와 Home 포인트로 순차적으로 이동하는 것을 나타냅니다. |