サンプルプログラム7：MM_S7_Viz_SwitchTCP
プログラム概要
機能 |
ロボットはMech-Vizプロジェクトをトリガーして実行し、計画された経路を取得します。その後、把持点のロボットハンド番号に基づいて対応するロボットハンドを切り替え、把持および配置を実行します。 |
||
ファイル場所 |
Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある
|
||
必要なプロジェクト |
Mech-VisionとMech-Vizプロジェクト |
||
使用前提 |
|
| このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。 |
プログラム説明
以下はMM_S7_Viz_SwitchTCPサンプルプログラムのコードと関連する説明です。
| このサンプルは、MM_S2_Viz_Basicサンプルに比べて、ロボットハンド番号に基づいてロボットハンドを切り替える機能（太字部分のコード）のみが追加されています。そのため、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信号をオフにし、リセットすることができます。以下は、サンプルコードの一例です。 |