サンプルプログラム

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

事前準備

1. KUKA 標準インターフェースの通信設定 に従って標準インターフェースの通信設定が行われます。

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

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

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

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

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

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

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

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

• Mech-Vizから経路計画を取得

• Mech-Visionの「経路計画」ステップを使用して経路を取得

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

&ACCESS RVO
&REL 5
DEF MM_SAMPLE01 ( )
  ;FUNCTION:Eye to Hand simple pick and place with Mech-Vision
  ;mechmind, 2022-5-31
  INT Job
  INT Pos_Num
  INT Last_Data
  INT MM_Status
  E6POS MM_pick
  E6POS MM_waypoint
  E6POS MM_camera_capture
  E6POS MM_drop
  INT MM_Label
  INT MM_Speed
  ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD

  BAS(#TOOL,1)
  LIN MM_camera_capture
  ;Set ip address of IPC
  MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
  wait sec 0.1
  ;Set vision recipe
  MM_Switch_Model(1,1)
  ;Run vision project
  MM_Start_Vis(1,1,2,snap_jps)
  wait sec 1
  MM_Get_VisData(1,Last_Data,Pos_Num,MM_Status)
  IF MM_Status<> 1100 THEN
    halt
  ENDIF
  MM_Get_Pose(1,MM_pick,MM_Label,MM_Speed )
MM_pick.z=MM_pick.z+100
LIN MM_pick
MM_pick.z=MM_pick.z-100
  LIN MM_pick
  ;Add object grasping logic here.
  LIN_REL {z -100}#TOOL
 LIN MM_waypoint
MM_drop.z=MM_drop.z+100
LIN MM_drop
MM_drop.z=MM_drop.z-100
  LIN MM_drop
  ;Add object releasing logic here.
  LIN_REL {z -100}#TOOL
    ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD
END

プログラムロジック

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

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

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

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

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

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

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

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

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

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

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

カスタマイズ

• TCP座標を定義

  BAS(#TOOL, 1)を実際のTCPを保存するツール座標系に変更します。

• 画像撮影姿勢を定義

  MM_camera_captureで画像撮影位置を定義しています。

• 行動経路を定義

  把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。MM_waypointに一個または複数個設定することが可能です。

• 配置位置を定義

  MM_dropで配置位置を定義しています。

• 把持、配置位置からのZ方向オフセットを定義

  • 把持位置に近づけます

    MM_pick.z=MM_pick.z+100

    LIN MM_pick

    把持位置に接近する際に100mmだけZ方向にオフセットされます。

  • 把持位置から離れます

    LIN_REL {z -100}#TOOL

    把持位置から離れる際に100mmだけZ方向にオフセットされます。

  • 配置位置に近づけます

    MM_drop.z=MM_drop.z+100

    LIN MM_drop

    配置位置に接近する際に100mmだけZ方向にオフセットされます。

  • 配置位置から離れます

    LIN_REL {z -100}#TOOL

    配置位置から離れる際に100mmだけZ方向にオフセットされます。

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

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

• 初期位置を定義

  ロボットの初期位置姿勢は、事前に設定する必要があります。

Mech-Vizから経路計画を取得

&ACCESS RVO
&REL 2
DEF MM_SAMPLE02 ( )
INT Job
  INT Pos_Num
  INT VisPos_Num
  INT Last_Data
  INT MM_Status
  DECL E6POS MM_movepoint[20]
  E6POS MM_waypoint
  E6POS MM_camera_capture
  E6POS MM_drop
  DECL INT MM_Label[20]
  INT MM_Speed[20]
  INT count
  ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD

  BAS(#TOOL,1)
  LIN MM_camera_capture
  ;Set ip address of IPC
  MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
  wait sec 0.1
  ;Set vision recipe
  ;MM_Switch_Model(1,1)
  ;Run Viz project
  MM_Start_Viz(1,snap_jps)
  wait sec 0.1
  ;set branch exitport
  ;MM_Set_Branch(1,1)
  ;get planned path
  MM_Get_VizData(2,Last_Data,Pos_Num,VisPos_Num,MM_Status)
  IF MM_Status<> 2100 THEN
    halt
  ENDIF
  FOR count=1 TO Pos_Num
  MM_Get_Pose(count,MM_movepoint[count],MM_Label[count],MM_Speed[count])
  ENDFOR
  ;follow the planned path to pick
  FOR count=1 TO Pos_Num
  LIN MM_movepoint[count]
  IF count==VisPos_Num THEN
   ;add object grasping logic here
  ENDIF
  ENDFOR
  ;go to drop location
  LIN MM_waypoint
MM_drop.z=MM_drop.z+100
  LIN MM_drop
MM_drop.z=MM_drop.z-100
  LIN MM_drop
  ;Add object releasing logic here.
  LIN_REL  {z -100}#TOOL
    ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD
END

プログラムロジック

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

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

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

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

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

5. MM_START_VIZを実行してMech-Vizプロジェクトを実行します。

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

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

8. FORループを利用して、取得した移動位置をMM_movepoint[]に格納します。

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

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

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

カスタマイズ

• TCP座標を定義

  BAS(#TOOL, 1)を実際のTCPを保存するツール座標系に変更します。

• 画像撮影姿勢を定義

  MM_camera_captureで画像撮影位置を定義しています。

• 行動経路を定義

  把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。MM_waypointに一個または複数個設定することが可能です。

• 配置位置を定義

  MM_dropで配置位置を定義しています。

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

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

• 初期位置を定義

  ロボットの初期位置姿勢は、事前に設定する必要があります。

Mech-Visionの「経路計画」ステップを使用して経路を取得

&A&ACCESS RVO
&REL 2
DEF MM_SAMPLE03 ( )
  ;FUNCTION:Eye to Hand simple pick and place with Mech-Vision Path Planning Step
  ;mechmind, 2023-1-10
  INT Pos_Num
  INT VisPos_Num
  INT Last_Data
  INT MM_Status
  DECL E6POS MM_movepoint[20]
  E6POS MM_waypoint
  E6POS MM_camera_capture
  E6POS MM_drop
  INT MM_Label[20]
  INT MM_Speed[20]
  INT count
  ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD

  BAS(#TOOL,1)
  LIN MM_camera_capture
  ;Set ip address of IPC
  MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
  wait sec 0.1
  ;Set vision recipe
  MM_Switch_Model(1,1)
  ;Run vision project
  MM_Start_Vis(1,1,2,snap_jps)
  wait sec 1
  ;get planned path
  MM_Get_VisPath(1,2,Last_Data,Pos_Num,VisPos_Num,MM_Status)
  IF MM_Status<> 1103 THEN
    halt
  ENDIF
  FOR count=1 TO Pos_Num
  MM_Get_Pose(count,MM_movepoint[count],MM_Label[count],MM_Speed[count])
  ENDFOR
  ;follow the planned path to pick
  FOR count=1 TO Pos_Num
  LIN MM_movepoint[count]
  IF count==VisPos_Num THEN
   ;add object grasping logic here
  ENDIF
  ENDFOR
  ;go to drop location
  LIN MM_waypoint
MM_drop.z=MM_drop.z+100
  LIN MM_drop
MM_drop.z=MM_drop.z-100
  LIN MM_drop
  ;Add object releasing logic here.
  LIN_REL  {z -100}#TOOL
    ;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
  ;ENDFOLD
END

プログラムロジック

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

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

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

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

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

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

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

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

8. 計画さ経路点の数に応じて、ループ文で取得した経路点を変数に保存します。

9. ループ文で計画された経路に沿ってロボットを動作し、把持を完了させ、ビジョン領域から離れさせます。

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

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

カスタマイズ

• TCP座標を定義

  BAS(#TOOL, 1)を実際のTCPを保存するツール座標系に変更します。

• 画像撮影姿勢を定義

  MM_camera_captureで画像撮影位置を定義しています。

• 行動経路を定義

  把持位置から配置位置までの経路を設定します。この経路はロボットが周辺環境と衝突しないように設定する必要があります。MM_waypointに一個または複数個設定することが可能です。

• 配置位置を定義

  MM_dropで配置位置を定義しています。

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

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

• 初期位置を定義

  ロボットの初期位置姿勢は、事前に設定する必要があります。