샘플 프로그램 20: MM_S20_Viz_PlanAllVision
프로그램 소개
기능 설명 |
로봇이 Mech-Viz 프로젝트 실행을 트리거한 후 루프를 통해 모든 계획된 경로를 획득하여 피킹 및 배치 작업을 수행합니다. 이 샘플에서는 카메라가 이미지를 한 번만 캡처하면 Mech-Viz가 모든 비전 결과에 대한 피킹 경로를 계획합니다. 일반적으로 멀티 피킹 시나리오에 적용합니다. |
||
파일 경로 |
Mech-Vision 및 Mech-Viz의 설치 디렉토리로 이동하여
|
||
필요한 프로젝트 |
Mech-Vision와 Mech-Viz 프로젝트
|
||
사용 전제 조건 |
|
| 이 샘플 프로그램은 참고용으로 제공됩니다. 사용자는 실제 상황에 맞춰 이 내용을 바탕으로 수정해야 하며, 해당 프로그램을 그대로 사용하지 않도록 하십시오. |
프로그램 설명
다음에는 MM_S20_Viz_PlanAllVision 샘플 프로그램의 코드와 관련 설명입니다.
| MM_S2_Viz_Basic 샘플과 비하면, 이 샘플은 루프를 통해 모든 계획된 경로를 획득하여 피킹 및 배치 작업을 수행하는 기능만 수정되었습니다.(이 부분의 코드는 굵게 표시됨) 따라서, MM_S20_Viz_PlanAllVision의 MM_S2_Viz_Basic과 일치하는 부분은 다시 설명하지 않습니다. (일치하는 부분에 대한 정보는 MM_S2_Viz_Basic 샘플 프로그램 설명을 참조하십시오). |
MODULE MM_S20_Viz_PlanAllVision
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project, plan all vision
! results and get all planned results using command 210
! Mech-Mind, 2023-12-25
!----------------------------------------------------------
!define local num variables
LOCAL VAR num pose_num:=0;
LOCAL VAR num status:=0;
LOCAL VAR num vis_pose_num:=0;
LOCAL VAR num count:=0;
LOCAL VAR num pick_cnt:=0;
LOCAL VAR num residual:=0;
LOCAL VAR num movetype{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num toolnum{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
LOCAL VAR num speed{20}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,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 VAR robtarget pickpoint:=[[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 VAR robtarget pick_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 VAR 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 VAR 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{20}:=
[
[[-4.5438,26.6029,0.467,-0.1719,62.9165,-5.0819],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-4.5664,32.0439,7.6047,-0.1988,50.3379,-5.0558],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-4.5363,25.4009,-2.7016,-0.1659,67.2872,-5.0885],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-20.7719,15.0701,16.7232,0.1053,57.9831,-111.82],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-20.7567,21.7711,23.8763,0.1281,44.1289,-111.841],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-20.777,13.5249,13.6202,0.1005,62.6312,-111.816],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0.3363,10.0698,23.0021,0.0017,56.8211,89.2201],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0.3365,17.2854,30.3891,0.0022,42.2185,89.2197],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[0.3362,8.4182,19.8363,0.0017,61.6386,89.2202],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-2.6715,8.5325,24.7355,0.0089,56.6276,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-2.6701,15.8879,32.2202,0.0112,41.7875,265.897],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[-2.672,6.8572,21.5409,0.0085,61.4976,265.9],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[21.387,12.8326,19.5238,-0.0095,57.6221,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[21.3856,19.7174,26.8272,-0.0117,43.434,110.351],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[21.3874,11.2544,16.3788,-0.0091,62.3455,110.349],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[10.5881,41.5724,-23.6476,-0.3127,72.1244,189.431],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[10.5502,45.3582,-15.4893,-0.3431,60.1803,189.468],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[10.6008,40.9927,-27.4632,-0.306,76.5197,189.419],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[28.7848,90.399,-13.628,14.5039,10.6719,-165.371],[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_20()
!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;
RECAP:
!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;
!get planned path
MM_Get_PlanData 0, 3, 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
!close socket connection
MM_Close_Socket;
!save all waypoint data to local variables using for-loop, a maximum of 50 points are supported
FOR i FROM 1 TO pose_num DO
count:=i;
MM_Get_PlanJps count,3,JPS{count},movetype{count},toolnum{count},speed{count};
ENDFOR
!parse pick cycle count, here suppose 3 points per planned path
pick_cnt:= pose_num DIV 3;
residual:= pose_num MOD 3;
!check if parsed data is valid; if not, retry to get planned path or add some error handling logic
IF (pick_cnt<1) OR (residual<>0) THEN
Stop;
GOTO RECAP;
ENDIF
!repeatedly run pick-and-place cycle using for-loop
FOR i FROM 1 TO pick_cnt DO
count:=(i-1)*3;
!move to intermediate waypoint of picking
MoveJ pick_waypoint,v1000,z50,gripper1;
!follow the planned path to pick
MoveAbsJ jps{1+count},v1000,fine,gripper1;
MoveAbsJ jps{2+count},v1000,fine,gripper1;
!add object grasping logic here, such as "setdo DO_1, 1;";
Stop;
MoveAbsJ jps{3+count},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;
!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 to intermediate waypoint of placing
MoveJ drop_waypoint,v1000,z50,gripper1;
ENDFOR
!finish pick and-place cycle, and jump back to camera capturing
GOTO RECAP;
ENDPROC
ENDMODULE위 샘플 프로그램 코드에 해당하는 워크플로는 아래 그림에 표시되어 있습니다.
아래 표는 새롭게 추가된 기능의 설명입니다. 명령어 이름의 링크를 클릭하면 해당 명령의 상세 설명을 확인할 수 있습니다.
| 워크플로 | 코드와 설명 |
|---|---|
계획 경로를 획득하기 |
|
루프를 통해 계획 경로를 저장하기 |
|
pick_cnt 및 residual를 계산하기 |
본 샘플에서는 매번 계획된 피킹 경로에 3개의 웨이포인트가 포함된다고 가정합니다. "pose_num DIV 3"는 pose_num을 3로 나눈 몫을 의미합니다. "pose_num MOD 3"는 pose_num을 3로 나눈 나머지를 의미합니다. pick_cnt는 계획된 총 피킹 횟수를 나타냅니다. residual 값이 0이 아닌 경우, 특정 계획된 피킹 웨이포인트 개수가 3개 미만임을 의미하며, 이는 경로 계획에 이상이 발생했음을 나타내므로 다시 계획을 수행해야 합니다. |
경로 계획 오류가 발생했는지 확인하기 |
피킹 횟수 pick_cnt가 1보다 작은 경우 또는 residual 값이 0이 아닌 경우, 경로 계획 중 오류가 발생했음을 의미하며 사용자는 여기에서 코드 처리를 추가해야 합니다. 예를 들어 Mech-Viz 프로젝트를 다시 트리거하여 계획 경로를 획득하는 코드를 추가합니다. |
루프를 통해 피킹 및 배치 작업을 실행하기 |
위의 코드는 FOR 루프 내에서 로봇이 매번 계획된 3개의 웨이포인트로 이동하여 피킹 작업을 수행한 후, 이어서 배치 작업을 실행하는 것을 나타냅니다. i는 순환 횟수를 제어하는 변수로, 1부터 시작하여 매 순환마다 1씩 증가하며, 피킹 횟수 pick_cnt보다 커지면 루프가 종료됩니다. i가 1 증가할 때마다 count가 3 증가하며, 이를 통해 {1+count}~{3+count}는 매번 계획된 3개의 웨이포인트가 전체 계획 경로에서 차지하는 번호를 나타냅니다. |