サンプルプログラム19：MM_S19_Vis_PlanAllVision

現在ご覧いただいているのは最新版の内容です（V2.1.2）。異なるバージョンを参照する場合は、画面右上のボタンから切り替えが可能です。

■ ご利用中のバージョンが分からない場合や、サポートが必要な場合はお気軽にサポート窓口までご連絡ください。

プログラム概要

機能

ロボットはMech-Visionプロジェクトをトリガーして実行し、その後ループですべての計画された経路を取得して、把持および配置を実行します。このサンプルでは、カメラで1回撮影することで、Mech-Visionがすべてのビジョン結果に基づいた把持経路を計画します。通常は「1回の撮影で複数回の把持を行う」シーンで使用されます。

ファイル場所

Mech-VisionとMech-Vizソフトウェアのインストールディレクトリにある Communication Component/Robot_Interface/FANUC/sample/MM_S19_Vis_PlanAllVision。

必要なプロジェクト

Mech-Visionプロジェクト

経路計画設定ツール の ワークフロー 内にある 全体的設定 ステップで、全部のビジョン処理の結果を計画 パラメータが有効になっている必要があります。
sample19 1

使用前提

  1. 標準インターフェース通信設定 が完了しました。

  2. 自動キャリブレーション が完了しました。
このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。

プログラム説明

以下はMM_S19_Vis_PlanAllVisionサンプルプログラムのコードと関連する説明です。

このサンプルは、MM_S3_Vis_Pathサンプルに比べて、すべての計画された経路をループで取得し、把持および配置を行う機能（太字部分のコード）のみが追加されています。そのため、MM_S3_Vis_Pathサンプルと同じ部分のコードについては、以下で再度説明することはありません（詳細は MM_S3_Vis_Pathサンプルの説明 をご参照ください）。 
   1:  !-------------------------------- ;
   2:  !FUNCTION: trigger Mech-Vision ;
   3:  !project, plan all vision results ;
   4:  !and get all planned paths ;
   5:  !Mech-Mind, 2023-12-25 ;
   6:  !-------------------------------- ;
   7:   ;
   8:  !set current uframe NO. to 0 ;
   9:  ﾕｰｻﾞｻﾞﾋｮｳﾊﾞﾝｺﾞｳ=0 ;
  10:  !set current tool NO. to 1 ;
  11:  ﾂｰﾙｻﾞﾋｮｳﾊﾞﾝｺﾞｳ=1 ;
  12:  !move to robot home position ;
  13:ｶｸｼﾞｸ ｲﾁ[1] 100% ｲﾁｷﾞﾒ    ;
  14:  !initialize communication ;
  15:  !parameters(initialization is ;
  16:  !required only once) ;
  17:  ﾖﾋﾞﾀﾞｼ MM_INIT_SKT('8','127.0.0.1',50000,5) ;
  18:  ﾗﾍﾞﾙ[1:recap] ;
  19:  !move to image-capturing position ;
  20:ﾁｮｸｾﾝ ｲﾁ[2] 1000mm/sec ｲﾁｷﾞﾒ    ;
  21:  !trigger NO.1 Mech-Vision project ;
  22:  ﾖﾋﾞﾀﾞｼ MM_START_VIS(1,0,2,10) ;
  23:  !get planned path from NO.1 ;
  24:  !Mech-Vision project; 2nd ;
  25:  !argument (1) means getting pose ;
  26:  !in JPs ;
  27:  ﾖﾋﾞﾀﾞｼ MM_GET_VISP(1,1,51,52,53) ;
  28:  !check whether planned path has ;
  29:  !been got from Mech-Vision ;
  30:  !successfully ;
  31:  ﾓｼ ﾚｼﾞ[53]<>1103,ｼﾞｬﾝﾌﾟ ﾗﾍﾞﾙ[99] ;
  32:  !save all waypoint data to local ;
  33:  !variables using for-loop, a ;
  34:  !maximum of 50 points are support ;
  35:  !supported ;
  36:  FOR ﾚｼﾞ[10]=1 TO ﾚｼﾞ[51] ;
  37:  ﾚｼﾞ[11]=59+ﾚｼﾞ[10]    ;
  38:  ﾚｼﾞ[12]=69+ﾚｼﾞ[10]    ;
  39:  ﾚｼﾞ[13]=99+ﾚｼﾞ[10]    ;
  40:  ﾖﾋﾞﾀﾞｼ MM_GET_JPS(ﾚｼﾞ[10],ﾚｼﾞ[11],ﾚｼﾞ[12],ﾚｼﾞ[13]) ;
  41:  ENDFOR ;
  42:  !parse pick cycle count, here ;
  43:  !suppose 5 points per planned ;
  44:  !path ;
  45:  ﾚｼﾞ[30]=ﾚｼﾞ[51] DIV 5    ;
  46:  ﾚｼﾞ[29]=ﾚｼﾞ[51] MOD 5    ;
  47:  !check if parsed data is valid; ;
  48:  !if not, retry to get planned ;
  49:  !path or add some error handling ;
  50:  !logic ;
  51:  IF ﾚｼﾞ[30]<1) ﾏﾀﾊ (ﾚｼﾞ[29]<>0 THEN ;
  52:  ｲﾁｼﾞﾃｲｼ ;
  53:  ｼﾞｬﾝﾌﾟ ﾗﾍﾞﾙ[1] ;
  54:  ENDIF ;
  55:  !repeatedly run pick-and-place ;
  56:  !cycle using for-loop ;
  57:  FOR ﾚｼﾞ[10]=1 TO ﾚｼﾞ[30] ;
  58:  ﾚｼﾞ[20]=ﾚｼﾞ[10]-1    ;
  59:  ﾚｼﾞ[21]=ﾚｼﾞ[20]*5    ;
  60:  ﾚｼﾞ[31]=60+ﾚｼﾞ[21]    ;
  61:  ﾚｼﾞ[32]=61+ﾚｼﾞ[21]    ;
  62:  ﾚｼﾞ[33]=62+ﾚｼﾞ[21]    ;
  63:  ﾚｼﾞ[34]=63+ﾚｼﾞ[21]    ;
  64:  ﾚｼﾞ[35]=64+ﾚｼﾞ[21]    ;
  65:  !follow the planned path to pick ;
  66:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[31]] 50% ﾅﾒﾗｶ100    ;
  67:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[32]] 50% ｲﾁｷﾞﾒ    ;
  68:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[33]] 10% ｲﾁｷﾞﾒ    ;
  69:  !add object grasping logic here, ;
  70:  !such as "DO[1]=ON" ;
  71:  ｲﾁｼﾞﾃｲｼ ;
  72:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[34]] 50% ｲﾁｷﾞﾒ    ;
  73:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[35]] 50% ﾅﾒﾗｶ100    ;
  74:  !move to intermediate waypoint ;
  75:  !of placing ;
  76:ｶｸｼﾞｸ ｲﾁ[3] 50% ﾅﾒﾗｶ100    ;
  77:  !move to approach waypoint ;
  78:  !of placing ;
  79:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]    ;
  80:  !move to placing waypoint ;
  81:ﾁｮｸｾﾝ ｲﾁ[4] 300mm/sec ｲﾁｷﾞﾒ    ;
  82:  !add object releasing logic here, ;
  83:  !such as "DO[1]=OFF" ;
  84:  ｲﾁｼﾞﾃｲｼ ;
  85:  !move to departure waypoint ;
  86:  !of placing ;
  87:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]    ;
  88:  !move to intermediate waypoint ;
  89:  !of placing ;
  90:ｶｸｼﾞｸ ｲﾁ[3] 50% ﾅﾒﾗｶ100    ;
  91:  ENDFOR ;
  92:  !finish pick-and-place cycle, and ;
  93:  !jump back to camera capturing ;
  94:  ｼﾞｬﾝﾌﾟ ﾗﾍﾞﾙ[1] ;
  95:  ｼｭｳﾘｮｳ ;
  96:   ;
  97:  ﾗﾍﾞﾙ[99:vision error] ;
  98:  !add error handling logic here ;
  99:  !according to different ;
 100:  !error codes ;
 101:  !e.g.: status=1003 means no ;
 102:  !point cloud in ROI ;
 103:  !e.g.: status=1002 means no ;
 104:  !vision results ;
 105:  ｲﾁｼﾞﾃｲｼ ;

上記のサンプルプログラムの処理流れは、下図の通りです。

sample19

下表は追加されたコードとその説明です。コマンド名のリンクをクリックすることで、その詳細な説明を確認できます。

処理流れ コートと説明

ループで計画された経路を保存

 
  32:  !save all waypoint data to local ;
  33:  !variables using for-loop, a ;
  34:  !maximum of 50 points are support ;
  35:  !supported ;
  36:  FOR ﾚｼﾞ[10]=1 TO ﾚｼﾞ[51] ;
  37:  ﾚｼﾞ[11]=59+ﾚｼﾞ[10]    ;
  38:  ﾚｼﾞ[12]=69+ﾚｼﾞ[10]    ;
  39:  ﾚｼﾞ[13]=99+ﾚｼﾞ[10]    ;
  40:  ﾖﾋﾞﾀﾞｼ MM_GET_JPS(ﾚｼﾞ[10],ﾚｼﾞ[11],ﾚｼﾞ[12],ﾚｼﾞ[13]) ;
  41:  ENDFOR ;

  • 36行目：FORは以下のループプログラム部分を示します。ﾚｼﾞ[10]はループの回数を制御します（ﾚｼﾞ[10]は1から始まり、毎回ループ後に1増加し、ﾚｼﾞ[51]より大きくなるとループが終了します）。ﾚｼﾞ[51]は MM_GET_VISP コマンドにおける3番目のパラメータが表す数値レジスタで、このレジスタはビジョンシステムが返す経路点の数を保存します。したがって、ﾚｼﾞ[10]は計画された経路における現在の経路点の番号を示します。

  • 37行目～39行目：ﾚｼﾞ[11]、ﾚｼﾞ[12]、ﾚｼﾞ[13]は、それぞれ現在の経路点に対応する関節角度、ラベル、ロボットハンド番号に使用されるレジスタ番号を示します。

  • 40行目：MM_GET_JPS コマンドは、ある経路点の関節角度、ラベル、およびロボットハンド番号を指定されたレジスタに保存します。したがって、このコマンドはﾚｼﾞ[10]番目の経路点の関節角度、ラベル、ロボットハンド番号をそれぞれﾚｼﾞ[11]、ﾚｼﾞ[12]、ﾚｼﾞ[13]に保存することを意味します。

ﾚｼﾞ[30]とﾚｼﾞ[29]を計算

 
  42:  !parse pick cycle count, here ;
  43:  !suppose 5 points per planned ;
  44:  !path ;
  45:  ﾚｼﾞ[30]=ﾚｼﾞ[51] DIV 5    ;
  46:  ﾚｼﾞ[29]=ﾚｼﾞ[51] MOD 5    ;

このサンプルでは、各計画された把持経路が5つの経路点を含むことを仮定としています。「ﾚｼﾞ[51] DIV 5」はﾚｼﾞ[51]を5で割った商、「ﾚｼﾞ[51] MOD 5」はﾚｼﾞ[51]を5で割った余りを表します。ﾚｼﾞ[30]は計画された把持回数の合計を示します。ﾚｼﾞ[29]が0でない場合、ある経路計画において把持経路点の数が5未満であることを意味し、経路計画に異常が発生したため再計画が必要です。

経路計画が異常かどうかを判断

 
  47:  !check if parsed data is valid; ;
  48:  !if not, retry to get planned ;
  49:  !path or add some error handling ;
  50:  !logic ;
  51:  IF ((ﾚｼﾞ[30]<1) ﾏﾀﾊ (ﾚｼﾞ[29]<>0)) THEN ;
  52:  ｲﾁｼﾞﾃｲｼ ;
  53:  ｼﾞｬﾝﾌﾟ ﾗﾍﾞﾙ[1] ;
  54:  ENDIF ;

把持回数ﾚｼﾞ[30]が1未満である場合、またはﾚｼﾞ[29]が0でない場合、経路計画に異常が発生していることを示します。この場合、処理ロジックを追加する必要があります。例えば、Mech-Visionプロジェクトを再度トリガーして実行し、計画された経路を再取得するなどの対応が必要です。

ループで把持および配置を実行

 
  55:  !repeatedly run pick-and-place ;
  56:  !cycle using for-loop ;
  57:  FOR ﾚｼﾞ[10]=1 TO ﾚｼﾞ[30] ;
  58:  ﾚｼﾞ[20]=ﾚｼﾞ[10]-1    ;
  59:  ﾚｼﾞ[21]=ﾚｼﾞ[20]*5    ;
  60:  ﾚｼﾞ[31]=60+ﾚｼﾞ[21]    ;
  61:  ﾚｼﾞ[32]=61+ﾚｼﾞ[21]    ;
  62:  ﾚｼﾞ[33]=62+ﾚｼﾞ[21]    ;
  63:  ﾚｼﾞ[34]=63+ﾚｼﾞ[21]    ;
  64:  ﾚｼﾞ[35]=64+ﾚｼﾞ[21]    ;
  65:  !follow the planned path to pick ;
  66:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[31]] 50% ﾅﾒﾗｶ100    ;
  67:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[32]] 50% ｲﾁｷﾞﾒ    ;
  68:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[33]] 10% ｲﾁｷﾞﾒ    ;
  69:  !add object grasping logic here, ;
  70:  !such as "DO[1]=ON" ;
  71:  ｲﾁｼﾞﾃｲｼ ;
  72:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[34]] 50% ｲﾁｷﾞﾒ    ;
  73:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[ﾚｼﾞ[35]] 50% ﾅﾒﾗｶ100    ;
  74:  !move to intermediate waypoint ;
  75:  !of placing ;
  76:ｶｸｼﾞｸ ｲﾁ[3] 50% ﾅﾒﾗｶ100    ;
  77:  !move to approach waypoint ;
  78:  !of placing ;
  79:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]    ;
  80:  !move to placing waypoint ;
  81:ﾁｮｸｾﾝ ｲﾁ[4] 300mm/sec ｲﾁｷﾞﾒ    ;
  82:  !add object releasing logic here, ;
  83:  !such as "DO[1]=OFF" ;
  84:  ｲﾁｼﾞﾃｲｼ ;
  85:  !move to departure waypoint ;
  86:  !of placing ;
  87:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]    ;
  88:  !move to intermediate waypoint ;
  89:  !of placing ;
  90:ｶｸｼﾞｸ ｲﾁ[3] 50% ﾅﾒﾗｶ100    ;
  91:  ENDFOR ;

上記のコードは、FORループ内でロボットが各計画された経路における5つの経路点に移動し、把持を完了した後に配置を続けて実行することを意味します。ﾚｼﾞ[10]はループ回数を制御するために使用されます。ﾚｼﾞ[10]は1から始まり、各ループ後に1ずつ増加します。ﾚｼﾞ[10]が把持回数ﾚｼﾞ[30]を超えるとループは終了します。ﾚｼﾞ[10]が1増加するごとに、ﾚｼﾞ[21]も5ずつ増加します。「60+ﾚｼﾞ[21]～64+ﾚｼﾞ[21]」（ﾚｼﾞ[31]からﾚｼﾞ[35]）は、各経路計画における5つの経路点が格納される5つのレジスタ番号を示します。

この情報は役に立ちましたか？

ご意見・ご要望がございましたら、以下よりお寄せください：

Mech-Mindは、お客様のプライバシーを重視しています

このサイトでは最高の体験を提供するために Cookie を使用しています。サイトの閲覧を続ける場合、Cookie の使用に同意したことになります。「拒否する」を選択すると、このサイトを訪れた際に追跡や記憶が行われないように単独の Cookie が使用されます。

  • Preference
  • 同意する
  • 拒否する