サンプルプログラム

本節ではMech-Centerで提供される標準インターフェースを用いた把持と配置のサンプルプログラムとその操作手順について紹介します。

事前準備

  1. FANUC 標準インターフェースの通信設定 を参照して標準インターフェースの通信設定が行われます。

  2. キャリブレーションの操作手順 を参照し、コントローラにインストールされたキャリブレーションプログラムを実行するか、手動でロボットを移動させてキャリブレーションポイントを追加します。この手順によって外部パラメータのキャリブレーションが行われます。

  3. 必要なMech-Vision、Mech-Vizのプロジェクトを作成し、現在のプログラムを自動的に読み込む にチェックを入れます。

  4. TCP座標を正しく設定します。

  5. ロボットが予期しない動作する可能性があるので、初めは速度を低く設定してください。

サンプルプログラムの紹介

簡単な把持・配置のサンプルプログラムは、Mech-Mindソフトウェアシステムのインストールパス Mech-Center/Robot_Interface/FANUC/sample フォルダに格納されています。

以下のサンプルプログラムを紹介します。

Mech-Visionから視覚結果を取得

 1:  !FUNCTION:Eye to Hand simple pick ;
 2:  !MechMind,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,50,51,52) ;
19:  IF (R[52]<>1100) THEN ;
20:  PAUSE ;
21:  ENDIF ;
22:  CALL MM_GET_POS(1,60,70,80) ;
23:L PR[60] 800mm/sec CNT100 Offset,PR[1]    ;
24:L PR[60] 800mm/sec FINE    ;
25:  !Add object grasping logic here ;
26:   ;
27:L PR[60] 800mm/sec CNT100 Offset,PR[1]    ;
28:  !Add transition point ;
29:L P[3] 800mm/sec FINE    ;
30:  !Move to DROP Position ;
31:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
32:L P[4] 200mm/sec FINE    ;
33:  !Add object releasing logic here ;
34:   ;
35:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
36:  !Move to HOME Position ;
37:J P[1] 100% FINE    ;

プログラムロジック

  1. ロボットを初期位置に移動させます。

  2. ロボットを画像撮影位置に移動させます。

  3. MM_INIT_SKTでソケット通信を初期化します。

  4. Mech-Visionのプロジェクトでパラメータレシピを使用する場合、使用するレシピはMM_SET_MODで設定することができます。

  5. MM_START_VISでMech-Visionプロジェクトに実行をトリガーします。

  6. 1秒待機します。Eye-In-Handでは、撮像が完了するまでロボットが完全に停止している必要があるため、WAIT命令が必要です。Eye-To-Handでは、MM_START_VISとMM_GET_VISの間に移動命令がある場合、このWAIT命令は必要ありません。

  7. Mech-Visionからの処理結果を取得します。

  8. 返されたMech-Visionのステータスコードを確認します。エラーコードが返された場合、プログラムは一時停止します。

  9. ロボットを把持位置に移動させ、把持を行います。

  10. ロボットを把持位置と配置位置間の経路に移動させます。

  11. 設定した配置位置にロボットを移動し、ワークを配置します。ロボットを初期位置に戻します。

カスタマイズ

  • 初期位置を定義

    J P[1] 100% FINE：レジスタ P[1]で初期位置を定義しています。

  • TCP座標を定義

    UTOOL_NUM=1：サンプルではUTOOL_NUM=1でTCP座標を定義しています。

  • 画像撮像位置を定義

    L P[2] 1000mm/sec FINE：レジスタ P[2]で画像撮影位置を定義しています。

  • 行動経路を定義

    L P[3] 800mm/sec FINE：把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。サンプルではレジスタ[3]に保存されています。途中経路は複数個設定することも可能です。

  • 配置位置を定義

    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:  !MechMind,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(1,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,50,51,52,53) ;
23:  IF (R[53]<>2100) THEN ;
24:  PAUSE ;
25:  ENDIF ;
26:  CALL MM_GET_POS(1,60,70,80) ;
27:  CALL MM_GET_POS(2,61,71,81) ;
28:  CALL MM_GET_POS(3,62,72,82) ;
29:  !follow the planned path to pick ;
30:L PR[60] R[80]mm/sec FINE    ;
31:L PR[61] R[81]mm/sec FINE    ;
32:  !Add object grasping logic here ;
33:   ;
34:L PR[62] R[82]mm/sec FINE    ;
35:  !Add transition point ;
36:L P[3] 800mm/sec FINE    ;
37:  !Move to DROP Position ;
38:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
39:L P[4] 200mm/sec FINE    ;
40:  !Add object releasing logic here ;
41:   ;
42:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
43:  !Move to HOME Position ;
44:J P[1] 100% FINE    ;

プログラムロジック

Mech-Vizが出力した位置姿勢に従い、設定した配置位置にロボットを移動し、ワークを配置します。

  1. ロボットを初期位置に移動させます。

  2. ロボットを画像撮影位置に移動させます。

  3. MM_INIT_SKTでソケット通信を初期化します。

  4. Mech-Visionのプロジェクトでパラメータレシピを使用する場合、使用するレシピはMM_SET_MODで設定することができます。詳細については 標準インターフェースコマンド をご参照ください。

  5. Mech-Vizプロジェクトの実行をトリガーします。

  6. Mech-Vizから経路計画を取得します。

  7. 返されたステータスコードを確認します。エラーコードが返された場合、プログラムは一時停止します。

  8. 取得した位置姿勢を、PR[60]、PR[61]およびPR[62] に書き込みます。

  9. 計画された経路に沿ってロボットを動かし、把持を実行します。

  10. ロボットを把持位置と配置位置間の経路に移動させます。

  11. 設定した配置位置にロボットを移動し、ワークを配置します。

カスタマイズ

  • 初期位置を定義

    J P[1] 100% FINE：レジスタ P[1]で初期位置を定義しています。

  • TCP座標を定義

    UTOOL_NUM=1：サンプルではUTOOL_NUM=1でTCP座標を定義しています。

  • 画像撮像位置を定義

    L P[2] 1000mm/sec FINE：レジスタ P[2]で画像撮影位置を定義しています。

  • 行動経路を定義

    L P[3] 800mm/sec FINE：把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。サンプルではレジスタ[3]に保存されています。途中経路は複数個設定することも可能です。

  • 配置位置を定義

    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:  !MechMind,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,1,2,10) ;
19:  WAIT   1.00(sec) ;
20:  CALL MM_GET_VISP(1,2,50,51,52,53) ;
21:  IF (R[53:status]<>1103) THEN ;
22:  PAUSE ;
23:  ENDIF ;
24:  FOR R[100:i]=1 TO R[51:pos_num] ;
25:  R[101:preg no]=59+R[100:i]    ;
26:  R[102:lbl no]=69+R[100:i]    ;
27:  R[103:spd no]=79+R[100:i]    ;
28:  CALL MM_GET_POS(R[100:i],R[101:preg no],R[102:lbl no],R[103:spd no]) ;
29:  ENDFOR ;
30:  !follow the planned path to pick ;
31:  FOR R[110:j]=1 TO R[51:pos_num] ;
32:  R[111:preg]=59+R[110:j]    ;
33:  R[112:spd]=79+R[110:j]    ;
34:L PR[R[111]] R[R[112]]mm/sec FINE    ;
35:  IF (R[110:j]=R[52:vpos_num]) THEN ;
36:  PAUSE ;
37:  !Add object grasping logic here ;
38:  ENDIF ;
39:  ENDFOR ;
40:  !Add transition point ;
41:L P[3] 800mm/sec FINE    ;
42:  !Move to DROP Position ;
43:L P[4] 800mm/sec CNT100 Offset,PR[2]    ;
44:L P[5] 200mm/sec FINE    ;
45:  !Add object releasing logic here ;
46:   ;
47:L P[6] 800mm/sec CNT100 Offset,PR[2]    ;
48:  !Move to HOME Position ;
49:J P[7] 100% FINE    ;

プログラムロジック

このサンプルは、Mech-Visionの「経路計画」ステップで計画した経路を取得して把持を行い、固定配置点に配置します。

  1. ロボットを初期位置に移動させます。

  2. ロボットを画像撮影位置に移動させます。

  3. MM_INIT_SKTでソケット通信を初期化します。

  4. Mech-Visionのプロジェクトでパラメータレシピを使用する場合、使用するレシピはMM_SET_MODで設定することができます。

  5. MM_START_VISでMech-Visionプロジェクトに実行をトリガーします。

  6. MM_GET_VISPでMech-Visionの「経路計画」ステップから出力された動作経路を取得します。

  7. 返されたステータスコードを確認します。エラーコードが返された場合、プログラムは一時停止します。

  8. 計画した経路点の数に従って、ループ文で取得した経路点の位置姿勢をPR[60]から順に位置レジスタに格納します。

  9. 移動コマンドを用いて計画した経路に沿ってロボットを移動させ、把持を完了したらビジョン領域から離れさせます。

  10. ロボットを把持位置と配置位置間の経路に移動させます。

  11. 設定した配置位置にロボットを移動し、ワークを配置します。

カスタマイズ

  • 初期位置を定義

    J P[1] 100% FINE：レジスタ P[1]で初期位置を定義しています。

  • TCP座標を定義

    UTOOL_NUM=1：サンプルではUTOOL_NUM=1でTCP座標を定義しています。

  • 画像撮像位置を定義

    L P[2] 1000mm/sec FINE：レジスタ P[2]で画像撮影位置を定義しています。

  • 行動経路を定義

    L P[3] 800mm/sec FINE：把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。サンプルではレジスタ[3]に保存されています。途中経路は複数個設定することも可能です。

  • 配置位置を定義

    L P[4] 200mm/sec FINE：レジスタ P[4]で配置位置を定義しています。

  • エンドエフェクタの制御ロジックを追加

    ワークを把持、配置する際の制御ロジックを追加することができます。