サンプルプログラム20:MM_S20_Viz_PlanAllVision

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

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

プログラム概要

機能

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

ファイル場所

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

必要なプロジェクト

Mech-VisionとMech-Vizプロジェクト

ビジョン処理による移動 ステップの ビジョン処理の結果を再使用 パラメータにチェックが入っている必要があります。
sample20 1

使用前提

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

  2. 自動キャリブレーション が完了しました。

このサンプルプログラムは参考用です。ユーザーは実際の状況に応じて、このプログラムを基に変更を加える必要があります。このプログラムをそのまま使用しないでください。

プログラム説明

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

このサンプルは、MM_S2_Viz_Basicサンプルに比べて、すべての計画された経路をループで取得し、把持および配置を行う機能(太字部分のコード)のみが追加されています。そのため、MM_S2_Viz_Basicサンプルと同じ部分のコードについては、以下で再度説明することはありません(詳細は MM_S2_Viz_Basicサンプルの説明 をご参照ください)。
DEF  MM_S20_Viz_PlanAllVision ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Viz project, plan all
; vision results and get all planned results using
; command 210
; Mech-Mind, 2023-12-25
;---------------------------------------------------
   ;set current tool no. to 1
   BAS(#TOOL,1)
   ;set current base no. to 0
   BAS(#BASE,0)
   ;move to robot home position
PTP HOME Vel=100 % DEFAULT
   ;initialize communication parameters (initialization is required only once)
   MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
RECAP:
   ;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
   ;trigger Mech-Viz project
   MM_Start_Viz(2,init_jps)
   ;get planned path
   MM_Get_PlanData(0,3,pos_num,vis_pos_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
      halt
   ENDIF
   ;save waypoints of the planned path to local variables one by one
   FOR count=1 TO pos_num
      MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count])
   ENDFOR
   ;parse pick cycle count, here suppose 3 points per planned path
   pick_cnt = pos_num / 3
   residual = pos_num - pick_cnt*3
   ;check if parsed data is valid; if not, retry to get planned path or add some error handling logic
   IF (pick_cnt<1) OR (residual<>0) THEN
      halt
      GOTO RECAP
   ENDIF
   ;repeatedly run pick-and-place cycle using for-loop
   FOR i=1 TO pick_cnt
      count=(i-1)*3
      Xpick_point1=pick_point[1+count]
      Xpick_point2=pick_point[2+count]
      Xpick_point3=pick_point[3+count]
      ;move to intermediate waypoint of picking
PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0]
      ;follow the planned path to pick
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
      ;add object grasping logic here, such as "$OUT[1]=TRUE"
      halt
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
      ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
      ;add object releasing logic here, such as "$OUT[1]=FALSE"
      halt
      ;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   ENDFOR
   ;finish pick and-place cycle, and jump back to camera capturing
   GOTO RECAP
END

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

sample20

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

処理流れ コートと説明

計画された経路を取得

;get planned path
MM_Get_PlanData(0,3,pos_num,vis_pos_num,status)
  • MM_Get_PlanData:計画された経路を取得するためのコマンド。このコマンドを使用して取得した「ビジョン処理による移動」の経路点は、位置姿勢に加えて、「ビジョン処理による移動」の計画結果やカスタムデータ(ある場合)も含まれます。一方、MM_Get_VizData コマンドを使用して取得した「ビジョン処理による移動」の経路点には、「ビジョン処理による移動」の計画結果やカスタムデータは含まれていません。

  • 0:Mech-Vizから計画された経路を取得します。

  • 3:返されるデータ形式。「位置姿勢(関節角度形式)、移動方式、ロボットハンド番号、速度、Mech-Vizの「ビジョン処理による移動」の計画結果、カスタムデータ項目1、…、カスタムデータ項目N」といった形式でデータを返します。

  • pose_num:この変数はビジョンシステムから返された経路点の数を保存します。

  • vis_pose_num:この変数は、「ビジョン処理による移動」の最後の経路点(把持点)が経路全体における位置番号を保存します。

  • status:この変数はコマンド実行のステータスコードを保存します。

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

FOR count=1 TO pos_num
MM_Get_PlanJps(count,3,pick_point[count],move_type[count],tool_num[count],speed[count])
ENDFOR
  • 1行目:FORは以下がループプログラム部分を示します。countはループの回数を制御します(countは1から始まり、毎回ループ後に1増加し、pose_numより大きくなるとループが終了します)。pose_numは MM_Get_PlanData コマンドにおける3番目のパラメータで、このパラメータはビジョンシステムが返す経路点の数を示します。

  • 2行目:MM_Get_PlanJps コマンドは、ある経路点の関節角度、移動方式、ロボットハンド番号、速度をそれぞれ指定された変数に保存することを意味します。つまり、このコマンドはcount番目の経路点の関節角度、移動方式、ロボットハンド番号、速度をそれぞれpick_point[count]、move_type[count]、tool_num[count]、speed[count]に保存することを意味します。

  • 3行目:FORループを終了します。

pick_cntとresidualを計算

;parse pick cycle count, here suppose 3 points per planned path
pick_cnt = pos_num / 3
residual = pos_num - pick_cnt*3

本サンプルでは、計画された経路には3つの経路点が含まれると仮定しています。「pick_cnt = pos_num / 3」はpose_numを3で割った商、「pos_num - pick_cnt*3」はpose_numを3で割った余りを表します。pick_cntは計画された把持回数を表します。residualが0でない場合、ある経路計画において把持経路点の数が3未満であることを意味し、経路計画に異常が発生したため再計画が必要です。

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

IF (pick_cnt<1) OR (residual<>0) THEN
   halt
   GOTO RECAP
ENDIF

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

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

   FOR i=1 TO pick_cnt
      count=(i-1)*3
      Xpick_point1=pick_point[1+count]
      Xpick_point2=pick_point[2+count]
      Xpick_point3=pick_point[3+count]
      ;move to intermediate waypoint of picking
PTP pick_waypoint CONT Vel=50 % PDAT7 Tool[1] Base[0]
      ;follow the planned path to pick
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
      ;add object grasping logic here, such as "$OUT[1]=TRUE"
      halt
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
      ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
      ;add object releasing logic here, such as "$OUT[1]=FALSE"
      halt
      ;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
   ENDFOR

上記のコードは、FORループ内でロボットが各計画された経路における3つの経路点に移動し、把持を完了した後に配置を続けて実行することを意味します。iはループ回数を制御するために使用されます。iは1から始まり、各ループ後に1ずつ増加します。iが把持回数pick_cntを超えるとループは終了します。iが1増加すると、countが3増加します。そうすることで、[1+count]~[3+count]が計画された3つの経路点が経路全体における番号を表すようになります。

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

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

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

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