Grasp プラグイン解説

GraspプラグインはgraspPlugin for Choreonoidの中で最も基礎となるプラグインです。本プラグインが提供する機能は多指ハンドに対する把持計画と、他のプラグインが共通に利用する関数です。

基本情報

プラグイン名： Grasp
依存プラグイン：無
動作を確認したロボットモデル：PA10, SmartPAL5, RH1, HRP-3, HRP-2 JRL

プランナーバー

「=Planner=」というタイトルを持つプランナーバーを使って、把持計画を作成します。

詳細はプランナーバーヘルプをご参照ください。

チュートリアル

把持計画作成チュートリアルにPA10を用いた把持計画の例を示します。
Graspプラグインにおいて新しいロボットのモデルを導入する方法についても解説しています。

動作計画について

Grasp プラグインは、把持や設置のために適切なグリッパの位置を求めるもので、アーム全体をどのように動かすかについては考慮されていません。周囲の環境に干渉しないアームの動作計画を行うには、PRM プラグインを用います。PRM プラグイン解説をご覧ください。

把持計画アルゴリズム

本プラグインでは様々な種類のロボットに対応できるように、ハンドの把持領域と対象物の両方にバウンディングボックスモデルを仮定した把持計画手法を採用しています。本手法を用いることで、例えば多指ハンドであれば３指の指先把持、４指の指先把持、あるいは包み込み把持など様々な把持形態を実現することができます。
ここで、ハンドの把持領域のバウンディングボックスをGrasping Rectangular Convex (GRC)、対象物のバウンディングボックスをObect Convex Polygon (OCP)と呼び、これらの幾何学的関係により大まかな手首の位置姿勢を求め、その上でランダムサンプリングにより最終的な把持姿勢を求めます。
また、把持計画を高速化するために、摩擦円錐の楕円近似を用いたForce Closureの判定手法を用いています。ただし、Force Closure判定の従来手法もプラグインの関数として用意しています。アルゴリズムの詳細については下記リファレンスを参照してください．

References

[1]Kensuke Harada, Kenji Kaneko, and Fumio Kanehiro, Fast Grasp Planning for Hand/Arm Systems Based on Convex Model, 2008 IEEE International Conference on Robotics and Automation, pp. 1162-1168, 2008.
[2]T. Tsuji, K. Harada, and K. Kaneko, Easy and Fast Evaluation of Grasp Stability by using Ellipsoidal Approximation of Friction Cone， Proc. of 2009 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems，pp.1830-1837、2009.
[3]原田、辻、金子、金広、丸山，直方体モデルに基づく多指ハンドの把握計画，日本機械学会論文集C編，76-762，pp.331-339, 2010
[4]T. Tsuji, K. Harada, K. Kaneko, F. Kanehiro, and K. Maruyama, Grasp Planning for a Multifingered Hand with a Humanoid Robot, Journal of Robotics and Mechatronics, Vol.22 No.2, pp. 230-238, 2010.

Graspプラグインにおいて新しいロボットのモデルを導入する方法

HRP2JRLを例として、新しいロボットのモデルを導入した際に把持計画を行うまでに必要な準備について説明します。

(1)yamlファイルの編集
graspPlugin/RobotModels/HRP2JRL/HRP2.yamlを編集します。これは、以下のようになっています。各エントリの意味については右側に書いています。

 modelFile: HRP2main.wrl
 graspPluginSetting: 
   name: HRP2   %ロボット名
   base: CHEST_JOINT1  %IKを解く場合のベースリンク
   palm: RARM_JOINT5   %IKを解く場合の手首リンク
   fingerEnds: [ RHAND_JOINT1, RHAND_JOINT4 ] %指先リンク
   prehensionList: [ prehension ] %把持の参照姿勢（prehension.pos, prehension.prm)、複数の参照姿勢を指定可能
   armStandardPose: [0, 0, 0, -1.57, 0, 0] %繰り返し計算でIKを解く場合に用いるアームの基準姿勢(baseからpalmまでの関節角）
   fingerOpenPose: [0.78, -0.78, 0.78, -0.78, 0.78, -0.78] %指を最大限開いたポーズ
   interlink: [   [ RARM_JOINT6, 1.0 , RHAND_JOINT0, -1.0, RHAND_JOINT1, 1.0  ], [ RHAND_JOINT2, -1.0, RHAND_JOINT3, 1.0, RHAND_JOINT4, -1.0  ]  ]  %連動リンクの定義
   GrasplotPluginDir: Plugin %独自のIKを解くプログラムを格納するディレクトリ名

特に説明が必要なのはinterlinkの定義だと思います。HRP2JRLロボットはハンドは２指６リンクあるのですが、これらは連動して動きます。この連動を定義するのが、このエントリです。

(2)PRMファイルの編集
次に、上記yamlファイルの中にあるprehensionListには、用いる把持形態（３指指先把握、４指指先把握、包み込み把握など）に関するパラメータを記述するファイル（prmファイルとposファイル）を把持形態毎に用意します。この例ではprmファイルはprehension.prmのみです。また、posファイルは、対応する把持形態において基準となる把持姿勢を格納するファイルです。この例ではprehension.posです。prmファイルとposファイルは拡張子以外では同じ名前にしなければなりません。prmファイルとposファイルはgraspPlugin/RobotModels/HRP2JRL/dataに格納します。

(2-1) prehension.prm
このファイルは[START]と[END]の間に例えば以下のようになエントリを記入します。

 Reference_Motion prehension
 Finger0Contact  0 1 0
 Finger0Comp     0 0 0
 Finger0Close   0.01 -0.01 0.01 
 Finger1Contact  0 1 0
 Finger1Comp     0 0 0
 Finger1Close   -0.01 0.01 -0.01  
 GRCdes_Position 0.0 0.0 -0.19
 GRCmax_Position 0.0 0.0 -0.30
 GRCmin_Position  0.0 0.0 -0.19
 GRCdes_Rpy   -1.57 0 0 
 GRCmax_Rpy  -1.57 0 0 
 GRCmin_Rpy  -1.57 0 0 
 GRCdes_Edges  0.10 0.05 0.05
 GRCmax_Edges  0.30 0.30 0.10
 GRCmin_Edges  0.01 0.01 0.01 
 Max_Load  1.0
 Min_Load  0

それぞれのエントリについて、以下に説明します。

Finger[i]Contact：第i指のどのリンクが対称物と接触するかを定義します。HRP2JRLモデルの場合、第０指第１指共に２番目のリンクのみが対称物と接触します。
Finger[i]Comp：第i指の第jリンクを対称物に接触させる場合どの関節を使ってリンクの位置を補償するかを定義します。HRP2JRLモデルの場合、全てについて第０関節を使います。
Finger[i]Close：第i指の第jリンクを対称物に接触させる場合、補償する関節の刻み幅を指定します。関節が閉じる方向に指定しなければなりません。
GRC[max,des,min]_Position, GRC[max,des,min]_Rpy, GRC[max,dex,min]_Edges：把持形態を指定する直方体であるGRC(Grasping Rectangular COnvex)の位置、姿勢、辺長を定義します。下の図にGRCmaxとGRCdesの例を示します。注意すべき点としてGRCmaxは対象物の位置をランダムサンプルする範囲を指定します。より正確に言うと、対象物のバウンディングボックスを求め、このバウンディングボックスがGRCmaxに含まれる範囲でハンドの位置をランダムサンプリングします。また、GRCdesは望ましい把持形態を指定します。中尉すべき点としては、GRCの姿勢を指定する際に、ハンドが対象物にアプローチ方向はGRC座標系で+y方向にしなくてはなりません。またGRCの姿勢はとりあえず全てについて同じにしておいてください。また、位置／姿勢については手首リンクからの相対位置／姿勢として定義します。
[Max,Min]_Load：把持対象物の最大／最小の重量をkg単位で指定します。

なお、GRCの編集をする場合、予め直方体のvrmlモデルを用意し、EditModelプラグインにより形状を変更すると便利です。また、直方体のや手首リンクの位置／姿勢はChoreonoidのボディ/リンクタブで調べることができるので、これにより手首リンクから見た位置／姿勢を求めることが出来ます。

(2-2) prehension.pos
このファイルは例えば以下のようなエントリになっています。

0 0.78 -0.78 0.78 -0.78 0.78 -0.78

つまり、最初の0は任意の数で結構ですが、それ以降はVRMLファイルに指定している順番で指関節の角度を指定します。

以上によりGraspプラグインを使える準備が整いました。

GRCmax
GRCdes

アームの軸構成に応じて予めちゃんと動く逆運動学の関数を用意しないと、必要以上に把持計画の実装に苦労することになります。graspPluginではArm.cppの中にデフォルトの逆運度学を用意しています。この関数で十分な場合は多いですが、デフォルトの逆運動学を使わずにロボット固有の逆運動学を用いる場合、その関数はgraspPlugin/RobotModels/ModelName/Pluginに置きます。

大まかなパームの姿勢は求まるが、指の姿勢がなかなか定まらない場合は、Finger0CloseやFinger1Closeのパラメータの値を小さくしてみてください。