サンプルプログラム2：MM_S2_Viz_Basic
プログラム概要
機能 |
ロボットがMech-Vizプロジェクトをトリガーして実行し、その後計画された経路を取得して、把持および配置を実行します。 |
||
ファイル場所 |
Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある
|
||
必要なプロジェクト |
Mech-VisionとMech-Vizプロジェクト |
||
使用前提 |
|
| このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。 |
プログラム説明
以下はMM_S2_Viz_Basicサンプルプログラムのコードと関連する説明です。
MODULE MM_S2_Viz_Basic
!----------------------------------------------------------
! FUNCTION: trigger Mech-Viz project and get planned path
! 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}:=
[
[[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_2()
!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;
!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
!close socket connection
MM_Close_Socket;
!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;
!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;
!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;
ENDPROC
ENDMODULE上記のサンプルプログラムの処理流れは、下図の通りです。
下表は上記のプログラムのコードとその説明です。コマンド名のリンクをクリックすることで、その詳細を確認できます。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
変数を定義 |
ローカル変数を定義します。上記のローカル変数は本プログラムのみで有効となります。 ユーザーは事前に初期位置（home）、Mech-Vizプロジェクトに送信する位置姿勢（snap_jps）、撮影位置（camera_capture）、配置の中間点（drop_waypoint）、配置点（drop）をティーチングし、ロボットハンドのデータ（gripper1）を設定する必要があります。
|
||
加速度と速度を設定 |
|
||
初期位置に移動 |
このコードは、ロボットが3000mm/sの速度で、ティーチングされた初期位置までばっちり移動する（外部軸からの影響なし）ことを意味します。 |
||
通信初期化 |
ロボットが MM_Init_Socket コマンドを使用して、通信対象（IPC）のIPアドレス（127.0.0.1）、ポート番号（50000）、タイムアウト待機時間（300秒）を指定することを意味します。
|
||
撮影位置に移動 |
このコマンドは、ロボットが1000mm/sの移動速度で、直線移動の方式で撮影位置に移動することを意味します。 |
||
通信確立 |
ロボットは MM_Open_Socket コマンドを使用して、ビジョンシステムとの間のTCPプロトコル通信接続を確立します。 |
||
Mech-Vizプロジェクトをトリガー |
このコマンドは、ロボットがビジョンシステムをトリガーしてMech-Vizプロジェクトを実行し、Mech-VizがMech-Visionから出力されたビジョン結果に基づいて把持経路を計画することを意味します。 |
||
計画された経路を取得 |
このコマンドは、ロボットがMech-Vizプロジェクトから返された計画経路を取得することを意味します。
ステータスコードが2100の場合、ロボットが計画された経路の取得に成功したことを意味します。そうでない場合、ビジョンシステムにエラーが発生したことを意味します。ユーザーは、特定のエラーコードに応じて適切な処理を行うことが可能です。 |
||
通信接続を切断 |
ロボットは MM_Close_Socket コマンドを使用して、ビジョンシステムとの間のTCPプロトコル通信接続を切断します。 |
||
計画された経路を保存 |
「MM_Get_JPS 1,jps{1},label{1},toolid{1};」コマンドは、最初の経路点の関節角度、ラベル、およびロボットハンド番号をそれぞれ指定された変数に保存することを意味します。
|
||
把持のアプローチ点に移動 |
ロボットが把持のアプローチ点（jps{1}が表す位置）に移動します。 |
||
把持点に移動 |
ロボットが把持点（jps{2}が表す位置）に移動します。 |
||
DO設定による把持 |
ロボットが把持点に到達した後、DOコマンド（例：setdo DO_1, 1;）を設定して、ロボットハンドを制御し、ワークを把持します。実際の状況に応じてDO設定を調整する必要があります。
|
||
離れる点に移動 |
ロボットが把持の離れる点（jps{3}が表す位置）に移動します。 |
||
中間点に移動 |
このコマンドは、ロボットが関節移動の方式で、速度1000mm/s、ブレンド半径50mmで、把持の離れる点と配置のアプローチ点の間の中間点に移動することを意味します。
|
||
配置のアプローチ点に移動 |
このコマンドは、ロボットが配置点から100ｍｍ上まで移動することを意味します。この位置を配置のアプローチ点と呼びます。
|
||
配置点に移動 |
ロボットが配置のアプローチ点から配置点に移動します。 |
||
DO設定による配置 |
ロボットが配置点に到達した後、DOコマンド（例：setdo DO_1, 0;）を設定して、ロボットハンドを制御し、ワークを配置します。実際の状況に応じてDO設定を調整する必要があります。
|
||
配置の離れる点に移動 |
ロボットは配置点から100㎜上まで移動します。この位置を配置の離れる点と呼びます。
|
||
初期位置に移動 |
ロボットが配置の離れる点から初期位置に戻ります。 |