GripperManipulationプラグインはパラレルグリッパを持つロボットに対してピックアンドプレースの動作計画を行うプラグインです。把持計画手法についてはパラレルグリッパに特化した独自の手法を実装しています。この手法は非常に高速に計算できるの実時間で把持姿勢を求めることが出来ます。また、バウンディングボックスを用いないため複雑な形状の対象物にも比較的容易に適用できます。
本プラグインを導入すると、プランナバーのStartボタンを押すとGraspプラグインの把持計画が実行されるのではなく、GripperManipulationプラグインの把持計画が実行されるようになります。また、本プラグインを新たに導入するとINTENTIONバーが現れます。このバーは把持対象物の部位を指定するインテンションパラメータを指定するものです。
下記文献[1]には、把持計画アルゴリズムのうち、非実時間部と実時間部の両方が記述してありますが、本プラグインではその実時間部を実装しています。非実時間部はGeometryHandlerという公開バージョンには含まれていないプラグインを使います。(追記予定)
[1] K. Harada, T. Tsuji et al, "Grasp Planning for Parallel Grippers Considering Flexibility on its Grasping Surface", Proc. of IEEE Int. Conf. on Robotics and Biomimetics, 2011.
Choreonoid/extPlugin/graspPlugin/GripperManipulation というプラグインに、RTコンポーネントの「口」を取り付ける。
双腕ロボットの把持計画を行う graspPlugin プラグインであり、ファイル構成は以下のようになっている。
ここで、プログラムの構造がわかりやすいよう、GripperManipulationMain.cpp を、ManipPlugin.cpp にリネームする。
合わせて CMakeLists.txt も修正して、コンパイルできることを確認する。
下準備として、まず GripperManipulation の下に rtcディレクトリを作成し、ここに GraspController.idl を置く。
さらに、Eclipse を起動し、このrtc ディレクトリにダミーの新規 Eclipse プロジェクトを設定する。(*注) 本文では作業ディレクトリを /home/asahi/workspace として進めているので、読者はこの部分を自分の作業ディレクトリに置き換えて読むこと。
完了ボタンを押すと、C++ パースペクティブを開きますか? というダイアログが出るが、そのまま閉じればよい。
これで下準備は完了したので、Eclipse で RTC Builder パースペクティブを開く。
RTのアイコンをクリックすると、RtcBuilder ビューが開くので、タブごとに必要な情報を設定していく。
その他の項目はデフォルトのままでよい。
C++を選択する。
画面の指示にしたがって C++パースペクティブに切り替える。
このとき 一緒に保存されている RTC.xml に、一連の設定が保存されている。
RTコンポーネントの設定を変更する時は、RTC Builderの基本タブの中にある、プロファイルのインポート機能で、この設定を呼び戻すことができる。
端末を開いて GripperManipulation/rtc ディレクトリに移って、以下のコマンドを実行する。
% omniidl -bcxx GraspController.idl
% make -f Makefile.GripperManipulation
graspPlugin/GraspConsumer から、
GraspRtcController.h と GraspRtcController.cpp を GripperManipulation にコピーして、編集する。
ManipController.h(cpp) とまぎらわしいので、まずファイル名を ManipRtc.h(cpp) に変更する。
サンプルとして、一連のファイル編集を一度に行うパッチファイルを作成した。
% svn co -r131 http://subaru.ait.kyushu-u.ac.jp/svn/grasp-plugin/trunk/graspPlugin/GripperManipulation
% patch -p0 < GripperManipulation_patch.txt
以下は、パッチファイルの変更点の概要である。
クラス名を ManipRtc に変更する。(Eclipseのリファクタリング機能が便利だが、誤って元のGraspConsumer まで変更しないよう、注意!)
ManipPlugin::initialize メソッドに、RTコンポーネントをスタートさせるコードを追加する。
virtual bool initialize() { //Robotics::SceneBodyManager* manager = Robotics::SceneBodyManager::instance(); grasp::GraspController::instance(ManipController::instance()); ManipRtc::instance()->RtcStart(); //manage(manager->addSceneBodyFactory(create));
grasp::ManipRtc クラスを、GripperManipulation のフレンドクラスに設定する。
using namespace RTC; namespace grasp { class ManipRtc; } ... class GripperManipulation : public RTC::DataFlowComponentBase { public: friend class grasp::ManipRtc;
RTCBuilderはIDLデータ型の引数をCORBAとして出力するが、omniidlは同じものを::CORBAとして出力するため、型が異なるとコンパイラがエラーを出してしまう。
そこで、omniidlの出力に合わせて、CORBAネームスペースの型の引数の宣言を、::CORBA に書き換える。
また、GraspPlanResultSVC_impl クラスのメソッドの引数について、GraspPlanStart ネームスペースの型の引数宣言を GraspPlanResult に書き換える。(これはRTCBuilder のバグか)
初めの一回は、Choreonoid ディレクトリで ccmake を実行する。
% ccmake .
Choreonoid と RT System Editor を起動して、NameServer の127.0.0.1 の(ホスト名)|host_cxtの下に、GripperManipulation0|rtc があることを確認する。
これを System Diagram 上にドラッグ&ドロップできれば成功である。
(ただし、これを他のRTコンポーネントと結びつけても、まだ何も起こらない。GripperManipulation をRTコンポーネントに改造する・実装編につづく)
新しい対象物を掴ませたい場合、以下のステップを踏むことでできます。
1. vrmlモデルをgraspPlugin/Samples/Object以下に置いてください。
2.対象物固有のyamlファイルを編集します。開発者用レポジトリにはGeometryHandler プラグインがあり、これを使うことで自動的にyamlファイルが生成されるのですが、簡単な形状の対象物であれば比較的簡単に自力で生成することが出来ます。具体的にはgraspPlugin/Samples/Object/W0.wrl とgraspPlugin/GripperManipulation/project/W0.yamlを見比べることで分かるのではないかと思います。
3.最後に、Choreonoidを起動し、掴ませたい対象物のモデル(yamlファイル)を選択してPlannerバーのSetObjectを押すことで把持対象物と認識されます。
以上です。
次に、新しいロボットのモデルを導入したい場合は、まずロボットのVRMLモデルとyamlファイルをgraspPlugin/RobotModels以下に置いた後、graspPlugin/GripperManipulation/project/[ロボット名].prmという名前のファイルを編集します。例がPA10.prmやHIRO.prmとして同じディレクトリの中にあります。本把持計画の手法では、まず接触点の位置が与えられて、それに従って手首の位置・姿勢が決定されます。(なので、オフラインでForceClosureの計算を行うことが出来、オンラインでの計算を高速化できます。)先に手首の位置・姿勢を与えるタイプの把持計画(例えばGraspプラグインで実装した把持計画)とは異なりますので、本手法では接触点の位置とアプローチベクトルが与えられた時に手首の位置・姿勢を導出する関数を自力で作る必要があります。この関数はgraspPlugin/RobotModels/[RobotName]/Plugins/GrasplotPlugin.cppにあり、現時点でPA10とHIROに対応しています。
HiroNXには手首に力センサが装着されているモデルがあります。この力センサ分のオフセットを考慮する方法を以下に記します。
extplugin/graspPlugin/RobotModels/HIRO/Plugin/CMaleLists.txtを見てください。この中でfs_offset.diffのパッチを当てている部分がコメントアウトされていますので、このコメントを外すことでvrmlファイルでオフセットが含まれます。
次に、IKを解く部分でも、オフセットを考慮しなければいけません。これは、extplugin/graspPlugin/RobotModels/HIRO/Plugin/GrasplotPluginHIRO.cppの38-42行目辺りにあるコメントを外すことで可能になります。