フォース機能のプログラム例

フォース機能を組み合わせた、簡単な作業例を説明します。

注意

例で説明するパラメーターは参考値です。

比較的安定的なパラメーターを使用していますが、動作環境によっては作業が成功しない場合や、振動的な動作をする場合があり、パラメーターの調整が必要な場合があります。注意してください。

また、説明のため、低速で安定的なパラメーターを使用しています。より高速な動作をする場合も、パラメーターの調整が必要になります。

押し付け作業

一定の方向に目標の力で押し付ける作業のプログラム例を説明します。

作業例は、接触位置から10[mm]上空へ移動し、力制御機能によってハンドで把持しているワークを作業台に20[N]の力で押しあてます。

同時に、フォーストリガー機能を使って、作業中に過大な力(100[N]以上)を監視し、過大な力を検知したときエラーとします。

また、フォース動作制限機能を使って、作業中に想定外の移動 (20[mm]以上)を行った場合、作業台が存在しない異常状態として、ロボットの動作を停止します。

また、フォースモニター機能によって、作業完了後の力覚センサー値と作業中にかかった最大の力を計測します。
ツール座標系は、ハンド先端に設定され、ハンド進行方向がTlz軸方向です

サンプルプログラム

Function PressSample_Main
  Real rVar(8)
  Integer iVar
  Motor On
  Go P0                                 ' 作業開始位置へ移動
  PressSample_PropertySetting           ' プロパティー設定
  FSet FS1.Reset                        ' 力覚センサーリセット
  Trap 1, FT1 Call PressSample_EHandle  ' 過大な力の監視開始
  FSet FM1.PeakForceClear, True, True, True, False, False, False, True, False
    ' ピーク値計算開始
  Till FMR1                             ' 想定範囲外に移動した場合の動作停止条件を設定
  FCKeep FC1 Till, 10                   ' 力制御機能を10秒間実行
  Print "Motion End"
  FGet FM1.Forces, rVar()               ' 力覚センサー値取得
  Print "Force Fz:", rVar(FG_FZ), ", Fmag:", rVar(FG_FMAG)
  FGet FM1.PeakForces, rVar()           ' ピーク値取得
  Print "PeakForce Fz:", rVar(FG_FZ), ", Fmag:", rVar(FG_FMAG)
  FGet FMR1.Triggered, iVar             '動作制限の結果取得
  If iVar = True Then                   '制限されていた場合はOverrunエラーを表示
    Print "Overrun Error"
  EndIf
Fend

Function PressSample_PropertySetting
  FSet FCS1.Position, 0, 0, 30          ' フォース座標系の原点はZ30 mm位置
  FSet FCS1.Orientation, FG_TOOL        ' 方向はツール座標系と一致する

  FSet FC1.CoordinateSystem, FCS1       ' 定義したフォース座標1番を指定
  FSet FC1.Enabled, False, False, True, False, False, False
    ' Fz方向のみ力制御機能を有効
  FSet FC1.Fz_TargetForce, -20          ' 20Nの押し付け
  FSet FC1.Fz_Spring, 0                 ' 仮想弾性係数は0
  FSet FC1.Fz_Damper, 10                ' 仮想粘性係数は10
  FSet FC1.Fz_Mass, 10                  ' 仮想慣性係数は10

  FSet FT1.ForceSensor, 1               ' 力覚センサー1番を指定
  FSet FT1.CoordinateSystem, FCS1       ' 定義したフォース座標1番を指定
  FSet FT1.TriggerMode, FG_FORCE        ' 力を監視する
  FSet FT1.Fmag_Axes, FG_XYZ
  FSet FT1.Enabled, False, False, False, False, False, False, True, False
    ' Fmagのみを有効
  FSet FT1.Fmag_Polarity, FG_OUT        ' 閾値範囲外になったときトリガー検知
  FSet FT1.Fmag_Levels, 0, 100          ' Fmagの範囲は0~100

  FSet FM1.ForceSensor, 1               ' 力覚センサー1番を指定
  FSet FM1.CoordinateSystem, FCS1       ' 定義したフォース座標1番を指定

  FSet FMR1.TriggerMode, FG_REL_TOOL    ' ツール相対モードを指定
  FSet FMR1.PosEnabled, False, False, True, False, False
    ' Z方向のみを有効
  FSet FMR1.PosZ_Levels, -20, 20        ' Zの範囲は-20mm~20mm
  FSet FMR1.PosZ_Polarity, FG_OUT       ' 閾値範囲外になったとき制限有効
  FSet FMR1.ForceSensor, 1              ' 力覚センサー1番を指定
  FSet FMR1.CoordinateSystem, FCS1      ' 定義したフォース座標1番を指定
Fend

Function PressSample_EHandle
  Real rVar(8)
  FGet FM1.PeakForces, rVar()           ' ピーク値取得
  Print "Error Handle"
  Print "PeakForce Fz:", rVar(FG_FZ), ", Fmag:", rVar(FG_FMAG)
  AbortMotion All                       ' ロボット動作を停止し、エラー状態にする
Fend

解説

PressSample_Main関数を実行すると、ロボットは作業開始位置へ移動します。
PressSample_PropertySettingを呼び出し、各プロパティーの設定を実行します。
ただし、各プロパティーの設定はGUIのフォースエディターで事前に行うこともできます。その場合、PressSample_PropertySettingを呼び出す必要はありません。
フォース座標オブジェクトを設定します。フォース座標系は、ツール座標系におけるワーク端の位置を指定するため、例では、Z30[mm]位置を指定します。方向はツール座標系の方向と同じとします。
フォースコントロールオブジェクトを設定します。力制御機能を実行する座標系として設定したFCS1を指定します。Fzの正方向に向かって押し付けるため、目標力は負の値を指定します。仮想弾性係数, 仮想粘性係数, 仮想慣性係数をそれぞれ設定します。この例では、仮想弾性係数を“0”に設定したため、ロボットは仮想的な“ばね”を持たず、目標力を達成するまで移動し続けます。また、仮想粘性係数と仮想慣性係数は、安定的なパラメーターを使用しています。高速な動作をさせるためにはこれらを段階的に小さくしながら調整します。ただし、小さくなるにしたがい、力のオーバーシュートは大きくなります。
フォーストリガーオブジェクトを設定します。使用する力覚センサー番号とフォーストリガー機能を実行する座標系を指定します。過大な力がかかるかを監視するため、TriggerModeプロパティーは力を指定します。監視する合成力はFx~Fz全てを使って計算するためXYZを指定します。過大な力を100[N]とするため、Fmagの範囲として0~100[N]を指定し、この範囲外になるかを監視するように設定します。
フォースモニターオブジェクトを設定します。計測する力覚センサー番号と座標系を指定します。
フォース動作制限オブジェクトを設定します。ロボットは、(a), (b)の設定により、ツール座標系の+Z方向に移動します。例では、20㎜を超えて移動した場合、押し付ける対象物がないとして検出する設定を行います。ツール座標系の移動距離を検出するため、TriggerModeにツール相対モードを指定します。Z方向を検出するため、EnabledでPosZのみを有効にします。Z方向の範囲として-20~20[mm]を指定し、この範囲外になるかを監視するように設定します。
フォース機能を使用する前に力覚センサーをリセットします。
Trapにフォーストリガーオブジェクトを指定し、フォーストリガー機能を実行します。これにより過大な力を監視します。
Tillの条件にフォース動作制限オブジェクトを指定します。
作業中にかかる力のピーク値の計算を開始します。
力制御機能を10秒間実行します。実行中、Till条件に指定したフォース動作制限の条件を達成した場合は動作を中断します。
力覚センサーの現在値とピーク値を取得、表示します。この例では、値を表示するだけですが、これらの値を使って作業の成否判定を行い、処理を分岐させることもできます。
フォース動作制限の結果を取得します。この例では、条件を達成していた場合はオーバーランとしてエラーを表示するだけですが、リカバリー処理などができます。
もし、作業中に過大な力を検出した場合は、プログラムは中断され、PressSample_EHandle関数が割り込み実行されます。作業中にかかったピーク値を取得, 表示して、ロボット動作を停止しエラー状態とします。この例では、エラー状態にしていますが、リトライするなどエラー時の処理を実行することもできます。

← フォース動作制限機能のSPEL+プログラミングチュ－トリアル →