graspPlugin for Choreonoid

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のパラメータの値を小さくしてみてください。