샘플 프로그램9: MM_S9_Viz_RunInAdvance
프로그램 소개
기능 설명 |
로봇이 물체를 피킹할 때 Mech-Viz프로젝트를 트리거하고 메시지 분기를 통해 배치할 때 카메라 이미지 캡처를 트리거합니다. 이를 통해 다음 경로를 미리 계획하여 사이클 타임을 단축할 수 있습니다. |
||
파일 경로 |
Mech-Vision 및 Mech-Viz의 설치 디렉토리로 이동하여
|
||
필요한 프로젝트 |
Mech-Vision와 Mech-Viz 프로젝트
|
||
사용 전제 조건 |
|
| 이 샘플 프로그램은 참고용으로 제공됩니다. 사용자는 실제 상황에 맞춰 이 내용을 바탕으로 수정해야 하며, 해당 프로그램을 그대로 사용하지 않도록 하십시오. |
프로그램 설명
다음에는 MM_S9_Viz_RunInAdvance 샘플 프로그램의 코드와 관련 설명입니다.
| MM_S5_Viz_SetBranch 샘플과 비하면, 이 샘플은 다음 경로를 미리 계획할 수 있는 기능만 추가되었습니다.(이 기능의 코드가 굵게 표시됨). 따라서, 아래 부분에서는 다음 경로를 미리 계획하는 기능만 설명됩니다. (MM_S9_Viz_RunInAdvance의 MM_S5_Viz_SetBranch과 일치하는 부분에 대한 정보는 MM_S5_Viz_SetBranch 샘플 프로그램 설명을 참조하십시오). |
MODULE MM_S9_Viz_RunInAdvance
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project then set branch and get
! planned path, trigger Mech-Viz project and set branch in
! advance during pick&place process
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num toolid{5}:=[0,0,0,0,0];
LOCAL VAR num vis_pose_num:=0;
LOCAL VAR num count:=0;
LOCAL VAR num label{5}:=[0,0,0,0,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 CONST jointtarget snap_jps:=[[0,0,0,0,90,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
LOCAL PERS robtarget camera_capture:=[[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]];
LOCAL PERS jointtarget jps{5}:=
[
[[11.1329,49.0771,-36.9666,0.5343,79.2476,-169.477],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[11.2355,52.1281,-23.3996,0.5938,62.6295,-169.548],[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]],
[[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]]
];
!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_9()
!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;
!initialize communication parameters (initialization is required only once)
MM_Init_Socket "127.0.0.1",50000,300;
!move to image-capturing position
MoveL camera_capture,v1000,fine,gripper1;
!open socket connection
MM_Open_Socket;
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps;
!set branch exit port
MM_Set_Branch 1,1;
LOOP:
!get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData 1, pose_num, vis_pose_num, status;
!check whether planned path has been got from Mech-Viz successfully
IF status <> 2100 THEN
!add error handling logic here according to different error codes
!e.g.: status=2038 means no point cloud in ROI
Stop;
ENDIF
!save waypoints of the planned path to local variables one by one
MM_Get_Jps 1,jps{1},label{1},toolid{1};
MM_Get_JPS 2,jps{2},label{2},toolid{2};
MM_Get_JPS 3,jps{3},label{3},toolid{3};
!follow the planned path to pick
!move to approach waypoint of picking
MoveAbsJ jps{1},v1000,fine,gripper1;
!move to picking waypoint
MoveAbsJ jps{2},v300,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;"
Stop;
!trigger Mech-Viz project but not to trigger camera capturing
MM_Start_Viz 2,snap_jps;
!move to departure waypoint of picking
MoveAbsJ jps{3},v1000,fine,gripper1;
!move to intermediate waypoint of placing
MoveJ drop_waypoint,v1000,z50,gripper1;
!move to approach waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,gripper1;
!set branch exit port and trigger camera capturing when robot moves out of camera’s field of view
MM_Set_Branch 1,1;
!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;
END_LOOP:
!close socket connection
MM_Close_Socket;
ENDPROC
ENDMODULE위 샘플 프로그램 코드에 해당하는 워크플로는 아래 그림에 표시되어 있습니다.
아래는 다음 경로를 미리 계획하는 코드와 관련된 설명입니다.
| 워크플로 | 코드와 설명 | ||
|---|---|---|---|
루프(피킹→다음 경로 계획을 트리거하기→배치)를 통해 다음 경로를 미리 계획하기 |
위 코드는 LOOP와 GOTO LOOP 사이의 코드를 순환 실행함을 나타냅니다. 위 코드는 로봇이 MM_Get_VizData 명령어를 통해 Mech-Viz가 계획한 경로를 획득함을 나타냅니다. 위 코드는 로봇이 MM_Get_Jps 명령어를 통해 계획된 경로를 지정된 변수에 저장함을 나타냅니다. 이 샘플에서는 jps{1}, jps{2}, jps{3}이 각각 피킹 접근 웨이포인트, 피킹 웨이포인트, 피킹 출발 웨이포인트로 설정됩니다. 위 코드는 로봇이 순차적으로 피킹 접근 웨이포인트와 피킹 웨이포인트로 이동한 후, DO 명령어(예: "setdo DO_1, 1;")를 설정하여 말단 장치를 제어하여 피킹를 수행하는 것을 나타냅니다.
위 코드는 로봇이 MM_Start_Viz명령어를 통해 Mech-Viz 프로젝트를 다시 트리거함을 나타냅니다. 이제 로봇은 이번 계획된 피킹 경로를 획득하고 피킹 웨이포인트로 이동했으므로 배치 완료를 기다릴 필요 없이 Mech-Viz 프로젝트 실행을 미리 트리거하여 다음 경로 계획을 할 수 있습니다.
위 코드는 로봇이 순서대로 피킹 출발 웨이포인트(jps{3}), 중간 웨이포인트(drop_waypoint), 배치 접근 웨이포인트(RelTool(drop,0,0,-100))로 이동합니다. 위 코드는 로봇이 MM_Set_Branch 명령어를 통해 "메시지 분기" 스텝의 아웃 포트를 설정함을 나타냅니다. 이 아웃 포트 분기에서 "비전 인식" 스텝이 실행되며, Mech-Viz는 비전 인식 결과에 따라 다음 피킹 경로를 미리 계획합니다. 위 코드는 로봇이 먼저 배치 웨이포인트(drop)로 이동한 후, 배치 작업(예: setdo DO_1, 0;)을 수행하고, 그 후 배치 출발 웨이포인트(RelTool(drop,0,0,-100))와 Home 포인트로 순차적으로 이동하는 것을 나타냅니다. |