サンプルプログラム
以下では、標準インターフェースを用いた把持と配置のサンプルプログラムとその操作手順について説明します。
事前準備
-
標準インターフェースの通信設定 を参照して標準インターフェースの通信設定が行われます。
-
キャリブレーションの操作手順 を参照し、コントローラにインストールされたキャリブレーションプログラムを実行するか、手動でロボットを移動させてキャリブレーションポイントを追加します。この手順によって外部パラメータのキャリブレーションが行われます。
-
必要なMech-Vision、Mech-Vizのプロジェクトを作成し、 プロジェクトの自動読み込み が設定されます。
-
TCP座標を正しく設定します。
-
ロボットが予期しない動作する可能性があるので、初めは速度を低く設定してください。
サンプルプログラムの紹介
簡単な把持・配置のサンプルプログラムは、Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある Mech-Center/Robot_Interface/FANUC/sample
フォルダに格納されています。
Mech-Visionからビジョン結果を取得
1: !FUNCTION:Eye to Hand simple pick ;
2: !2022-05-30 ;
3: ;
4: !SET Tool ;
5: UTOOL_NUM=1 ;
6: !Move to HOME Position ;
7:J P[1] 100% FINE ;
8: !Move to Camera capture Position ;
9:L P[2] 3000mm/sec FINE ;
10: !Set IP address and Port ;
11: CALL MM_INIT_SKT('8','192.168.1.20',50000,1) ;
12: WAIT .10(sec) ;
13: !Set Vision Recipe ;
14: //CALL MM_SET_MOD(1,1) ;
15: !Run Vision Project ;
16: CALL MM_START_VIS(1,1,2,10) ;
17: WAIT 1.00(sec) ;
18: CALL MM_GET_VIS(1,51,53) ;
19: IF R[53:status]<>1100,JMP LBL[99] ;
20: CALL MM_GET_POS(1,60,70,80) ;
21:L PR[60] 800mm/sec CNT100 Offset,PR[1] ;
22:L PR[60] 800mm/sec FINE ;
23: !Add object grasping logic here ;
24: ;
25:L PR[60] 800mm/sec CNT100 Offset,PR[1] ;
26: !Add transition point ;
27:L P[3] 800mm/sec FINE ;
28: !Move to DROP Position ;
29:L P[4] 800mm/sec CNT100 Offset,PR[2] ;
30:L P[4] 200mm/sec FINE ;
31: !Add object releasing logic here ;
32: ;
33:L P[4] 800mm/sec CNT100 Offset,PR[2] ;
34: !Move to HOME Position ;
35:J P[1] 100% FINE ;
36: END ;
37: ;
38: LBL[99:vision error] ;
39: PAUSE ;
プログラムロジック
-
ロボットを初期位置に移動させます。
-
ロボットを画像撮影位置に移動させます。
-
MM_INIT_SKTでソケット通信を初期化します。
-
Mech-Visionのプロジェクトでパラメータレシピを使用する場合、使用するレシピはMM_SET_MODで設定することができます。
-
MM_Start_VisでMech-Visionプロジェクトに実行をトリガーします。
-
1秒待機します。Eye-In-Handでは、撮像が完了するまでロボットが完全に停止している必要があるため、WAIT命令が必要です。Eye-To-Handでは、MM_START_VISとMM_GET_VISの間に移動命令がある場合、このWAIT命令は必要ありません。
-
Mech-Visionからの処理結果を取得します。
-
返されたステータスコードを確認します。エラーコードが返された場合、プログラムは一時停止します。
-
ロボットを把持位置に移動させ、把持を行います。
-
ロボットを把持位置と配置位置間の経路に移動させます。
-
設定した配置位置にロボットを移動し、ワークを配置します。
カスタマイズ
-
初期位置を定義
J P[1] 100% FINE
:レジスタ P[1]で初期位置を定義しています。 -
TCP座標を定義
UTOOL_NUM=1
:サンプルではUTOOL_NUM=1でTCP座標を定義しています。TCP位置は、TCP座標系1の値を変更することで設定できます。 -
画像撮像位置を定義
L P[2] 3000mm/sec FINE
:レジスタ P[2]で画像撮影位置を定義しています。 -
移動経路を定義
L P[3] 800mm/sec FINE
:把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。 -
配置位置を定義
L P[4] 200mm/sec FINE
:レジスタ P[4]で配置位置を定義しています。 -
把持、配置位置からのZ方向オフセットを定義
-
把持位置に近づけます
L PR[60] 800mm/sec CNT100 Offset,PR[1]
:把持位置の上まで移動します。オフセットはPR[1]を使用します。オフセットはロボットハンドに衝突が発生しないようにするために設定しています。 -
把持位置から離れます
L PR[60] 800mm/sec CNT100 Offset,PR[1]
:把持位置から上げます。オフセットはPR[1]を使用します。 -
配置位置に近づけます
L P[4] 800mm/sec CNT100 Offset,PR[2]
:配置位置の上まで移動します。オフセットはPR[2]を使用します。 -
配置位置から離れます
L P[4] 800mm/sec CNT100 Offset,PR[2]
:配置位置から上げます。オフセットはPR[2]を使用します。
-
-
ロボットハンドの制御ロジックを追加
ワークを把持、配置する際、ロボットハンドの制御ロジックを追加します。
Mech-Vizから動作計画を取得
1: !FUNCTION:Eye to Hand simple pick ;
2: !and place with Mech-Viz ;
3: !2022-05-30 ;
4: ;
5: !SET Tool ;
6: UTOOL_NUM=1 ;
7: !Move to HOME Position ;
8:J P[1] 100% FINE ;
9: !Move to Camera capture Position ;
10:L P[2] 3000mm/sec FINE ;
11: !Set IP address and Port ;
12: CALL MM_INIT_SKT('8','192.168.1.20',50000,1) ;
13: WAIT .10(sec) ;
14: !Set Vision Recipe ;
15: //CALL MM_SET_MOD(1,1) ;
16: !Run Viz Project ;
17: CALL MM_START_VIZ(0,10) ;
18: WAIT .10(sec) ;
19: !set branch exitport ;
20: //CALL MM_SET_BCH(1,1) ;
21: !get planned path ;
22: CALL MM_GET_VIZ(2,51,52,53) ;
23: IF R[53:status]<>2100,JMP LBL[99] ;
24: CALL MM_GET_POS(1,60,70,80) ;
25: CALL MM_GET_POS(2,61,71,81) ;
26: CALL MM_GET_POS(3,62,72,82) ;
27: !follow the planned path to pick ;
28:L PR[60] R[80]mm/sec FINE ;
29:L PR[61:Vpos] R[81]mm/sec FINE ;
30: !Add object grasping logic here ;
31: ;
32:L PR[62] R[82]mm/sec FINE ;
33: !Add transition point ;
34:L P[3] 800mm/sec FINE ;
35: !Move to DROP Position ;
36:L P[4] 800mm/sec CNT100 Offset,PR[2] ;
37:L P[4] 200mm/sec FINE ;
38: !Add object releasing logic here ;
39: ;
40:L P[4] 800mm/sec CNT100 Offset,PR[2] ;
41: !Move to HOME Position ;
42:J P[1] 100% FINE ;
43: END ;
44: ;
45: LBL[99:vision error] ;
46: PAUSE ;
プログラムロジック
Mech-Vizが出力した位置姿勢に従い、設定した配置位置にロボットを移動し、ワークを配置します。
-
ロボットを初期位置に移動させます。
-
ロボットを画像撮影位置に移動させます。
-
MM_INIT_SKTでソケット通信を初期化します。
-
Mech-Visionのプロジェクトでパラメータレシピを使用する場合、使用するレシピはMM_SET_MODで設定することができます。
-
Mech-Vizプロジェクトの実行をトリガーします。
-
Mech-Vizから計画した動作経路を取得します。
-
返されたステータスコードを確認します。エラーコードが返された場合、プログラムは一時停止します。
-
取得した位置姿勢を、PR[60]、PR[61]およびPR[62] に書き込みます。
-
Mech-Vizから計画された経路に沿ってロボットを動かし、把持を実行します。
-
ロボットを把持位置と配置位置間の経路に移動させます。
-
設定した配置位置にロボットを移動し、ワークを配置します。ロボットを初期位置に戻します。
カスタマイズ
-
初期位置を定義
J P[1] 100% FINE
:レジスタ P[1]で初期位置を定義しています。 -
TCP座標を定義
UTOOL_NUM=1
:サンプルではUTOOL_NUM=1でTCP座標を定義しています。TCP位置は、TCP座標系1の値を変更することで設定できます。 -
画像撮像位置を定義
L P[2] 3000mm/sec FINE
:レジスタ P[2]で画像撮影位置を定義しています。 -
移動経路を定義
L P[3] 800mm/sec FINE
:把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。 -
配置位置を定義
L P[4] 200mm/sec FINE
:レジスタ P[4]で配置位置を定義しています。 -
ロボットハンドの制御ロジックを追加
ワークを把持、配置する際、ロボットハンドの制御ロジックを追加します。
Mech-Visionの「経路計画」ステップを使用して経路を取得
1: !FUNCTION:Eye to Hand simple pick ;
2: !and place with Mech-Vision Path ;
3: !Planning Step ;
4: !2023-01-10 ;
5: ;
6: !SET Tool ;
7: UTOOL_NUM=1 ;
8: !Move to HOME Position ;
9:J P[1] 100% FINE ;
10: !Move to Camera capture Position ;
11:L P[2] 3000mm/sec FINE ;
12: !Set IP address and Port ;
13: CALL MM_INIT_SKT('8','192.168.1.20',50000,1) ;
14: WAIT .10(sec) ;
15: !Set Vision Recipe ;
16: //CALL MM_SET_MOD(1,1) ;
17: !Run Vision Project ;
18: CALL MM_START_VIS(1,0,2,10) ;
19: WAIT 1.00(sec) ;
20: CALL MM_GET_VISP(1,2,51,52,53) ;
21: IF R[53:status]<>1103,JMP LBL[99] ;
22: FOR R[100:i]=1 TO R[51:pos_num] ;
23: R[101:preg no]=59+R[100:i] ;
24: R[102:lbl no]=69+R[100:i] ;
25: R[103:spd no]=79+R[100:i] ;
26: CALL MM_GET_POS(R[100:i],R[101:preg no],R[102:lbl no],R[103:spd no]) ;
27: ENDFOR ;
28: !follow the planned path to pick ;
29: FOR R[110:j]=1 TO R[51:pos_num] ;
30: R[111:preg]=59+R[110:j] ;
31: R[112:spd]=79+R[110:j] ;
32:L PR[R[111]] R[R[112]]mm/sec FINE ;
33: IF (R[110:j]=R[52:vpos_num]) THEN ;
34: PAUSE ;
35: !Add object grasping logic here ;
36: ENDIF ;
37: ENDFOR ;
38: !Add transition point ;
39:L P[3] 800mm/sec FINE ;
40: !Move to DROP Position ;
41:L P[4] 800mm/sec CNT100 Offset,PR[2] ;
42:L P[5] 200mm/sec FINE ;
43: !Add object releasing logic here ;
44: ;
45:L P[6] 800mm/sec CNT100 Offset,PR[2] ;
46: !Move to HOME Position ;
47:J P[7] 100% FINE ;
48: END ;
49: ;
50: LBL[99:vision error] ;
51: PAUSE ;
プログラムロジック
このサンプルは、Mech-Visionの「経路計画」ステップで計画した経路を取得して把持を行い、固定配置点に配置します。
-
ロボットを初期位置に移動させます。
-
ロボットを画像撮影位置に移動させます。
-
MM_INIT_SKTでソケット通信を初期化します。
-
Mech-Visionのプロジェクトでパラメータレシピを使用する場合、使用するレシピはMM_SET_MODで設定することができます。
-
MM_Start_VisでMech-Visionプロジェクトに実行をトリガーします。
-
MM_GET_VISPでMech-Visionの「経路計画」ステップから出力された動作経路を取得します。
-
返されたステータスコードを確認します。エラーコードが返された場合、プログラムは一時停止します。
-
計画した経路点の数に従って、ループ文で取得した経路点の位置姿勢をPR[60]から順に位置レジスタに格納します。
-
移動コマンドを用いて計画した経路に沿ってロボットを移動させ、把持を完了したらビジョン領域から離れさせます。
-
ロボットを把持位置と配置位置間の経路に移動させます。
-
設定した配置位置にロボットを移動し、ワークを配置します。ロボットを初期位置に戻します。
カスタマイズ
-
初期位置を定義
J P[1] 100% FINE
:レジスタ P[1]で初期位置を定義しています。 -
TCP座標を定義
UTOOL_NUM=1
:サンプルではUTOOL_NUM=1でTCP座標を定義しています。TCP位置は、TCP座標系1の値を変更することで設定できます。 -
画像撮像位置を定義
L P[2] 3000mm/sec FINE
:レジスタ P[2]で画像撮影位置を定義しています。 -
移動経路を定義
L P[3] 800mm/sec FINE
:把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。 -
配置位置を定義
L P[4] 200mm/sec FINE
:レジスタ P[4]で配置位置を定義しています。 -
ロボットハンドの制御ロジックを追加
ワークを把持、配置する際、ロボットハンドの制御ロジックを追加します。