샘플 프로그램7: MM_S7_Viz_SwitchTCP
프로그램 소개
기능 설명 |
로봇이 Mech-Viz프로젝트 실행을 트리거한 후, 경로 계획 결과를 휙득하고 피킹 웨이포인트 말단장치의 번호에 따라 적절한 말단장치를 전환하여 피킹 및 배치를 수해합니다. |
||
파일 경로 |
Mech-Vision 및 Mech-Viz의 설치 디렉토리로 이동하여
|
||
필요한 프로젝트 |
Mech-Vision와 Mech-Viz 프로젝트 |
||
사용 전제 조건 |
|
이 샘플 프로그램은 참고용으로 제공됩니다. 사용자는 실제 상황에 맞춰 이 내용을 바탕으로 수정해야 하며, 해당 프로그램을 그대로 사용하지 않도록 하십시오. |
프로그램 설명
다음에는 MM_S7_Viz_SwitchTCP 샘플 프로그램의 코드와 관련 설명입니다.
MM_S2_Viz_Basic 샘플과 비하면, 이 샘플은 주로 말단장치의 번호에 따라 적절한 말단장치를 전환할 수 있는 기능이 추가되었습니다.(이 기능의 코드가 굵게 표시됨). 따라서, MM_S7_Viz_SwitchTCP의 MM_S2_Viz_Basic과 일치하는 부분은 다시 설명하지 않습니다. (일치하는 부분에 대한 정보는 MM_S2_Viz_Basic 샘플 프로그램 설명을 참조하십시오). |
MODULE MM_S7_Viz_SwitchTCP
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project and get planned path,
! switch TCP according to the label
! 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 robtarget pos{5}:=
[
[[502.502,99.5586,399.968],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[500,100,300],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[500,100,300],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[500,100,300],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]],
[[502.502,99.5586,399.968],[0.00226227,-0.99991,-0.00439596,0.0124994],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]]
];
!define LOCAL tooldata variables
LOCAL PERS tooldata cur_gripper:=[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];
LOCAL PERS tooldata gripper{5}:=,[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]],
[TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]]];
PROC Sample_7()
!set the acceleration parameters
AccSet 50, 50;
!set the velocity parameters
VelSet 50, 1000;
!move to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,cur_gripper;
!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,cur_gripper;
!open socket connection
MM_Open_Socket;
!trigger Mech-Viz project
MM_Start_Viz 2,snap_jps;
!get planned path, 1st argument (2) means getting pose in TCP
MM_Get_VizData 2, 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 waypoints of the planned path to local variables one by one
MM_Get_Pose 1,pos{1},label{1},toolid{1};
MM_Get_Pose 2,pos{2},label{2},toolid{2};
MM_Get_Pose 3,pos{3},label{3},toolid{3};
!reset tool signals according to received toolId
IF toolid{vis_pose_num}=1 Then
cur_gripper:=gripper{toolid{vis_pose_num}};
!reset tool signal;
ELSEIF toolid{vis_pose_num}=2 Then
cur_gripper:=gripper{toolid{vis_pose_num}};
!reset tool signal;
ELSE
Stop;
!reset tool signal;
ENDIF
!follow the planned path to pick
!move to approach waypoint of picking
MoveL pos{1},v1000,fine,cur_gripper;
!move to picking waypoint
MoveL pos{2},v300,fine,cur_gripper;
!add object grasping logic here
IF toolid{vis_pose_num}=1 Then
Stop;
!open tool signal;
ELSEIF toolid{vis_pose_num}=2 Then
Stop;
!open tool signal;
ELSE
Stop;
!open tool signal;
ENDIF
!move to departure waypoint of picking
MoveL pos{3},v1000,fine,cur_gripper;
!move to intermediate waypoint of placing
MoveJ drop_waypoint,v1000,z50,cur_gripper;
!move to approach waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,cur_gripper;
!move to placing waypoint
MoveL drop,v300,fine,cur_gripper;
!add object releasing logic here
IF toolid{vis_pose_num}=1 Then
Stop;
!reset tool signal;
ELSEIF toolid{vis_pose_num}=2 Then
Stop;
!reset tool signal;
ELSE
Stop;
!reset tool signal;
ENDIF
!move to departure waypoint of placing
MoveL RelTool(drop,0,0,-100),v1000,fine,cur_gripper;
!move back to robot home position
MoveAbsJ home\NoEOffs,v3000,fine,cur_gripper;
ENDPROC
ENDMODULE
위 샘플 프로그램 코드에 해당하는 워크플로는 아래 그림에 표시되어 있습니다.

아래 표는 새롭게 추가된 코드의 설명입니다.
워크플로 | 코드와 설명 | ||
---|---|---|---|
계획 경로를 획득하기 |
따라서, 이 명령어는 로봇이 Mech-Viz 프로젝트가 반환한 계회된 경로를 획득하는 명령어입니다.
|
||
계획 경로를 저장하기 |
따라서,“MM_Get_Pose 1,pos{1},label{1},toolid{1}”는 첫 번째 웨이포인트의 말단장치 포즈, 레이블 및 말단장치 번호를 지정된 변수에 저장하는 명령어입니다.
|
||
말단장치의 번호에 따라 적절한 말단장치를 전환하기 |
toolid{vis_pose_num}는 피킹 웨이포인트의 말단 장치 번호입니다. 위의 설명은 toolid{vis_pose_num} 변수 값에 따라 로봇이 목표 포즈(피킹 웨이포인트)로 이동할 때 사용할 말단장치를 결정할 수 있음을 의미합니다. 즉, 말단장치 번호에 따라 말단장치를 변경하는 것입니다. 위 코드의 설명은 아래와 같습니다.
로봇이 피킹 웨이포인트로 이동할 때, 말단장치 번호에 해당하는 DO 신호를 활성화할 수 있습니다. 아래 코드를 참조하십시오.
로봇이 배치 웨이포인트로 이동할 때, 말단장치 번호에 해당하는 DO 신호를 비활성화하고 리셋할 수 있습니다. 아래 코드를 참조하십시오.
|