1.基本的情報

プラグイン名：GraspDataGen
依存プラグイン：Grasp;GeometryHandler;SoftFingerStability;ConstraintIK;PCL

2.主な機能・説明

・ロボットハンドで対象物を把持可能な姿勢・位置のデータを生成します。
・箱に入っている対象物を掴むのに最適なロボットの腰位置探索を行います。

2.1. DataGenバー

- GenerateGraspPattern
　対象物体の把持データベースを作成する
- AppendGraspPattern
　把持データベースに手動で把持姿勢を追加する
- DisplayBoundingBox
　指定されたクラスタ数に分割したときのバウンディングボックスを表示する
- SearchHandScale
　ハンドサイズを変更して把持データベースを作成する
- Grasp
　把持データベース中の指定した把持姿勢を確認・修正を行う
- SearchWaistPos
　箱の中の把持対象物を掴むのに最適なロボット位置を探索します。
- DisplayWaistPos
　SearchWaistPosの結果を表示します。
- WaistPosMotion
　SearchWaistPosのロボットの移動モーションを生成します（実装中）。
- SetCollectionBox
　WaistPosMotionでモーションを生成する際、ロボットと一緒に動く箱を設定します。
- SetCollectionObj
　WaistPosMotionでモーションを生成する際、ロボットと一緒に動くオブジェクトを設定します。
- ClearCollection
　SetCollectionBox,SetCollectionObjでの設定をクリアします。
- Param
　把持計画パラメータを設定します。

2.2.使用方法

2.2.1.把持データベース作成方法

1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押す。
2.アイテムタブから対象ロボットを選択し、PlannerバーのSetRobotボタンを押す。
　ハンドとロボット本体が分かれたモデルの場合は、PlannerバーのAttachHandで対象ハンドをアタッチする。
３.DataGenBarのGenerateGraspPatternボタンを押すとパラメータを設定するダイアログが表示されます。
4.パラメータを設定しOKボタンを押すと把持可能な姿勢・位置を探索し始めます。
　number of clusters:指定したクラスタ数に分割し、把持データベースを作成します。
　overlap volume ratio:クラスタ同士の重なりが指定した割合になるように分割し、把持データベースを作成します。
　scale: 対象オブジェクトが元の何倍であるか指定します(通常は1.00を指定します)
　distance between search point:探索ポイント数（探索する点の間隔）
５.メッセージウィンドウに"GraspPattern generating is finished"が表示されたら終了です。
extplugin/graspPlugin/RobotModels/"対象ロボット名"/"把持した腕のデータパス"/preplanning_"対象物名".txtに把持可能な姿勢・位置が保存されます。
ここで"把持した腕のデータパス"は対象ロボットのyamlファイル中のdataFilePathの値です。設定されていない場合はdataとなります。

2.2.2.把持データベースへの把持姿勢追加

1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押す。
2.アイテムタブから対象ロボットを選択し、PlannerバーのSetRobotボタンを押す。
　ハンドとロボット本体が分かれたモデルの場合は、PlannerバーのAttachHandで対象ハンドをアタッチする。
3.AppendGraspPatternボタンを押すと対象物体のクラスタリングパラメータを設定するダイアログが表示されます。
　パラメータの意味は把持データベース作成方法に示したものと同じです。
4.パラメータを設定しOKボタンを押すと把持姿勢追加ダイアログが表示されます。
5.x,y,z,roll,pitch,yaw(手首座標系)の+,-ボタンで対象物体が移動します。右端の列は刻み幅で、単位はx,y,zが[m]、roll,pitch,yawが[deg]です。
　Nextfaceボタンをクリックするとバウンディングボックスが回転するように対象物体が移動します。
　NextBBボタンをクリックすると次のバウンィングボックスが対象となります。
　stabilityに把持安定性の値が表示されます。
6.Appendボタンをクリックすると把持データベースに把持姿勢が追加されます。

2.2.3.クラスタのバウンディングボックス表示

1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押す。
2.DataGenBarのDisplayBoundingBoxボタンを押すとパラメータを設定するダイアログが表示されます。
3.パラメータを設定しOKボタンを押すとバウンディングボックスが表示されます。
　パラメータの意味は2.2.1の4と同じです。
　show only target clusterをチェックすると把持データベース作成の対象となるクラスタのみ表示されます。

2.2.4.ハンドサイズを変更して把持データベース作成

この機能は対象ロボットモデルがTrobotの時しか正常に動作しません。
1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押す。
2.アイテムタブから対象ロボットを選択し、PlannerバーのSetRobotボタンを押す。
3.DataGenBarのSearchHandScaleボタンを押すとパラメータを設定するダイアログが表示されます。
　(a)xyz軸を同時に変化させる
　　"xyz-axis"を選択する。
　　"Scale range(xyz)"を設定する。
　　start:開始倍率
　　end:終了倍率
　　step:倍率の刻み幅
　(b)x,y,z軸のどれか1つの軸に対し変化させる
　　変化させる軸を選択する。x軸ならば"x-axis"。
　　"Scale range(xyz)"を設定する。
　　start:開始倍率
　　end:終了倍率
　　step:倍率の刻み幅
　　"Scale"で変化させない軸の倍率を設定する

　"Clustering parameters"以下のパラメータの意味は2.2.1の4と同じです。
4.パラメータを設定しOKボタンを押すと把持データベースが作成されます。
倍率ごとに把持データベース「extplugin/hrgPlugin/Trobot/data/preplanning_"対象物名""倍率".txt」が生成されます。

2.2.5.把持姿勢表示

1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押す。
2.アイテムタブから対象ロボットを選択し、PlannerバーのSetRobotボタンを押す。
　ハンドとロボット本体が分かれたモデルの場合は、PlannerバーのAttachHandで対象ハンドをアタッチする。
3.DataGenBarのGraspボタンを押すとパラメータを設定するダイアログが表示されます。
　target number:把持データベースの何番目の姿勢を表示するか
　show contact cluster: チェックすると対象オブジェクトの接触領域が表示されます
　show hand contact cluster: チェックするとハンドの接触領域が表示されます
　move object:ハンド位置を固定し、対象物を移動させ把持します
　move arm: 対象物位置を固定し、対象物を把持するようアームを移動します。対象物の位置姿勢により、把持できない場合があります。
Modifyボタンを押すと修正モードになります。操作方法は2.2.2.把持データベースへの把持姿勢追加の５と同様です。

2.2.6.ロボット位置探索

ロボット腰位置探索を参照してください。

2.2.7.把持計画パラメータ

把持計画パラメータ設定方法は把持計画パラメータ設定方法に記述してあります。

3.その他

3.1.オプション

以下の機能等はソースファイルを直接編集する必要があります。

3.1.1接触点表示

本機能を使用するにはDebugModeをONにする必要があります。
DebugModeをONにするにはGraspDataGenerator.cppに#define OUTPUT_RESULTを追加し、GraspDataGenBar.cppに#define DEBUG_MODEを追加します。
また、事前にDebugModeをONにした状態で把持データベース作成を行い接触点情報ファイルを作成する必要があります。
1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押します。
2.DataGenBarのDisplayContactPointボタンを押すと把持データベースの接触点が表示されます。

3.1.2.探索方法の変更

ばねなどのモデルでは、領域をきれいに分割することが難しいことがあります。ばねを全体を１つの領域とし、その領域内をすべて探索することで把持点を得ることができます。
変更方法はGraspDataGenerator.cpp:28行目
//#define NOCHECK_BOUNDINBOXSIZE
のコメントをはずします。ただし、領域の大きさによっては計算時間が増大するので注意が必要となります。

3.1.3.法線ベクトルがばらばらのときの対処

ポリゴンが時計回り、反時計回りが混在しているとき法線ベクトルがばらばらになります。このとき、把持安定性が正しく求まりません。
GraspDataGenerator.cpp:29行目
//#define FLIP_NORMAL
のコメントをはずすことで、対処療法的ですが対応できます。

3.1.4制御点描画の切替

デフォルトでは、データベース生成終了時に把持時の接触点が描画されます。
変更したい場合は以下をプリプロセッサ定義してください。
・接触点をクラスタ直方体に射影したものを描画したい場合：
DRAW_CONTACTPOINT_ON_BOX
・制御点（アプローチ点）をクラスタ直方体に射影したものを描画したい場合：
DRAW_APPROACHPOINT

3.2.出力ファイル形式

-把持データベース
　ForceClosure値の高い順に以下のデータが書き込まれる。
　　ForceClosure値　対象物から見たハンドの姿勢　対象物から見たハンドの位置　指の関節角度

4. 動作確認

4.1. バウンディングボックス表示

choreonoidを起動します。
extplugin/graspPlugin/GraspDataGen/project/displayBoundingBox.cnoidを読み込みます（「ファイル」->「プロジェクトの読み込み」）。
DataGenバーの「DisplayBoundingBox」ボタンをクリックすると下記ダイアログが表示されますのでOKを押します。

下図のようにバウンディングボックスが表示されます。

choreonoidを終了します。

4.2. 把持データベース作成・把持姿勢表示

choreonoidを起動します。
extplugin/graspPlugin/GraspDataGen/project/generateDB_HIRO.cnoidを読み込みます（「ファイル」->「プロジェクトの読み込み」）。
extplugin/graspPlugin/Samples/Object/mug2hrp.wrlを読み込みます（「ファイル」->「読み込み」->「OpenHRPモデルファイル」）。
アイテムタブからmug2を選択し、Plannerバーの「SetOject」をクリックします。
DataGenバーの「GenerateGraspPattern」ボタンをクリックすると下記ダイアログが表示されますのでOKを押します。

把持姿勢探索が始まります。探索が完了するまで30秒から1分程度要します。
メッセージウィンドに

 GraspPattern generating is finished

と表示されれば、完了です。
extplugin/graspPlugin/RobotoModels/HIRO/right_data/preplanning_mug2.txtが生成（上書き）されているか確認してください。

次に把持姿勢を表示します。
DataGenバーの「Grasp」ボタンをクリックすると下記ダイアログが表示されるのでOKボタンを押します。

下記画像のようにmug2を把持する姿勢に変化することを確認してください。

ダイアログの「target number」を変化させ、異なる把持姿勢になることを確認してください。
choreonoidを終了します。

4.2. 接触領域表示

choreonoidを起動します。
extplugin/graspPlugin/GraspDataGen/project/contact_region.cnoidを読み込みます（「ファイル」->「プロジェクトの読み込み」）。
アイテムタブからahiruを選択し、Plannerバーの「SetObject」ボタンを押します。
まず把持対象の接触領域を表示します。
DataGenバーの「Grasp」ボタンをクリックすると、ダイアログが表示されるので、「show contact cluster」にチェックを入れ、OKボタンを押します。
アイテムタブからHIRO,ahiruのチェックをはずし、非表示にします。
下記画像のように、接触領域が表示されたahiruが描画されます。

次に、ハンド側の接触領域を表示します。
DataGenバーの「Grasp」ボタンをクリックし、ダイアログの、「show contact cluster」にチェックをはずし、「show hand contact cluster」にチェックをいれ、OKボタンを押します。
下記画像のように、接触領域が表示されたハンドが描画されます。

choreonoidを終了します。

4.3. ピックアンドプレース

まず、動作確認用にソースを変更しビルドしなおします。
CCMake時にUSE_DB_IN_PICKANDPLACEをチェックしておいてください。
extplugin/graspPlugin/PickAndPlacePlanner/ManipController.hの12行目

 //#define OBJ_ENV_CONTACT

のコメントをはずします。
extplugin/graspPlugin/ObjectPlacePlanner/PlacePlanner.cppの16行目

 //#define BOUNDINGBOX_PUT

のコメントをはずし、ビルドします。

choreonoidを起動します。
extplugin/graspPlugin/GraspDataGen/project/pickandplace_mug.cnoidを読み込みます（「ファイル」->「プロジェクトの読み込み」）。
シーンビューを編集モードにします（適当なオブジェクトをダブルリック）。
テーブルの上の箱の中の中心付近にマウスを移動し、Ctrlを押しながらクリックします。すると下図画像のように矢印が現れます（オブジェクトを置く位置を指定しています）。

PlannerバーのGraspボタンをクリックします。
メッセージウィンドウに「Success: 0」と出ます。（もしfailと出るようならばオブジェクトを置く位置を移動するか、mugを移動してください）
PathPlanバーのStartをクリックします。
メッセージウィンドウに「Trajectory Planning is finished」と表示されれば成功です。
ツールバーのアニメーション開始ボタンを押すと、ピックアンドプレースのモーションが表示されます。
choreonoidを終了します。

4.4. 腰位置探索