基本情報

プラグイン名：PickAndPlacePlanner
依存プラグイン: Grasp, PRM, GeometryHandler, ObjectPlacePlanner

使い方

本プラグインを含めると、ccmake の際にUSE_DB_IN_PICKANDPLACEというオプションが現れるようになります。これをONにするかOFFにするかで、用いる把持計画手法が異なります。ONで参考文献[3], OFFで参考文献[1]の手法が用いられます。ONの場合はGraspプラグイン把持姿勢DB生成機能、OFFの場合はGeometryHandlerプラグインを参照して把持姿勢のデータベースを構築してください。

USE_DB_IN_PICKANDPLACEがOFFの場合は双腕ロボットデモ手順書rev6.pdfに主な使い方が記してあります。

USE_DB_IN_PICKANDPLACEがONの場合は戦略デモについてに主な使い方が記してあります。

戦略デモに関する付加的な説明

戦略デモの場合は、graspPlugin/PickAndPlacePlanner/ManipController.hにおいて、下記のdefine文のコメントを外してください。

 #define DEMO_20140612
 #define MOVE_ARM_ONEBYONE

そして、graspPlugin/ObjectPlacePlanner/dataにdata_put.matというファイルを置いてください。このファイルは、対象物を治具に置く位置・姿勢を定義しています。例えば、以下のような記述形式となります。

 #
 # Candidate - 0
 4 4
 -0.061396 -0.943419  0.325869  709.6
 0.975854 -0.125304 -0.178909  0043.4
 0.209619  0.307016  0.928332  1065.4
        0         0         0       1

また、戦略デモの場合は依存プラグインは上記に加えてPCL, RobotInterfaceとなります。

実験においてはカメラ座標系とロボット座標系の間でのキャリブレーションが必要になりますが、そのキャリブレーションを行った結果のキャリブレーション行列はgraspPlugin/PCL/calibtools/calibmat.txtに記述します。また、RobotInterfaceプラグインでこのキャリブレーション行列を使うために、graspPlugin/RobotInterface/RobotInterface.cppにおいて以下のdefine文にコメントを外します。

 #define USE_PCL_PLUGIN

RobotInterfaceプラグインではgraspPlugin/RobotInterface/dataに置かれた,data_cap.matという名前のファイルにカメラ座標系から見た対象物の位置・姿勢が記述されており、それをgraspPlugin/PCL/calibtools/calibmat.txtに記述された変換行列によってロボット座標系に変換します。

最後に、ロボットのyamlファイルを編集します。具体的には、graspPlugin/RobotModels/HIRO/HIRO_expipe_AirHand.yamlを参照してください。特に説明が必要なのは、以下のエントリです。

 graspPluginSetting: 
  - :右の吸着ハンドの設定
    base: WAIST
    palm: RARM_JOINT5
    fingerEnds: [  ]
    dataFilePath: right_data : 把持データベースを格納するファイルの相対パス
    prehensionList: [prehension_R_pipe] : 把持データベースのファイル名
    armStandardPose: [0, 0, 0, -1.57, 0, 0, 0]
    armFinalPose: [-0.78, 0.36, -0.79, -1.88, 0.32, 1.1, 0] : 持ち替えを行う際の基準姿勢
    regraspPoseRel_Roll: [0] : 基準姿勢からroll方向にどれだけ変化させながら持ち替え姿勢を探索するか
    regraspPoseRel_Pitch: [0] : 基準姿勢からpitch方向にどれだけ変化させながら持ち替え姿勢を探索するか
    regraspPoseRel_Yaw: [0, 1.033, -1.033, 2.067,  -2.067, 3.1, -3.1] : 基準姿勢からyaw方向にどれだけ変化させながら持ち替え姿勢を探索するか
    regraspPoseRel_X: [0.0] : 基準姿勢からｘ方向にどれだけ変化させながら持ち替え姿勢を探索するか
    regraspPoseRel_Y: [0.0] : 基準姿勢からy方向にどれだけ変化させながら持ち替え姿勢を探索するか
    regraspPoseRel_Z: [0] : 基準姿勢からz方向にどれだけ変化させながら持ち替え姿勢を探索するか
    GrasplotPluginDir: ./
  - 　：多指ハンド（左手）の設定
    base: WAIST
    palm: LARM_JOINT5
    fingerEnds: [ LHAND_JOINT2, LHAND_JOINT5, LHAND_JOINT6 ]
    dataFilePath: left_data
    prehensionList: [prehension_L_pipe]
    armStandardPose: [0, 0, 0, -1.57, 0, 0, 0]
    fingerOpenPose: [-0.8726, 0, 0, -0.8726, 0, 0, 0.8726]：指を開いた姿勢
    fingerOpenPoseOffset: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]：指を閉じた姿勢から更にどの程度オフセットをのせるか（把持力を加えるために）
    fingerOffset: [0.0]
    GrasplotPluginDir: ./
    mu: 0.8　：対象物と指の摩擦係数（把持データベースを作成する際に利用）
    fmax: 15.0　：対象物と指の最大垂直抗力（把持データベースを作成する際に利用）

補足説明

GripperManipulationプラグインは開発者向けバージョンではPickAndPlacePlannerと改名しました。これは、パラレルグリッパ以外のハンドも扱えるようにしたことによることと、将来的にManipulationPlannerを分離する予定だからです。
本プラグインに関する詳細は公開バージョンのGripperManipulationプラグインのマニュアルを参照してください。ここで、2012年7月に開発者向けバージョンではGripperManipulationプラグインからObjectPlacePlannerプラグインを分離させました。また、公開バージョンではGeometryHandlerプラグインの機能の一部をGripperManipulationプラグインに含めていますが、開発者向けバージョンではGripperManipulationプラグインはGeometryHandlerの機能を使うため、GeometryHandlerプラグインに依存しています。よって、開発者バージョンを使う場合は依存プラグインの設定に注意してください。

参考文献

[1] K. Harada, T. Tsuji, K. Nagata, N. Yamanobe, K. Maruyama, A. Nakamura, and Y. Kawai, "Grasp Planning for Parallel Grippers with Flexibility on its Grasping Surface"， Proc. of IEEE Int. Conf. on Robotics and Biomimetics，pp.1540-1546、2011/12
[2] K. Harada et al, "Pick and Place Planning for Dual Arm Manipulators", Proc. IEEE Int. Conf. on Robotics and Automation, 2012/5
[3] K. Harada, K. Kaneko, and F. Kanehiro, "Fast Grasp Planning for Hand/Arm Systems Based on Convex Model", IEEE Int. Conf. on Robotics and Automation, pp. 1162-1168, 2008.