graspPluginは、アームの先端にハンドが取り付けられた構成のロボットシステムに対して、把持計画、軌道計画、作業計画など、種々の計画問題を解くことが出来るソフトウェアです。種々のロボットに対して種々の計画問題を解くために、ソフトウェアは再利用性が高いことが要求されます。例えば、グリッパをとってみても、平行グリッパ、回転グリッパ、片側可動、両側可動など種類が異なる多種のグリッパが存在します。graspPluginはロボットの仕様や作業目的に応じて拡張や機能をカスタマイズできるように、ロボット動作振り付け統合ソフトウェア Choreonoid をフレームワークとして用い,このプラグインとして個々のソフトウェアモジュールを開発しています。
なお、軌道計画を行うプラグインとしてスタンフォード大学で開発されたMPK(Motion Planning Kit)をポートしたもの、PRM, RRT, RRT*をgraspPlugin for Choreonoidでは用いることができます。このMPKはJean-Claude Latombe教授のご厚意でサイトにアップロードしているもので、そのライセンスはMPK本体のライセンスに従います。
詳しい使い方は、ContentsのgraspPluginのリンクをクリックすることでみることができます。
開発チーム
辻 徳生(金沢大)
原田 研介(大阪大学/産総研)
MIT
graspPluginのインストール方法 をご覧ください。
以下に様々な情報がありますので、必要に応じてみてください。
graspPluginのFree VersionはGitHubで公開しています。以下です。
Choreonoid 1.6やChoreonoid1.5のzipファイルを http://choreonoid.org/ja/download.html からダウンロードし,~/srcに展開する。
必要なアプリケーションを以下のコマンドでインストール
% cd ~/src/choreonoid/misc/script % ./install-requisities-ubuntu-14.04.sh (OSに応じてファイル名が異なります)
graspPluginを~/src/choreonoid/ext (Choreonoid1.5以降) , ~/src/choreonoid/extplugin (Choreonoid1.4) 以下に展開する。レポジトリからインストールする場合は、公開レポジトリを参照。
graspPlugin/Grasp, graspPlugin/PCL, graspPlugin/GeometryHandlerなどのディレクトリにinstall-requisities-ubuntu.shのスクリプトファイルがあるので,これらを実行する.
chorenoid1.5以上では、下記のコマンドでextplugin(extへのシンボリックリンク)を作成する。$ cd ~/src/choreonoid $ ln -s ext extplugin
OpenRTMを使用するプラグインを使う場合(RobotInterfaceなど)を使う場合にインストールする.詳細はOpenRTMのサイトで確認する.
OpenRTMのサイト http://www.openrtm.org/openrtm/ja/content/tools より、eclipse toolsの全部入りをインストールする。
% cd ~/src/choreonoid % ccmake .
GRASP_PLUGINS Grasp;PRM;GeometryHandler;ObjectPlacePlanner;PickAndPlacePlanner;PCL;RobotInterface;GraspDataGen;SoftFingerStability;ConstraintIK
GRASP_ROBOT_MODEL_PLUGIN HIRO/Plugin
HRGPLUGINS
HRGRobotModelsPlugin
のように設定する。また、Choreonoid 1.4の場合は
ENABLE_OSG ON
OSG_DIR /
とし、OpenSceneGraphが使えるようにする。また、Choreonoid1.6の場合には
USE_PYBIND11 OFF
USE_PYTHON3 OFF
USE_QT5 ON
とする。ここで、cを複数回押しながら、更にオプションの設定をする。例えば、PCLプラグインを用いている場合、一回cを押した時点で、BUILD_PCL_PLUGINが現れるので、これをONにする。PickAndPlacePlannerプラグインを用いている場合は、一回cを押した時点でUSE_DB_IN_PICKANDPLACEが現れる。戦略デモに関してはこれをONにする。(2011国際ロボット展デモではOFF) 更にcを押し、最後にgを押してMakefileを生成させる。最後にコンパイルする。例えば、5並列でコンパイルしたい場合は、以下のコマンドを用いる。
% make -j5
Ubuntu 14.04でChoreonoid1.4をコンパイルする際は、choreonoid/src/Util/Reference.hの以下の2行をコメント
//#include <boost/atomic.hpp>
//#define CNOID_REFERENCED_USE_ATOMIC_COUNTER
% sudo apt-get instlall p7zip
Ubuntu16.04、Choreonoid1.5、Qt5の環境で、Choreonoid上のシーンタブでダブルクリックをしてもオブジェクトを選択できない現象が起こりました。
以下の方法で解決ができます。
まず、シーンバーの設定ダイアログを開きます。(下図中赤枠)
「ピッキングにOpenGLピクセルバッファを使う」のチェックをはずし、「了解」ボタンを押します。
以上
windows10 64bitへのchorenoid1.5とgraspPluginのインストール方法を説明する
https://www.visualstudio.com/ja/vs/visual-studio-express/
ページ下へ移動し、"Express for Desktop"をクリック。
ダウンロードしたファイルを実行し、インストールを開始。
https://cmake.org/download/
ページ中央部のLatest ReleaseのWindows win64-x64 Installerのリンクをクリックしダウンロード。
ダウンロードしたファイルを実行し、インストールを開始。
インストール中に以下の画面が表示されるので"Add CMake to the system PATH for all user" もしくは"Add CMake to the system PATH for the current user"
を選択する。
https://sourceforge.net/projects/boost/files/boost-binaries/
インストール対象のバージョンをクリック。
choreonoidをビルドするVisualStudioのバージョンとビット数が一致するファイルのリンクをクリック。
例えばboostのバージョンが1.63.0、VisualStudioのバージョンが2015で64bitならば
boost_1_63_0-msvc-14.0-64.exe
を選択する。
ダウンロードしたファイルを実行し、インストール。
インストール先はバージョン1.63.0では、デフォルトでC:\local\boost_1_63_0となる。
※環境変数の設定方法は
1)画面左下のwindowsアイコンを右クリック。メニューが表示されます。
2)システムをクリック。システムウィンドウが表示されます。
3)左側のシステム詳細設定をクリック。システムプロパティウィンドウが表示されます。
4)環境変数ボタンをクリック。あとは新規作成する場合は新規ボタンを、既存の変数を編集する場合は変更ボタンを押して設定します。
https://www.qt.io/download-open-source/
ページ中のDownload Nowをクリックし、インストーラをダウンロード。
http://eigen.tuxfamily.org/index.php?title=Main_Page
対象バージョンのzipファイルをダウンロード。
ダウンロードしたファイルを解凍。
生成されたフォルダをeigen-"バージョン"(バージョン3.2.10ではeigen-3.2.10)に名前を変更し、C:\localの下へ移動。
http://choreonoid.org/ja/download.html
ソースパッケージ(choreonoid-1.5.0.zip)をダウンロード。
ダウロードしたファイルを適当なフォルダに解凍。以降、C:\localの下に解凍したと仮定し説明します。
Choreonoidはデフォルトで、計算速度を速くするようにビルドされる。
しかし計算精度が落ちるため、干渉判定に誤差が生じる。
この問題を解決するにはC:\local\choreonoid-1.5.0\CMakeLists.txtを編集する。
CMakeLists.txt内の
/fp:fast
を
/fp:precise
へ置換し、保存する。
スタートメニューからCMake GUIを起動。
1)"Where is the source code:"と"Where to build the binaries:"にchoreonoidのフォルダ(C:\local\choreonoid-1.5.0)を設定。
2)Configureをクリック
3)"Specify the generator for this project"に"Visual Studio 14 2015 Win64"を選択し、Finishをクリック。
4)"EIGEN_DIR"にEigenフォルダ(C:\local\eigen-3.2.10)を指定し、Configureをクリック。
5)"CMAKE_INSTALL_PREFIX"をchoreonoidをchoreonoidのフォルダ(C:\local\choreonoid-1.5.0)に変更し、Configureをクリック。
6)Generateをクリックしプロジェクトファイルを生成。C:\local\choreonoid-1.5.0内にChoreonoid.slnが生成されます。
7)CMakeを閉じます
1)C:\local\choreonoid-1.5.0¥Choreonoid.slnを開きます。Visual Studioが起動します。
2)画面上部の、ソリューションの構成を設定します。Choreonoidを使用するだけならReleaseを、開発を行う場合はDebugを選択します。
3)画面上部メニューから[ビルド]->[ソリューションのビルド]でビルドを実行します。
4)ビルドが成功したら、右側のソリューションエクスプローラにある"INSTALL"を右クリックします。
ポップアップメニューが表示されるので、その中にある"ビルド"をクリックします。
choreonoidがC:\local\choreonoid-1.5.0\binにインストールされます。
(構成をReleaseでビルドした場合はchoreonoid.exe、構成をDebugでビルドした場合はchoreonoid-debug.exeが実行ファイルとなります。)
http://www.qhull.org/download/
Download: Qhull 2015.2 for Windows 10, 8, 7, XP, and NT
をクリックし、ソースをダウンロードし、適当なフォルダで解凍。
7)Configureをクリックし、Generateをクリック。"解凍先フォルダ"にqhull.slnが生成されます。
8)CMakeを終了
"解凍先フォルダ"にqhull.slnを開きます。
1.2 ChoreoniodのビルドのVisualStudioによるビルドと同様の手順でビルドとインストールを行う。
※choreonoidをDebugでビルドするときはQhullもDebugバージョンが必要になるので、ソリューション構成をDebugにしてDebugバージョンもビルド・インストールを行います。
http://freeglut.sourceforge.net/index.php#download
Freeglut 3.0.0をダウンロードし、適当なフォルダで解凍(tar.gz形式を解凍できるソフトが必要です)。
1)CMake GUIを起動
2)Where is the source codeとWhere to build the binariesに"解凍先フォルダ"を指定
3)Configureをクリック。
4)MAKE_INSTALL_PREFIXに適当なフォルダを選択。この先C:\local\freeglutを指定したとして説明を進めます。
5)Configureをクリックし、Generateをクリック。"解凍先フォルダ"にfreeglut.slnが生成されます。
"解凍先フォルダ"にfreeglut.slnを開きます。
1.2 ChoreoniodのビルドのVisualStudioによるビルドと同様の手順でビルドとインストールを行う。
※choreonoidをDebugでビルドするときはソリューション構成をDebugにしてfreeglutのDebugバージョンもビルド・インストールを行います。
https://www.python.org/downloads/windows/
Latest Python2 Releaseをクリック。
Windows x86-64 MSI installerをクリックしてダウンロード。
ダウンロードしたファイルを実行しインストール。
途中で以下の画面になるので、Add python.exe to Pathをインストールするに変更します。
https://mingw-w64.org/doku.php/download
ページ中段のMingw-buildsのリンクをクリック。
Sourceforgeをクリックするとダウンロードが始まります。
ダウンロードしたファイルを実行し、インストールを開始します。
途中のSettingsダイアログでArchitectureをx86_64に変更する。
MinGWはC:\Program Files\mingw-w64\x86_64-6.3.0-posix-seh-rt_v5-rev1
にインストールされます(x86_64-6.3.0-posix-seh-rt_v5-rev1の部分はインストールするバージョンにより変わります。)
PATHに、C:\Program Files\mingw-w64\x86_64-6.3.0-posix-seh-rt_v5-rev1\mingw64\binを追加します。
(x86_64-6.3.0-posix-seh-rt_v5-rev1の部分はインストールするバージョンにより変わりますので適宜変更してください。)
http://www.netlib.org/lapack/#_previous_release
lapack-3.5.0.tgzをクリックしダウンロードし解凍。(3.7.0はWindowsではビルドができませんでした。)
1)CMake GUIを起動
2)Where is the source codeとWhere to build the binariesに"解凍先フォルダ"を指定
3)Configureをクリック。
4)"Specify the generator for this project"に"MinGW Makefile"を選択し、Finishをクリック。
5)BUILD_SHAREDをチェック
6)CMAKE_GNUtoMSをチェック
7)CMAKE_INSTALL_PREFIXをC:\local\LAPACKに変更
8)Configureをクリック。
9)VCVARSAMD64にC:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\bin\x86_amd64\vcvarsx86_amd64.batを設定。
10)Configureをクリックし、Generateをクリック
11)CMakeを閉じます。
> cd "解凍先フォルダのパス" > mingw32-make.exe -j8 # -jのあとの数字はビルド時の並列数です。適宜変更してください。 > mingw32-make.exe install
Windowsキー+xを押すと左下にメニューが表示される。プログラムと機能をクリック
インストールされているプロラムが表示されるので、その中からMicrosoft Visual C++ 2008 Redistributeable を探す。
見つからない場合は以下の手順でインストールを行う。
https://www.microsoft.com/ja-jp/download/details.aspx?id=26368
ページ中のダウンロードをクリック
ダウンロードするプログラムの選択画面が表示されるので、vcredist_x86.exeとvcredist_x64.exeにチェックを入れ、
次へを押すとダウンロードが開始される。
ダウンロードしたファイルそれぞれを実行し、インストールを行う。
http://unanancyowen.com/pcl18/
PCL 1.8.0 All-in-one Installer MSVC2015 x64をクリックしダウンロード
ダウンロードしたファイルを解凍し、インストーラを実行
途中で環境変数の設定をきかれるのでAdd PCL to the system PATH for all users もしくは Add PCL to the system PATH for current userにチェックする。
RobotInterfaceプラグインを使用する場合にインストールが必要となります。
http://www.openrtm.org/openrtm/ja/node/6034
OpenRTM-aist-1.1.2-RELESE_x86_64.msiをクリックしダウンロード
ダウンロードしたファイルを実行
リポジトリからgraspPluginを入手する。
※入手にはGitをインストールしている必要があります。
コマンドプロンプトを起動
以下コマンドChoreonoidのフォルダへ移動し、graspPluginを取得
> cd C:\local\choreonoid\ext > git clone http://spica.ms.t.kanazawa-u.ac.jp/gitlab/tsuji/grasp-plugin.git graspPlugin
> cd C:\local\choreonoid > mklink /D extplugin ext
途中まで1.2 のchoreonoidのビルドと同じ手順を実行します。
1)CMake GUIを起動。
2)Where is the source code:とWhere to build the binaries:(C:\local\choreonoid-1.5.0を選択。
3)Configureをクリック。
4)GRASP_PLUGINSに使用するgraspPluginを指定(例:Grasp;PRM;)
5)Configureをクリック。
6)QHULL_DIRにQhullのインストール先(C:\local\qhull)を指定。
7)FREEGLUT_DIRにfreeglutのインストール先(C:\local\freeglut)を指定。
8)Configureをクリック。
9)HIRO/Pluginを使用する場合は、LAPACKE_TOP_DIRにLAPACKのインストール先(C:\local\LAPACK)を指定し、Configureをクリック。
10)Generateをクリック。
11)ソリューションファイルが更新されるのであとは1.2 のchoreonoidのビルドと同じ手順でVisualStudioでビルドを行う。
プラグイン名:CnoidRos
依存プラグイン:Grasp
PRMやMotionFileで生成した動作をROSで通信する。
ROSノードはpythonスクリプトで書かれており、CnoidRosプラグインとPythonスクリプトはメモリマップドファイルでプロセス間通信を行う。
以下のフォーマットで通信する。
(命令1);(命令2);.......(命令n)
命令は以下の通り
goInitial()
goOff()
joints[0,0,0,....0]:1.0 (関節角を設定.[]内は関節角列を:のあとは動作時間をあらわしている)
rhandClose
lhandClose
rhandOpen
lhandOpen
$ sudo apt-get update && sudo apt-get install ros-indigo-rtmros-nextage ros-indigo-rtmros-hironx
$ mkdir -p ~/catkin_ws/src $ cd ~/catkin_ws/src $ catkin_init_workspace $ cd ~/catkin_ws/ $ catkin_make
$ source devel/setup.bash
$ catkin_create_pkg CnoidRos std_msgs rospy
$ cp "graspPluginのパス"/CnoidRos/ROS/CnoidRos.py ~/catkin_ws/src/CnoidRos/src/
$ chmod a+x ~/catkin_ws/src/CnoidRos/src/CnoidRos.py
1.choreonoidを起動します。
2.ターミナルを起動しroscoreを実行
$ roscore
$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch
$ source ~/catkin_ws_devel/setup.bash $ rosrun CnoidRos CnoidRos.py
・Ubuntuマシン
-ROS2のインストール
https://github.com/ros2/ros2/wiki/Linux-Install-Binary を参考にインストールを行います。
-CnoidRosTalker.pyのセットアップ
CnoidRosTalker.pyをROS2ディレクトリにコピーします。ROS2が~/ros2_installにインストールされたと仮定します。
$ cp "graspPluginのパス"/CnoidRos/ROS/CnoidRosTalker.py ~/ros2_install/ros2-linux/bin/ $ chmod a+x ~/ros2_install/ros2-linux/bin/CnoidRosTalker.py
※ROS2インストール時にPython3がインストールされます。python2系が既にインストールされていても、デフォルトでpython3が使用されるようになります。もしデフォルトでpython2系を使用したい場合は環境変数のPATH中のPython3のパスの後にPython2のパスがあるので、Python2のパス,Python3のパス順になるように編集してください。そしてpython3でスクリプトを処理するときは
> python スクリプトファイル名.py
> py -3 スクリプトファイル名.py
-CnoidRosListener.pyのセットアップ
CnoidRos/ROS下のCnoidRosListener.py, command_dispatcher.py, nextage_manipulator.pyをC:\dev\ros2\install\binフォルダにコピーします。
・Ubuntuマシン
以下の手順でCnoidRosTalkerを起動します。
ターミナルを起動します
$ . ros2_install/ros2-linux/local_setup.bash $ CnoidRosTalker.py
> cd C:\dev\ros2\install > local_setup.bat > python bin\CnoidRosListener.py
・pythonスクリプトについて
CnoidRosバーのボタンをクリックするとそれに応じてWindowsにメッセージが表示される。Windows側での表示処理はnextage_manipulator.pyに記述されている。
このファイルを書き換えることで処理内容を変更できる。
CnoidRosバーのボタンが押されたときに呼ばれるnextage_manipulator.pyの関数を以下に示す。
[Home] -> goInitial()
[OffPose]->goOffPose()
[Move]->動作に応じてrhandOpen(),rhandClose(),lhandOpen(),lhandClose(),setJointAngles(angle,time)が呼ばれる。
プラグイン名:ConstraintIK
依存プラグイン:Grasp;GeometryHandler
・障害物を避ける、関節角度制約付きのIK。参考文献[1]の3章を実装。
・このプラグインの実装にはEQuadProg++[2]を使用しています。
・障害物は楕円体で近似します。この情報はyamlファイルに記述します。
・ロボットの楕円体と環境の(SetEnvした)オブジェクトの楕円体が衝突しないように制約をかけIKを解きます。
・カメラを設定してあると、アームとカメラを同時にIKを解きます。
- SaveObstacleShape
対象物の楕円体情報を保存する
- DisplayObstacleShape
楕円体を表示
- HideObstacleShape
楕円体表示のクリア
-SetCamera
IKを解く時のカメラを選択する
#include "../ConstraintIK/ConstraintIK.h
Vector3 p; // 目標位置 Matrix3 R; // 目標姿勢 // Solverのインスタンスを作成。この時対象となるアームを設定する。 ConstraintIKSolver* cik = new ConstraintIKSolver(PlanBase::instance()->targetArmFinger); // パラメータの設定(必要な場合) cik->setStep(10); //現在地から目標付近まで何分割して解くか(デフォルト20) cik->setMaxIteration(10); //目標付近での微調整の最大回数(デフォルト10) cik->setFixJoint(0); //固定したい関節を設定します(JointNumを指定 ) cik->setTranslationalJointCoeff(10) //並進関節が回転関節に比べどれくらい動きづらくするか設定します if(!cik->ik(p,R)){ cerr << "ik failed" << endl; } delete cik;
choreonoidから、オブジェクトに障害物となる楕円体を設定する方法を説明します。(ロボットについては下記詳細な説明を参考に手動で設定してください。)
1.対象オブジェクトをSetObjectします。
2.ConstraintIKバーのSaveObstacleShapeをクリックします。
3.ダイアログが表示されるので、分割パラメータ、近似方法選択します。
Clusteringが分割パラメータで、
・Number of Clustersを選択すると指定したクラスタ数に分割された後、楕円体が配置されます。
・Overlap volume ratioを選択するとクラスタ同士の重なりが指定した割合になるように分割された後、楕円体が配置されます。
Approximation typeが楕円体の配置方法で、
・boundingboxを選択するとクラスタのバウンディングボックスの辺に沿うように楕円体が配置されます。
・ellipsoidを選択するとクラスタを2次曲面で近似した時の楕円体が配置されます。(クラスタを2次曲面近似した結果,楕円体とならない場合は表示されません。)
4.showボタンをクリックすると楕円体が表示されます。もし、違う結果が見たい場合はパラメータを変更して再度showボタンをクリックします。
5.設定する楕円体が決定したらwriteボタンをクリックします。
6.[対象物名].yamlが作成されます。yamlファイルの最初の行を確認し、対象のhrpファイルパスと違う場合は修正してください。
「3.1.2.yamlファイル形式」を参考にし、手動でロボットのyamlファイルに楕円体情報を追加してください。
yamlファイルのサンプルを示します。
modelFile: ./boxL1Hrp.wrl collisionShape: - center: [0.2325 , 0 , 0.095] rpy: [0 , -0 , 0] length: [0.005 , 0.1475 , 0.005] security_distance: 0.01 effective_distance: 0.2 xi: 0.1
下記のような記述をロボットのyamlファイルに追加します。
collisionShape: - jointName: RARM_JOINT2 center: [-0.025, -0.015, -0.1] rpy: [0, 0, 0] length: [0.05, 0.05, 0.125] - jointName: RARM_JOINT1 center: [-0.025, -0.11, -0.14] rpy: [0, 0, 0] length: [0.05, 0.05, 0.12]
カメラ情報は下記のような記述をロボットのyamlファイルに追加します。
camera: - name: HEADCAMERA type: HEAD base: WAIST cameraLink: HEAD_JOINT1 direction: [1.0, 0.0, 0.0] pos: [0.09, 0.0, 0.08] - name: HANDCAMERA type: HAND base: WAIST cameraLink: LARM_JOINT5 direction: [-1.0, 0.0, 0.0] focal_distance: 0.5 pos: [-0.02, 0.0, -0.0]
目標位置・姿勢まで20分割して文献[1]のIKを使用して解いていきます。
このままでは、目標位置・姿勢までとの誤差が大きいことがあります。
そのため微調整のため、文献[1]のIKを誤差が十分小さくなるまで繰り返し解きます。
choreonoidを起動します。
extplugin/graspPlugin/ConstraintIK/project/box3.yamlを読み込みます(「ファイル」->「読み込み」->「OpenHRPモデルファイル」)。
アイテムタブのbox3を選択し、Plannerバーの「SetEnv」をクリックします。
ConstraintIKバーの「DisplayObstacleShape」を押し、4.1で設定した楕円体が表示されることを確認します。
ConstraintIKバーの「HideObstacleShape」を押し、楕円体が消えることを確認します。
choreonoidを終了します。
[1] 金広 他,"実機の物理的制約を考慮した即応的脚動作生成手法," 日本ロボット学会論文誌, vol. 28, no. 10, pp. 1251-1261, 2010.
[2] EQuadProg++, http://www.cs.cmu.edu/~bstephe1/eiquadprog.hpp.
プラグイン名: GeometryHandler
依存プラグイン: Grasp
PickAndPlacePlanner(旧GripperManipulation)プラグインで実装されている把持計画において各対象物に対して必要なパラメータファイルを生成する方法を説明します。
1) graspPlugin/GeometryHandler/project/finger.cnoidをプロジェクトファイルとしてchoreonoidを起動します。
2) 対象物のVRMLモデルを選択し、setObjectをします。ここで、対象物の位置・姿勢(RPY)は全て0にしてください。
3) 対象物の把持させたい部分にカーソルを置いた状態でコントロールキーを押しながらマウスを左クリックします。すると、対象物の把持させたい場所に矢印が立ちます。
4) 対象物にカーソルが置いてある状態でFキーを押します。この操作でクラスタの部分が他の部分と色が変わります。
5) 対象物の把持させたい場所を変えながら、3) 4)を繰り返します。
6) 対象物にカーソルが置いてある状態でGキーを押します。これにより、yaml形式のパラメータファイルがchoreonoidを実行したディレクトリに生成されますので、これを適当なディレクトリ(例えばgraspPlugin/PickAndPlacePlanner/PRMなど)に置きます。
7) 把持計画の際、プロジェクトファイルでは、vrmlを直接指定するのではなく、このyamlファイルを指定します。
8) なお、論文中Algorithm 1のStep1におけるクラスタに含まれる三角形の数を増やすにはHキー、減らすにはIキーを用います。またインテンションパラメータはデフォルトでは1ですが、これを増やすにはJキーを使ってください。
アルゴリズムは参考文献[1]を参考にしてください。
ObjectPlacePlannerプラグインで実装されている対象物の配置計画において、対象物と環境の両方に対して必要なパラメータファイルを生成する方法を説明します。
対象物と環境のポリゴンモデルに対して以下の操作を実行します。
1) graspPlugin/GeometryHandler/project/finger.cnoidをプロジェクトファイルとしてchoreonoidを起動します。
2) VRMLモデルを選択し、setObjectをします。
4) モデルにカーソルが置いてある状態でKキーを押します。これにより、VRMLモデルの表面がクラスタリングされ、クラスタが色付けられた状態で表示されます。
5) モデル上で対象物と環境を接触させたい位置にカーソルを持っていき、コントロールキーを押した状態でマウスを左クリックします。これによりモデル上に矢印が立ちます。
6) モデルにカーソルがある状態でJキーを押します。これにより、5)で指定した位置が記憶されます。
7) 5)6)の操作を繰り返します。
8) モデルにカーソルがある状態でGキーを押します。これによりchoreonoidを実行したディレクトリにパラメータファイルが生成されます。
9) 環境モデルの場合、生成されたファイルをそのままgraspPlugin/PickAndPlacePlanner/PRMなどのディレクトリに置きます。対象物モデルの場合、1で生成されたパラメータファイルに対して2で生成された内容をマージします。このマージは現在は手作業で行っています。
アルゴリズムは参考文献[3]を参考にしてください。
なお、パラメータにはクラスタの境界を定義しているものがあります。これは、ポリゴンモデルに破れが無く、全ての三角形が左回りに定義されている必要があります。これが満たされていないポリゴンモデルを使う場合は、GeometryHandle.hにおいて、HEURISTICS_FOR_PCLの定義のコメントを活かしてください。
実行コマンド
% ~/src/Choreonoid$ bin/choreonoid extplugin/graspPlugin/GeometryHandler/project/assemble.cnoid
特に、Lを押すと対象物がクラスタ毎に違った色で表示される。なお、上記コマンドは、GeometryHandlerPlugin.cppに書いてある。
chorenoid1.4までは対象物の上でキーを押して操作を行っていたものが、chorenoid1.5からは対象物上で右クリックをしポップアップメニューを表示し、その中のGeometryのメニューを選択し、操作するように変更しました。
chorenoid1.4までのキーとchorenoid1.5のメニューの対応関係を以下に示します。
[X]:LimitedClustering
[B]:CreateBoundaryData
[C]:BinaryClustering
[V]:CreateDepartData
[S]:StepClustering
[R]:Rest
[M]:Quadrical Clustering
[D]:Plan Clustering(old)
[F]:Find Parallel Plane Pair
[G]:Output Parallel Plane Pair
[H]:Increase Seed Size
[I]:Decrease Seed Size
[J]:Set Intention Parameter
[K]:Plane Clustering (new)
[L]:Plane Clustering for Assembly
[N]:Show Contact Locus
[O]: Generate Assembly Graph
[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 et al., "Object Placement Planner for Robotic Pick and Place Tasks", Proc. of IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS '12), 2012
GraspプラグインはgraspPlugin for Choreonoidの中で最も基礎となるプラグインです。本プラグインが提供する機能は多指ハンドに対する把持計画と、他のプラグインが共通に利用する関数です。
「=Planner=」というタイトルを持つプランナーバーを使って、把持計画を作成します。
詳細は プランナーバーヘルプ をご参照ください。
Grasp プラグインは、把持や設置のために適切なグリッパの位置を求めるもので、アーム全体をどのように動かすかについては考慮されていません。周囲の環境に干渉しないアームの動作計画を行うには、PRM プラグインを用います。PRM プラグイン解説をご覧ください。
本プラグインでは様々な種類のロボットに対応できるように、ハンドの把持領域と対象物の両方にバウンディングボックスモデルを仮定した把持計画手法を採用しています。本手法を用いることで、例えば多指ハンドであれば3指の指先把持、4指の指先把持、あるいは包み込み把持など様々な把持形態を実現することができます。
ここで、ハンドの把持領域のバウンディングボックスをGrasping Rectangular Convex (GRC)、対象物のバウンディングボックスをObect Convex Polygon (OCP)と呼び、これらの幾何学的関係により大まかな手首の位置姿勢を求め、その上でランダムサンプリングにより最終的な把持姿勢を求めます。
また、把持計画を高速化するために、摩擦円錐の楕円近似を用いたForce Closureの判定手法を用いています。ただし、Force Closure判定の従来手法もプラグインの関数として用意しています。アルゴリズムの詳細については下記リファレンスを参照してください.
[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.
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を解くプログラムを格納するディレクトリ名
(2)PRMファイルの編集
次に、上記yamlファイルの中にあるprehensionListには、用いる把持形態(3指指先把握、4指指先把握、包み込み把握など)に関するパラメータを記述するファイル(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モデルの場合、第0指第1指共に2番目のリンクのみが対称物と接触します。
Finger[i]Comp:第i指の第jリンクを対称物に接触させる場合どの関節を使ってリンクの位置を補償するかを定義します。HRP2JRLモデルの場合、全てについて第0関節を使います。
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のパラメータの値を小さくしてみてください。
把持動作計画モジュールのコンシューマであるGraspConsumerプラグイン の操作について解説します。
知能化プロジェクトで把持動作計画モジュールを公開しています。これは把持計画を行うRTコンポーネントなのですが、実際には対象物座標系から見た手首位置・姿勢の集合をデータファイルとして置いておき、視覚センサなどから対象物の位置・姿勢が与えられた時にデータファイルを探索することで手首の位置・姿勢を求めています。GraspConsumer プラグインは、Grasp プラグインと同様に把持計画を行うプラグインですが、把持動作計画モジュールのコンシューマとして実装しています。
GraspRTCバーは、ツールバーの以下の部分です。
各ボタンの機能については、GraspRTCバーヘルプ をご参照ください。
GraspConsumer プラグインによる把持動作計画チュートリアル をごらんください。
プラグイン名:GraspDataGen
依存プラグイン:Grasp;GeometryHandler;SoftFingerStability;ConstraintIK;PCL
・ロボットハンドで対象物を把持可能な姿勢・位置のデータを生成します。
・箱に入っている対象物を掴むのに最適なロボットの腰位置探索を行います。
- GenerateGraspPattern
対象物体の把持データベースを作成する
- AppendGraspPattern
把持データベースに手動で把持姿勢を追加する
- DisplayBoundingBox
指定されたクラスタ数に分割したときのバウンディングボックスを表示する
- SearchHandScale
ハンドサイズを変更して把持データベースを作成する
- Grasp
把持データベース中の指定した把持姿勢を確認・修正を行う
- SearchWaistPos
箱の中の把持対象物を掴むのに最適なロボット位置を探索します。
- DisplayWaistPos
SearchWaistPosの結果を表示します。
- WaistPosMotion
SearchWaistPosのロボットの移動モーションを生成します(実装中)。
- SetCollectionBox
WaistPosMotionでモーションを生成する際、ロボットと一緒に動く箱を設定します。
- SetCollectionObj
WaistPosMotionでモーションを生成する際、ロボットと一緒に動くオブジェクトを設定します。
- ClearCollection
SetCollectionBox,SetCollectionObjでの設定をクリアします。
- Param
把持計画パラメータを設定します。
1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押す。
2.アイテムタブから対象ロボットを選択し、PlannerバーのSetRobotボタンを押す。
ハンドとロボット本体が分かれたモデルの場合は、PlannerバーのAttachHandで対象ハンドをアタッチする。
3.DataGenBarのGenerateGraspPatternボタンを押すとパラメータを設定するダイアログが表示されます。
4.パラメータを設定しOKボタンを押すと把持可能な姿勢・位置を探索し始めます。
number of clusters:指定したクラスタ数に分割し、把持データベースを作成します。
overlap volume ratio:クラスタ同士の重なりが指定した割合になるように分割し、把持データベースを作成します。
scale: 対象オブジェクトが元の何倍であるか指定します(通常は1.00を指定します)
distance between search point:探索ポイント数(探索する点の間隔)
5.メッセージウィンドウに"GraspPattern generating is finished"が表示されたら終了です。
extplugin/graspPlugin/RobotModels/"対象ロボット名"/"把持した腕のデータパス"/preplanning_"対象物名".txtに把持可能な姿勢・位置が保存されます。
ここで"把持した腕のデータパス"は対象ロボットのyamlファイル中のdataFilePathの値です。設定されていない場合はdataとなります。
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ボタンをクリックすると把持データベースに把持姿勢が追加されます。
1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押す。
2.DataGenBarのDisplayBoundingBoxボタンを押すとパラメータを設定するダイアログが表示されます。
3.パラメータを設定しOKボタンを押すとバウンディングボックスが表示されます。
パラメータの意味は2.2.1の4と同じです。
show only target clusterをチェックすると把持データベース作成の対象となるクラスタのみ表示されます。
この機能は対象ロボットモデルが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」が生成されます。
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.把持データベースへの把持姿勢追加の5と同様です。
ロボット腰位置探索を参照してください。
把持計画パラメータ設定方法は把持計画パラメータ設定方法に記述してあります。
本機能を使用するにはDebugModeをONにする必要があります。
DebugModeをONにするにはGraspDataGenerator.cppに#define OUTPUT_RESULTを追加し、GraspDataGenBar.cppに#define DEBUG_MODEを追加します。
また、事前にDebugModeをONにした状態で把持データベース作成を行い接触点情報ファイルを作成する必要があります。
1.アイテムタブから把持対象物を選択し、PlannerバーのSetObjectボタンを押します。
2.DataGenBarのDisplayContactPointボタンを押すと把持データベースの接触点が表示されます。
ばねなどのモデルでは、領域をきれいに分割することが難しいことがあります。ばねを全体を1つの領域とし、その領域内をすべて探索することで把持点を得ることができます。
変更方法はGraspDataGenerator.cpp:28行目
//#define NOCHECK_BOUNDINBOXSIZE
のコメントをはずします。ただし、領域の大きさによっては計算時間が増大するので注意が必要となります。
ポリゴンが時計回り、反時計回りが混在しているとき法線ベクトルがばらばらになります。このとき、把持安定性が正しく求まりません。
GraspDataGenerator.cpp:29行目
//#define FLIP_NORMAL
のコメントをはずすことで、対処療法的ですが対応できます。
デフォルトでは、データベース生成終了時に把持時の接触点が描画されます。
変更したい場合は以下をプリプロセッサ定義してください。
・接触点をクラスタ直方体に射影したものを描画したい場合:
DRAW_CONTACTPOINT_ON_BOX
・制御点(アプローチ点)をクラスタ直方体に射影したものを描画したい場合:
DRAW_APPROACHPOINT
-把持データベース
ForceClosure値の高い順に以下のデータが書き込まれる。
ForceClosure値 対象物から見たハンドの姿勢 対象物から見たハンドの位置 指の関節角度
choreonoidを起動します。
extplugin/graspPlugin/GraspDataGen/project/displayBoundingBox.cnoidを読み込みます(「ファイル」->「プロジェクトの読み込み」)。
DataGenバーの「DisplayBoundingBox」ボタンをクリックすると下記ダイアログが表示されますのでOKを押します。
下図のようにバウンディングボックスが表示されます。
choreonoidを終了します。
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
次に把持姿勢を表示します。
DataGenバーの「Grasp」ボタンをクリックすると下記ダイアログが表示されるのでOKボタンを押します。
下記画像のようにmug2を把持する姿勢に変化することを確認してください。
ダイアログの「target number」を変化させ、異なる把持姿勢になることを確認してください。
choreonoidを終了します。
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を終了します。
まず、動作確認用にソースを変更しビルドしなおします。
CCMake時にUSE_DB_IN_PICKANDPLACEをチェックしておいてください。
extplugin/graspPlugin/PickAndPlacePlanner/ManipController.hの12行目
//#define OBJ_ENV_CONTACT
//#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を終了します。
ロボット腰位置探索を参照してください。
以下は古い情報です。
choreonoidを起動します。
extplugin/graspPlugin/GraspDataGen/project/search_waist_pos_ahiru.cnoidを読み込みます(「ファイル」->「プロジェクトの読み込み」)。
GraspDataバーの「SearchWaistPos」をクリックします。下記ダイアログが表示されるので「half the width of box」に0.19を「half the depth of box」に0.13を選択し、OKボタンを押します。
メッセージウィンドウに"success!"と表示され、ロボットと箱とが干渉状態ではなければ成功です。
choreonoidを終了します。
ここではMotionFileで使えるコマンドそれぞれについてまとめる。
左手用コマンドはRARM, RHANDをLARM, LHANDにする
※時刻の単位はsec, 位置の単位はm, 姿勢の単位はdegree
ロボットのVRMLファイルで定義したジョイント達の、どこが腕や手にあたるかをYAMLファイルで記述する。
0 HEAD_FORWARD
1 PAUSE
動作内容)
ロボットの全関節角を指定した角度にする。
書式)開始時刻 終了時刻 JOINT_ABS (ロボットの関節数分の関節角度(degree))
例)
0 1 JOINT_ABS 0 10 20 5 20 -145 30 0 -15 0 0 -115 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
動作内容)
全関節角を現在の関節角から指定した角度動かす。
書式)開始時刻 終了時刻 JOINT_REL (ロボットの関節数分の関節角度の増分(degree))
例)
1 2 JOINT_REL 0 10 10 5 -60 30 30 15 30 10 10 45 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
動作内容)
手先を指定した位置・姿勢(world座標)に動かす。
書式)開始時刻 終了時刻 RARM_XYZ_ABS x座標 y座標 z座標 roll pitch yaw
例)
0 1 LARM_XYZ_ABS -0.12 0.3 0.274 145 -60 -170
動作内容)
手先を現在の位置・姿勢から指定した距離・角度分動かす。
書式)開始時刻 終了時刻 RARM_XYZ_REL x座標の増分 y座標の増分 z座標の増分 rollの増分 pitchの増分 yawの増分
例)
1 2 LARM_XYZ_REL 0.1 -0.05 0.05 90 0 0
動作内容)
手先を指定した位置・姿勢(world座標)に動かす。
ARM_XYZ_ABSとの違いは、所要時間と目標位置までの線分を分割数(最後のパラメータ)等分し、各区間ごとにARM_XYZ_ABSの操作を行う。
ARM_XYZ_ABSでは始点と終点の関節角の間を配位空間で補間するため、実空間での軌道は直線にならない。
これが影響する場合に細かく分割して軌道を直線に近づける。
書式)開始時刻 終了時刻 RARM_LINEAR_ABS x座標 y座標 z座標 roll pitch yaw 分割数
例)
0 1 LARM_LINEAR_ABS -0.15 -0.23 0.7 0 10 17 100
1 2 LARM_LINEAR_REL 0 0.30 0 0 0 0 100
動作内容)
腕の関節角を指定した角度にする。
最初のパラメータはIKを解くベースリンクの角度。指定したパラメータの数が不足しているとき、手先の方の関節角は0になる。
書式)開始時刻 終了時刻 RARM_JNT_ABS (右腕の関節数分の関節角度(degree))
例)
0 1 LARM_JNT_ABS 0 30 -45 -90 90 0 135
動作内容)
腕の関節角を現在の関節角から指定した角度動かす。
書式)開始時刻 終了時刻 RARM_JNT_REL (右腕の関節数分の関節角度の増分(degree))
例)
1 2 LARM_JNT_REL -30 15 -15 -30 -45 -30 -45
動作内容)
手の関節角を指定した角度にする。
書式)開始時刻 終了時刻 RHAND_JNT_ABS (右手の関節数分の関節角度(degree))
例)
0 1 LHAND_JNT_ABS 85 45 -85 -45
動作内容)
手の関節角を現在の関節角から指定した角度動かす。
書式)開始時刻 終了時刻 RHAND_JNT_REL (右手の関節数分の関節角度の増分(degree))
例)
1 2 LHAND_JNT_REL -50 -30 50 30
0 1 RARM_OBJECT 0 0 0 0 0 0
0 1 RHAND_JNT_OPEN
0 1 RHAND_JNT_CLOSE
動作内容)
[要確認]
書式)開始時刻 終了時刻 RHAND_GRASP # 値の指定なし
例)
0 1 RHAND_GRASP
プラグイン名:MotionFile
依存プラグイン:Grasp,PRM
graspPlugin/MotionFile/data/motion.datに動作コマンドを記述します。
次に、このプラグインを有効にすることでツールバーが現れ、そこにLoadボタンとClearボタンがあります。
Loadボタンを押すとChoreonoid内でロボットが動作コマンドのとおりに動きます。また、RobotInterfaceプラグインを
併用することで、実際のロボットも動作させることができます。なお、個々のコマンドについては、
MotionFileController.cppの中身を見てください。
サンプルの実行方法
graspPlugin/MotionFile/project/3dprinter.cnoidをプロジェクトファイルとしてChoreonoidを起動してください。
3Dプリンタからロボットがupper caseを取り出す動作が生成されます。
ここではchoreonoid 1.5のgraspPlugin でのMotionタブの使用方法を説明する。
ccmakeでGRASP_PLUGINSにMotionFileを追加
choreonoidにMotionタブが追加される:
タブ下方のカウンターで追加する行数を指定し、Appendボタンで追加。
Loadボタンで作成済みのファイルを指定して読み込む。
Loadボタンを押し、読み込むファイルを選択する。
読み込んだファイルの内容が表示される。
コマンドによってフォーマットが異なる。コマンドごとの詳細はMotionFileのコマンドを参照。
行の追加: Appendボタンで指定行数を追加して中身を編集する(末尾のみ。途中に挿入したいときはファイルを直接エディターで編集する)。
行の削除: コマンドをNoneにして保存し、再度ロードすれば消える。
例) 2行目を削除するとき
コマンドをNONEに変更する。
ファイルに保存したときにNONEの行は出力されないので、再読み込みすると表示上からも削除される。
編集時の注意:
・入力ボックスにマウスオーバーした状態でマウスホイールを回すと中の値が変わってしまうので、スクロールするときは何もないところにカーソルを置く。
・座標の絶対/相対の指定は、実際には座標指定のないコマンドとの組み合わせでも指定できてしまうので注意する。
Saveボタンで編集した内容をファイルに保存する。
ファイルをロード済みであればダイアログが出る
Noを選んだ場合は名前を付けて保存する
※もともとロードしたファイルにコメント行があった場合、上書きするとコメントがなくなるので注意。
Playボタンを押すと(その前にsetRobot, setObjectを実施しておく)タブ内の編集内容が関節角に変換されてアイテムに読み込まれる。
ツールバーのPlayメニューを使用した場合は、
choreonoid/ext/graspPlugin/MotionFile/data/motionfile.dat
が存在する場合、その中で指定されたパスのファイルを読み込む。
motionfile.datが存在しない場合は、choreonoid/ext/graspPlugin/MotionFile/data/motion.datが読み込まれる。
マニュアルでMotionファイルを作成する方法についてまとめる。
Motionファイルに動作コマンドを記述し指定時刻での腕の先端の位置姿勢を指定する。
それから動作(フレームごとの関節角)を生成する
手先はIKを計算するリンク
動作の初期姿勢はsetRobotを実行したときのロボットの位置姿勢
Motionファイルを作成
setRobot, setObject
ロボットのyamlファイルにgraspPluginSetting: を記述しておく必要がある
ツールバーのPlayボタン(choreonoid 1.4ではLoad)
プロジェクトファイルでロボットとオブジェクトの配置が決まっている
ロボットが手先に持ったものを動かしたいときはMotionファイルで指定する位置(手先)と
ものの位置が異なることを考慮する
時刻の指定をそのまま反映するには
PRM/TrajectoryPlanner.cpp で
USE_EXACT_TIMEのdefineを有効にする
PCLプラグインはPoint Cloud Libraryを利用してポイントクラウドを表示するプラグインで、PCL.rtcプラグインはrtcpclを利用してOpenRTM経由でポイントクラウドを表示させるプラグインです。
プラグイン名: PCL
依存プラグイン: Grasp, GeometryHandler
プラグインを利用する前に以下のコマンドを実行してください。Ubuntuのバージョンにより異なります。
Ubuntu16.04の場合
$ sudo apt-get install libpcl-dev $ sudo apt-get install libproj-dev
% sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl % sudo apt-get update % sudo apt-get install libpcl-1.7-all-dev
次に、ccmake においてプラグインとしてGraspとGeometryHandlerとPCLを指定してコンパイルしてください。また、ccmake で、一度cを押した後にBUILD_GRASP_PCL_PLUGINというオプションが現れ、デフォルトではOFFになっていますが、これを ONにしてください。ちなみに、ChoreonoidのデフォルトでBUILD_PCL_PLUGINというオプションがありますが、これではありません。
ここで、ccmake でcを押した後、以下のようなメッセージが出ることがあります。
** WARNING ** io features related to pcap will be disabled
RGB: カラーモードのON/OFF
Cap: ポイントクラウドをキャプチャする
Seg: セグメンテーションする
Tri: ポイントクラウドをポリゴンに変換する
Cyl: シリンダーセグメンテーション
MinZ:範囲を限定し、その範囲の中で座標が最少のポイントを求める
Dif: 二つのポイントクラウドの差分を求める
Cube:ポイントクラウドのポイントを立方体のVRMLモデルで置き換える
Grabber On/Off : Cap2ボタンと併用する。このボタンを押すことでポイントクラウドがキャプチャできる状態にし、Cap2ボタンでキャプチャする。(CapボタンはGrabberOn/OffボタンとCap2ボタンを連続して押していることの相当する。)
Cap2: 高速にポイントクラウドをキャプチャする。
最後に、/dev/usb以下にスーパーユーザ以外が常にアクセスできるようにするには、/etc/udev/rules.dに45-xaiox.rulesという名前のファイルを作り、その中に
SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", MODE="0777", GROUP="[your group name]"
プラグイン名: PCL.rtc
依存プラグイン: Grasp, GeometryHandler
本プラグインを利用する前に、以下のコマンドを実行してください。
% cd ~/src/Choreonoid/extplugin/graspPlugin/PCL.rtc % ./install-resuisities-ubuntu.sh
次に、ccmake においてプラグインとしてGraspとPCL.rtcを指定してコンパイルしてください。また、ccmake の際にBUILD_PCL_PLUGIN は ONにしてください。そして、Choreonoidをコンパイルし、Choreonoidを起動してください。PCLバーが追加されていると思います。このバーに含まれるボタンはそれぞれ以下のような機能を持っています。
Cap: ポイントクラウドをキャプチャする
MinZ:範囲を限定し、その範囲の中で座標が最少のポイントを求める
Cube:ポイントクラウドのポイントを立方体のVRMLモデルで置き換える
VRMLモデルからポイントクラウドへの変換は、PCLプラグインのPointCloudUtility.hにある PointCloudUtil::modelToPiontCloudでおこなう。使用方法はPCL/Calibration.cppの414から421行目が参考になる。
ここでは、PCLプラグインの位置・姿勢推定機能について説明する。
PCLプラグインについてはPCL, PCL.rtcプラグインを参照のこと。
CVFH特徴量を用いて位置・姿勢推定を行う場合は,事前に特徴量DBを作成する必要があります.
CVFH特徴量を用いた位置・姿勢推定方法:[inline:ObjectPoseRecognitionAlgorithm20150311.pdf]
セグメンテーションアルゴリズム:[inline:SegmentationAlgorithm.pdf]
PartialCloudGeneratorをビルドしていない場合,以下の手順でビルドを行います.
$ cd (choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/PartialCloudGenerator $ ccmake .
$ make
対象モデルのある視点から見た部分モデルをポイントクラウド化したものを複数個作成します.
まず,モデルデータをply形式に変更します.VRML(wrl)からplyへの変換はmeshlabで行います.
meshlabで対象VRMLファイルを読み込み,"Export Mesh As"でply形式で保存します.
保存したplyファイルを(choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/PartialCloudGeneratorにコピーします.
(以下,保存したplyファイルをmodel.plyとします.)
$ cd (choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/PartialCloudGenerator $ ./partial_cloud_generator -i model.ply -o (保存先ディレクトリ)
部分モデルに対して,CVFH特徴量等を算出します.
(1)Choreonoidを起動します.
(2)ツールバーのPCLバーにある「GenDes」ボタンを押します.
(3)ダイアログが表示されるので,「Partial model dir」の右側の「select...」ボタンを押し、先程作成した部分モデルがあるディレクリを選択します.
(4)作成時のパラメータを変更する場合は,「parameters」をクリックすると変更できるパラメータが表示されるので,これを変更してください.
パラメータの意味は以下の通りです。
sample density:ダウンサンプリング時のvoxel_leafのサイズ
normal radius:特徴量算出に使用する法線を半径raidus_normalで計算を行う
(5)「OK」ボタンを押すと特徴量ファイル等がダイアログで指定したディレクトリ内に作成されます.
boundingbox_X.txt:部分モデルのバウンディングボックス
centroid_X_Y.txt:部分モデルの中心
crh_X_Y.pcd:CRH特徴量
crhext_X_Y.pcd:CRH特徴量(追加分)
cvfh_X_Y.pcd:CVFH特徴量
pcd_X.pcd:ダウンサンプリングした部分モデルポイントクラウド
# VRML(HRP)ファイルを指定します modelFile: modelHRP.wrl # CVFH特徴量ファイルがあるディレクトリを指定します descriptorDir: ./descriptor/model/
(1)Choreonoidを起動します.
(2)推定したいオブジェクトをSetObjectします.
※CVFHを用いた位置・姿勢推定を行う場合は1.4の方法で作成したYAMLファイルを指定してください.
(3)ハンドに取り付けたカメラ等を使用する場合はSetCameraで使用カメラを選択します。
(4)PCLバーの「Est」ボタンを押します.
(5)パラメータを変更する場合は2.2の情報を参考に変更します.
(6)「OK」ボタンを押すと位置・姿勢推定が実行されます.
成功したら対象オブジェクトが移動します.
失敗した場合,対象オブジェクトは原点に移動します.
失敗した場合は,探索範囲の変更,平面除去回数の変更等を試してください.
「Initial alignment method」
初期位置合わせに使用する方法を選択します.
「Capture」
merge with previous captured pointcloudにチェックをいれると,前回キャプチャしたポイントクラウドとマージしたポイントクラウドで位置姿勢認識を行います。
「Regionタブ」
探索範囲を指定します.
"set target region"のチェックを外すとすべての範囲を探索します.
「PCD/Modelタブ」
load scene from:チェックがついている場合,選択したファイルからポイントクラウドを読み込みます.
save scene to:チェックがついている場合,選択したファイルにポイントクラウドを書き込みます.
obj file path:チェックがついている場合,SetObjectしたオブジェクトではなく,選択したファイルについて推定を行います.
チェックをつけた場合,推定されたオブジェクトがchoreonoidの左側アイテムタブに追加されます.
「Showタブ」
推定後の表示を選択します.
scene:入力されたポイントクラウドをそのまま表示します.
segmented scene:セグメンテーション結果を表示します.
clipped scene and move object:推定後のオブジェクト部分を取り除いたポイントクラウドを表示します.
「Parameterタブ」
「Sampling」
sample density:ダウンサンプリング時のvoxel_leafのサイズ
「Segmentation」
dist threthold(plane remove):平面除去時の閾値
number of planes(palne remove):平面除去回数
number of candidate clusters: 平面除去後にできるクラスタに対し、いくつのクラスタを探索対象とするか
「ICP」
iteration:ICPの最大繰り返し回数
「SAC-IAタブ」
線特徴量算出時の近傍点の基準を設定します。Radiusを選択した場合は距離で、K-nearestを選択した場合は点数で決定されます。
normalが法線計算時、featureが特徴量計算時に使用する値です。
「CVFHタブ」
Candidate pose:マッチング時の候補を何個選択するか
「calibrationタブ」
キャリブレーション用です(後述).
[ObjPoseEstimate] ;; 実行結果の表示方法。SCENE,SEGMENT,RESULTのいずれかを指定 show_scene=SEGMENT ;; ポイントクラウドの色表示 show_color=true ;; trueのときpcd_loadfilepathのファイルからポイントクラウド読み込む ;; pcd_loadfilepathはconfig.iniからの相対パスまたは絶対パスで指定 pcd_loadfile=true pcd_loadfilepath=./test_pcd.pcd ;; trueのときpcd_savefilepathファイルへポイントクラウドを書き込む pcd_savefile=false pcd_savefilepath=./test_pcd.pcd ;; trueのときpcd_objfilepahtのオブジェクトを推定対象とする。falseのときはchoreonoid内setobjectしたものを対象とする pcd_loadobj=false pcd_objfilepath=../../hrgPlugin/Samples/CarParts3/8657033040.yaml ;; ダウンサンプリング時のleafサイズ[m] sampling_density=0.0025 ;; 平面除去時のの閾値 segmentation_distth=0.007 ;; 平面除去回数 segmentation_numplane=0 ;; 推定対象クラスタ数 segmentation_searchcluster=10 ;;; セグメンテーション時のパラメータ ;; 使用するアルゴリズム true:LCCP、 false:EuclideanCluseteringベース use_lccp_segmentation=true ;; EuclideanCluseteringベースアルゴリズムのパラメータ ;; 点間距離の視線ベクトルと水平方向成分がsegmentation_tolerance以内かつ ;; 点間距離の視線ベクトルの垂直方向成分がsegmentation_vtolerance以内なら同一クラスタとする ;; リセグメント時の輪郭点からsegmentaion_resegment_radius以内の点を削除する。 ;; リセグメントはクラスタの細い部分で分離するがこの値で分離する細さが変化する ;; segmentation_boundary_radiusは輪郭点を求める時のパラメータ。この値が大きすぎると凹んだところの輪郭点が無視される。 ;; この値が小さすぎると欠落部分のまわりが輪郭点になってしまう。 segmentation_tolerance=0.004 segmentation_vtolerance=0.004 segmentation_resegment_radius=0.01 segmentation_boundary_radius=0.0075 ;; LCCPアルゴリズムのパラメータ ;; supervoxel(http://pointclouds.org/documentation/tutorials/supervoxel_clustering.php#supervoxel-clustering) ;; 作成時のパラメータR_seedに相当 segmentation_lccp_seedsize=0.01 ;; supervoxel作成時のパラメータW_sに相当 segmentation_lccp_spatialcoeff=0.4 ;; supervoxel作成時のパラメータW_nに相当 segmentation_lccp_normalcoeff=1.0 ;; クラスタとなるための最小の点の数 segmentation_lccp_minclustersize=20 ;; 凹みと判断する角度(degree)。この値が小さくすると少しの凹みでも分離される。大きくすると分離されにくくなる。 segmentation_lccp_concavitytolerance=10 ;; SACIAのパラメータ SACIA_use_radius=true SACIA_radius_normal=0.001 SACIA_radius_feature=0.002 SACIA_use_knearest=false SACIA_knearest_normal=10 SACIA_knearest_feature=10 ;; ICP回数 ICP_iteration=15 ;; region_enableがtrueのときポイントクラウドをクリッピングする region_enable=true region_xmax=0.60 region_xmin=0.30 region_ymax=0.0 region_ymin=-0.26 region_zmax=2.0 region_zmin=0.74 ;; CVFHで特徴量が類似している上位CVFH_NN個のポーズに対し認識を行う. CVFH_NN=15
(1)PCL/calibtools/calibmat.txtのキャリブレーション行列を単位行列に変更します.
(2)choreonoidを起動します.
(3)PCLバーの「Est」ボタンを押します.ダイアログが表示されます.
(4)calibrationタブを選択します.
(5)apply trans matのチェックをはずします.
(6)selectボタンをクリックし,キャプチャしたポイントクラウドの保存先を決定します.
(7)"capture and save"ボタンを押すと,ポイントクラウドが表示されます.(同時にファイルに保存されます.)
(8)表示されたポイントクラウドから実際の座標との対応点を4つ見つけ,そのペアを記録します.
ハンドカメラの場合はcalibrationタブで"show palm pos"ボタンを押すとpalmの位置・姿勢がメッセージウィンドウに表示されるので,これも記録します.
(9)固定カメラの場合はPCL/calibtools/caliration.mに(8)で記録した座標で上書きし,calibmat.txtを作成します.
ハンドカメラの場合はPCL/calibtools/calirationhandcamera.mに(8)で記録した座標とpalmの位置姿勢を上書きし,calibmat.txtを作成します.
(10)choreonoidを一旦終了させ、再度起動します.
(11)PCLバーの「Est」ボタンを押します.ダイアログのcalibrationタブを選択し,selectボタンをクリックし,先ほど保存したポイントクラウドファイルを選択します.
(12)loadボタンを押すとワールド座標系に変換されたポイントクラウドが表示されますので,正しいか確認します.ハンドカメラの場合はhand cameraのチェックをいれます.
※ハンドカメラの場合はロボットをポイントクラウドキャプチャ時の姿勢にセットしておきます.
(13)ずれている場合はポイントクラウドと実際の座標との対応点を見つけcaliration.mまたはcalirationhandcamera.mを更新し,calibmat.txtを作成します.
初期位置合わせ時の法線を見る方法を説明します。
まずObjectPoseEstimator.cppに
#define DEBUG_MODE
の記述を追加しビルドします。
choreonoidを起動しPCLプラグインの位置・姿勢推定機能を実行します。
extplugin/graspPlugin/PCL/の下にobj.pcdとscene.pcdが生成されます。
これは、オブジェクトと環境の法線付きポイントクラウドです。
次にこのpcdファイルを描画します。
[windowsの場合]
コマンドプロンプトを起動し、PCLがインストールされているフォルダに移動します。
> bin/pcd_viewer_release.exe -normals 1 -normal_scale 0.005 "対象pcdファイルのパス”
$ sudo apt-get install libpcl-1.6-bin
$ pcd_viewer -normals 1 -normal_scale 0.005 "対象pcdファイルのパス”
choreonoidを起動します。
PCLバーのEstをクリックするとダイアログが表示されます(使用方法を参照)。
ダイアログの中段PCD/modelの「load scene from」の右側の「select...」ボタンを押し、extplugin/graspPlugin/PCD/PoseEstimationPCD/poseestimation_sample.pcdを選択します。
ダイアログの中段PCD/modelの「obj file path」の右側の「select...」ボタンを押し、extplugin/graspPlugin/Samples/Object2/110418_ELPA_lowhrp.wrlを選択します。
ダイアログの中段Parmetersの「number of planes(plane remove)」を2に設定し、OKボタンを押します。
下図のようなポイントクラウドが表示されます。
アイテムタブに「110418_ELPA_Merged」が追加されているのでこれにチェックをいれます。下図のように箱の真ん中にリモコンが表示されます(向き等は下図と違う場合があります。)
choreonoidを終了します。
graspPlugin が内蔵しているアームの動作計画プラグインである PRM プラグインは、環境との干渉を回避しながらアームを初期姿勢から目標姿勢まで到達させるための動作計画を行います。以下、PRMプラグイン の操作について解説します。
動作計画作成チュートリアル をご参照ください。
このプラグインはスタンフォード大のJ.C. Latombe教授らのグループで開発されたMotion Planning Kit (MPK) [1]で使われている動作計画アルゴリズムを graspPlugin プラグインとしてポーティングしたものです。アルゴリズム自体はPRMの派生版である SBL(Single-Query Bi-directional Lazy-collision checking) probabilistic roadmap planner を用いています。詳細は下記リファレンスを参照してください.
[1] http://robotics.stanford.edu/~mitul/mpk/
[2] G. Sanchez and J.C. Latombe, "A Single-Query Bi-Directional Probabilistic Roadmap Planner with Lazy Collision Checking", Int. Symposium on Robotics Research (ISRR'01), November 2001. Published in Robotics Research: The Tenth Int. Symp., R.A. Jarvis and A. Zelinsky (eds.), Springer Tracts in Advanced Robotics, Springer, pp. 403-417, 2003.
http://robotics.stanford.edu/~latombe/papers/isrr01/spinger/latombe.pdf
PRMの基本的な情報・操作方法はPRM プラグイン解説を参照してください。
PathPlanバーのParamボタンをクリックするとダイアログが表示されます。
各項目の意味は以下の通りです。
・algorithm: 使用するアルゴリズム
・epsilon:干渉チェックを行う間隔。大きい値に設定すると計算時間が短縮されますが、障害物と干渉する軌道が生成される可能性が高くなります。
・max iteration:最大イテレーション回数。小さい値に設定すると、複雑な経路を求められないことがあります。大きい値にすると計算時間が増大する可能性があります。
・delta:現在、未使用。
・smoothe step:スムージングの回数
・number of paths: ここで設定した値のパス数見つかるまで探索します。(RRT*アルゴリズムのみ)。大きい値に設定すると、よい解が得られる可能性が高くなりますが、その分計算時間が増大します。
・NN search method:近傍点の探索アルゴリズム。(RRT*アルゴリズムのみ)
・radius:NN search methodでradiusを選択したときの近傍点探索の距離(コンフィギュレーション空間)。大きい値にするとスムージングがよくなりますが、計算時間が増大します。
ここで設定したパラメータは、PathPlanバーから実行した軌道計画だけではなく、RPMプラグインで軌道計画を行うプラグイン(PickAndPlacePlanner,AssemblyPlanner,PickingTaskPlanner等)にも適用されます。
CMake時にPRM_ENABLE_MULTITHREADをONにすることで、マルチスレッド処理を行うようになりますが、現在マルチスレッド処理を行うと計算時間が増加してしまうため、
PRM_ENABLE_MULTITHREADはOFFにすることを推奨します。
軌道計画に成功すると、アイテムタブの対象ロボット、オブジェクトアイテムの下の階層にGraspPoseSeqItemX(Xは数字)が生成されます。
生成されたモーションを再生するには、ロボットのGraspPoseSeqItemXと、オブジェクトのGraspPoseSeqItemXの下の階層のmotionを選択した状態であることが必要です。
Ctrlを押しながら左クリックで、2つ以上のアイテムを選択できます。
軌道計画実行直後は選択された状態ですが、軌道計画後にアイテムタブを操作すると選択が解除されてしまいます。
プラグイン名: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の場合は双腕ロボットデモ手順書[inline:rev6.pdf]に主な使い方が記してあります。
USE_DB_IN_PICKANDPLACEがONの場合は戦略デモについてに主な使い方が記してあります。
戦略デモの場合は、graspPlugin/PickAndPlacePlanner/ManipController.hにおいて、下記のdefine文のコメントを外してください。
#define DEMO_20140612 #define MOVE_ARM_ONEBYONE
# # 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
実験においてはカメラ座標系とロボット座標系の間でのキャリブレーションが必要になりますが、そのキャリブレーションを行った結果のキャリブレーション行列はgraspPlugin/PCL/calibtools/calibmat.txtに記述します。また、RobotInterfaceプラグインでこのキャリブレーション行列を使うために、graspPlugin/RobotInterface/RobotInterface.cppにおいて以下のdefine文にコメントを外します。
#define USE_PCL_PLUGIN
最後に、ロボットの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] : 基準姿勢からx方向にどれだけ変化させながら持ち替え姿勢を探索するか 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.
RobotInterface プラグインは、視覚認識とロボットの操作を行うプラグインです。サーバを通じて視覚認識を行ったり、RTコンポーネントとして接続した単腕ロボット・双腕ロボットをコントロールします。ここで、単腕のロボットを操作するためのインタフェースとしてはACT共通IFを用い、双腕のロボットをロボットを操作するためのインタフェースとしては双腕ロボットACT共通IFを用いています。また視覚認識についてはスクリプトベースですが、もし視覚認識についても共通IFに準拠したものを接続したい場合はVisualTriggerプラグインを使ってください。
とりあえず、共通IFに準拠しているロボットは何でも繋げるはずです。
「=Interface=」のタイトルを持つインターフェースバーで操作します。
詳細はインターフェースバーヘルプをご参照ください。
1.graspPlugin/RobotInterface/ArmControllerRtc/HiroNX.idl に以下のコマンドを追加
void rhandAttach(); void rhandDetach(); void lhandAttach(); void lhandDetach();
% omniidl -bcxx HiroNX.idl % make -f Makefile.ArmController
3. graspPlugin/RobotInterface/RobotInterface.cppに以下の形でコマンドを追加
controllerRtc()->manipulator()->rhandAttach();
5. idlコンパイル
% sh idlcomplie.sh
NextageInterfaceの場合、rhandAttach()とrhandDetach()の項目を
追加し、そこに
HiroNXManipulator.rhandAttach()
と
HiroNXManipulator.rhandDetach()
を追加
HiroNXManipulator.pyにrhandAttach()とrhandDetach()の項目を追加し、ここにrobot._hand....とかのハンドを外すコマンドとつけるコマンドを記述
プラグイン名: RtcGraspPathPlan
依存プラグイン: Grasp, PRM
% cd ~/src/choreonoid % bin/choreonoid extplugin/hrgPlugin/hrp2.cnoid
3. 新しい把持対象物のVRMLモデルがある場合
(1)VRMLモデルをgraspPlugin/Sample/Objectに置く
(2)hrgPlugin/hrp2jrl.cnoidをプロジェクトファイルとしてchoreonoidを起動する。
(3)ファイルメニュー->読み込み->OpenHRPモデルファイルを選択し、その後追加したVRMLファイルを選択する。
(4)アイテムメニューで把持対象物をチェックし、更に=Planner= バーのsetObjectボタンを押すことで、把持対象物として認識させる。
(5)Graspボタンを押すと把持計画がスタートする。
(6)把持が成功すると、=Planner=バーのSaveGraspPatternボタンにより把持姿勢を保存する。
(7)把持計画を対象物の姿勢を変えながら実行し、その都度saveGraspPatternボタンを押すことで把持姿勢を保存する。保存された把持姿勢の集合はhrgPlugin/RobotModel/HRP2JRL/data/preplanning_対象物名.txtというファイルに保存される。
(8)graspPlugin/Sample/Object/objList.txtを編集し、上記ファイルをRtcGraspPathPlanプラグインに認識させる。
プラグイン名:SoftFingerStability
依存プラグイン:Grasp;GeometryHandler
・ソフトフィンガを持つハンドの把持安定性を計算します
・Enパラメータ計算方法の説明([inline:EnCalculationAlgorithm.pdf])
SoftFingerStabilityプラグインを使用できるようにします。
SoftFingerStabilityHandler.hをインクルードします。
#include "../SoftFingerStablity/ConstraintIK.h
double Q; // 把持安定性評価値
int nContact; // 接触点数
vector< Vector3> objPos(nContact); // 接触点座標
vector< Vector3> objN(nContact); // 接触点法線ベクトル
// 接触点を求めるコードをここに記述
// ....
//
// 把持安定性評価値を計算する
SoftFingerStabilityHandler::instance()->initialize();
Q = SoftFingerStabilityHandler::instance()->calcStability(nContact,&objPos[0],&objN[0]);
ハンドのポリゴンモデルに閉じたポリゴンモデル以外を使用すると、把持安定性評価値が正しく計算されません。
ロボットアームに基準を設置し、アームを動かしポイントクラウドを撮像することにより、kinectをキャリブレーションする方法を説明します。
あらかじめ、基準モデルとそのVRMLファイルを作成し、基準モデルをロボットに設置します。
MotionFile、PCL、RobotInterfaceプラグインを使用できる状態にします。
■基準情報の設定
extplugin/graspPlugin/PCL/config.iniファイルを開きます。
[Calibration]セクションを編集します(無ければ追加します)。
[Calibration] target_link_name=LARM_JOINT4 ;基準を設置したリンク名を指定します。 calib_model_file=./calibleftHrp.wrl ;基準のVRMLファイルパスを記述します。
■キャリブレーション行列の設定
あらかじめ、おおまかなキャリブレーション行列を設定しておきます。
編集ファイルは固定カメラ使用時は、extplugin/graspPlugin/PCL/calibtools/calibmat.txt
ハンドに取り付けたカメラ使用時は、ロボットのYAMLファイルに記述したパス(ViewPlannerプラグイン参照)のファイルを編集します
■キャリブレーション用データディレクトリのクリア
extplugin/graspPlugin/PCL/calibrationdataディレクトリがある場合は削除します。
・キャリブレーション用ポイントクラウドのキャプチャ
1.Choreonoidを起動します。
2.SetRobot,SetCameraで対象ロボットと使用カメラを選択します。
3.MotionFileツールバーのLoadボタンを押し,ロボットのモーションを生成します。
4.InterfaceツールバーのMoveボタンを押し、ロボットを動作させます。
5.PCLツールバーのCalibCapボタン押すと、ポイントクラウドがキャプチャされポイントクラウドとロボットの関節各情報が保存されます。
※CalibCapボタンを押すとき、Choreonoid上のロボットの姿勢が実際のロボットの姿勢と同じであることを確認してください。
6.3-5を繰り返します。
・キャリブレーション
1.PCLツールバーのGenCalibMatを押します。
2.キャリブレーション行列の保存先を指定します。
3.画面上にポイントクラウドが表示、基準モデル(アイテム名:Calib)が追加、ダイアログが表示されます。
基準モデルをポイントクラウド上の基準位置の近くに移動させ、ダイアログのOKボタンを押します。(ポイントクラウドに基準がなければSkipを押します)
4.基準モデルとポイントクラウドの位置合わせが実行され基準モデルが移動しますので、ポイントクラウドと一致していたらAcceptを押します。
一致していなければRejectボタンを押すか、基準モデルを移動させ、Retryボタンを押し再度位置合わせを実行します。
5.キャプチャデータの個数分3-4を繰り返します。指定した保存先にキャリブレーション行列が保存されます。
kinectで得られたデータはレンズ歪み等で歪んでいるため補正を行う。
kinectで得た点の座標を(x,y,z)を下記の式を用い座標(x,y,z')へ補正を行う。
r^2=(x^2+y^2) / z^2
z_tmp = z * (1+ k1*r^2 + k2*r^4)
z' = d1 * z_tmp + d2 * z_tmp^2
係数k1,k2,d1,d2はextplugin/graspPlugin/PCL/config.iniファイルに記述する。以下に例を示します。
[Capture]
do_correcton=true
k1=-0.09
k2=0.193
d1=0.95
d2=0.07
Chorenoid、graspPluginでpythonインターフェースが用意されている機能は、pythonスクリプトとしてchoreonoid上で実行可能となります。
ここでは、pythonスクリプトのサンプルと使用方法、pythonインターフェース作成方法を説明します。
Choreonoidのビルド時のCmakeで以下の項目をONにしてビルドする必要あります。
- BUILD_PYTHON_PLUGIN
- ENABLE_PYTHON
コマンドラインでChorenoidを起動するときにオプションで実行するPythonスクリプトを指定します。
$ choreonoid [project-file] -p <pythonscriptfile >
$ cd <chorenoidのパス> $ bin/chorenoid -p ext/graspPlugin/Grasp/python/script/grasp_example1.py
次は、プロジェクトファイルを読み込み、その後Pythonスクリプトを実行する例です。
$ cd <chorenoidのパス> $ bin/chorenoid ext/graspPlugin/Grasp/python/script/grasp_example.cnoid \ -p ext/graspPlugin/Grasp/python/script/grasp_example2.py
まず、実行方法を説明します。
プロジェクトに使用するPythonスクリプト情報を保存したファイルとして、
ext/graspPlugin/Grasp/python/script/grasp_example_withpython.cnoid
があります。
これをコレオノイドで起動します。
$ bin/chorenoid ext/graspPlugin/Grasp/python/script/grasp_example_wihtpython.cnoid
プロジェクトにPythonスクリプト情報を追加する方法を説明します。
・まず、もととなるプロジェクトファイル(cnoid)を読み込む、もしくは、まっさらな状態からモデル等をロードしておきます。
・メニューの[ファイル]->[読み込み]->[Pythonスクリプト]をクリックすると、ファイル選択ダイアログが表示されるので、対象となるPythonファイルを選択します。
・アイテムタブに対象ファイル名のアイテムが追加されます。対象アイテムにチェックが入っていることを確認してください。
チェックが入っていないとスクリプトが実行されません。
・メニューの[ファイル]->[名前を付けてプロジェクトを保存]で保存します。
上記の例では、スクリプト実行タイミングがスクリプト実行ボタンをクリックした時ですが、以下の手順でプロジェクトが読み込まれたとき
に実行するようにできます。
・アイテムタブの対象Pythonスクリプトアイテムを選択します。
・アイテムタブの下にプロパティタブがあり、その中の「読み込み時に実行」をTrueに変更します。
- grasp_exmaple1.py
PA10とahiruを読み込み、把持計画する処理
from cnoid.Util import * from cnoid.Base import * from cnoid.Body import * from cnoid.BodyPlugin import * from cnoid.GraspPlugin import * # choreonoidのディレクトリを取得 topdir = executableTopDirectory() # choreonoidのrootItemを取得 rootItem = RootItem.instance() ## load robot model # BodyItemを作成 robotItem = BodyItem() # PA10を読み込む robotItem.load(topdir + "/ext/graspPlugin/RobotModels/PA10/PA10.yaml") # PA10をrootItemに追加(アイテムタブに表示されるようになる) rootItem.addChildItem(robotItem) # PA10のアイテムをチェックする(シーンタブにPA10が表示される) ItemTreeView.instance().checkItem(robotItem) ## load object model # BodyItemを作成 objItem = BodyItem() # ahiruを読み込む objItem.load(topdir + "/ext/graspPlugin/Samples/Object/ahiruLowHrp.wrl") # ahiruを移動する objItem.body().rootLink().setTranslation([0.59, -0.1, 0.55]) # ahiruをrootItemに追加(アイテムタブに表示されるようになる) rootItem.addChildItem(objItem) # ahiruのアイテムをチェックする(シーンタブにahiruが表示される) ItemTreeView.instance().checkItem(objItem) ## set grasping robot # PA10に対しSetRobotする(PlannerバーのSetRobot) set_robot(robotItem) ## set target object # ahiruに対しSetObjectする(PlannerバーのSetObject) set_object(objItem) ## execute grasp # 把持計画を実行(PlannerバーのGrasp) grasp()
from cnoid.Util import * from cnoid.Base import * from cnoid.BodyPlugin import * from cnoid.GraspPlugin import * # choreonoidのrootItemを取得 rootItem = RootItem.instance() ## set grasping robot # アイテムタブからPA10を探す robotItem = rootItem.find("PA10") # PA10に対しSetRobotする(PlannerバーのSetRobot) set_robot(robotItem) ## set target object # アイテムタブからahiruを探す objItem = rootItem.find("ahiru") # ahiruに対しSetObjectする(PlannerバーのSetObject) set_object(objItem) ## execute grasp # 把持計画を実行(PlannerバーのGrasp) grasp()
プラグインにpythonインタフェースの追加方法を説明します。
既にpythonインタフェースがあるプラグインで追加・修正を行う場合は
1. 2. は行う必要はありません。
1.CMakeLists.txtの末尾に以下を追加
if(ENABLE_PYTHON) add_subdirectory(python) endif()
3.pythonディレクトリにCMakeLists.txtを作成
Grasp/python/CMakeLists.txtを参考にソースファイルなどを適宜変更
set(target PyGraspPlugin) # "PyGraspPlugin"を対象プラグインに合わせて名前変更 add_cnoid_python_module(${target} PyGraspPluginModule.cpp # 対象プラグインに合わせて名前変更 # 以下PythonInterfaceを記述したソースファイルを追加 PyPlanBase.cpp PyGrasp.cpp ) target_link_libraries(${target} CnoidGraspPlugin CnoidPyBase PyBody PyBodyPlugin)
-PyGraspPluginModule.cpp
#include <cnoid/PyUtil> // pythonへエクスポートする処理の関数宣言 // Graspプラグインでは // PyPlanBase.cppに定義されているPlanBaseクラスをエクスポートするexportPlanBase() // PyGrasp.cppに定義されているSetRobot,SetObject,Grasp機能をエクスポートするexportGrasp() void exportPlanBase(); void exportGrasp(); BOOST_PYTHON_MODULE(GraspPlugin) { // GraspPlugin部分がモジュール名に相当 boost::python::import("cnoid.Base"); boost::python::import("cnoid.Body"); boost::python::import("cnoid.BodyPlugin"); // pythonへエクスポートする関数を実行 // 処理はPyPlanBase.cpp,PyGrasp.cppを参照。 exportPlanBase(); exportGrasp(); }
#include < iostream> .... using namespace boost::python; using namespace grasp; namespace { // 実際に行う処理をここに記述 void Grasp() { .... } bool Set_grasping_robot(cnoid::ItemPtr item, int arm_id = 0) { cnoid::BodyItemPtr bodyitem = cnoid::dynamic_pointer_cast<cnoid::BodyItem, cnoid::Item>(item); .... } BOOST_PYTHON_FUNCTION_OVERLOADS(Set_grasping_robot_overloads, Set_grasping_robot, 1, 2) void Set_grasped_object(cnoid::ItemPtr item) { cnoid::BodyItemPtr bodyitem = cnoid::dynamic_pointer_cast<cnoid::BodyItem, cnoid::Item>(item); PlanBase::instance()->SetGraspedObject(bodyitem); } } // エクスポートする処理 void exportGrasp() { def("grasp", Grasp); def("set_robot", Set_grasping_robot, Set_grasping_robot_overloads()); def("set_object", Set_grasped_object); }
def("set_robot", Set_grasping_robot, Set_grasping_robot_overloads());
はデフォルト引数がある関数を呼び出している例です。
Set_grasping_robotがデフォルト引数がある関数で、Set_grasping_robot_overloadsがデフォルト引数を扱うためのクラスで
BOOST_PYTHON_FUNCTION_OVERLOADS(Set_grasping_robot_overloads, Set_grasping_robot, 1, 2)
で作成しています。ここで第3引数、第4引数は引数の最小と最大の数です。
ここではVRMLモデルへのBox,Cylinderフィッテイング機能を説明します。
この機能はPCLプラグインに含まれています。
Box,Cylinderフィッテイング手法:[inline:FittingAlgorithm.pdf]
対象とするオブジェクトをSetObjectします。
PCLバーにある「Fitting」ボタンを押します。
フィッテイングされたBox,Cylinderが表示されます。
※表示されるBox,Cylinderはオブジェクトの位置・姿勢がすべて0の状態を仮定したものです。
パラメータ変更は(choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/config.iniの
[Fitting]以下を編集します。
以下は各項目の意味です。
sampling_density : ダウンサンプリングのサンプリング幅
high_curvature : クラスタリング時にこの値以上のcurvatureを持つ点を除去する。
show_curvature_result : デバッグ用。high_curvature以上の点を除去した結果を表示。
cluster_tolerance : クラスタリング時の近傍点範囲[m]
cluster_eps_angle : クラスタリング時の法線の許容角度[rad]。この値を小さくすると曲率が高い曲面が分割されやすくなります。
show_cluster_result : デバッグ用。クラスタリング結果を表示します。
rsd_radius : RSD算出時の近傍点範囲[m]
cylinder_rsd_tolerance : cylinderフィッティング時の妥当判定の半径誤差の閾値[m]
cylinder_radius_tolerance : cylinderフィッティング時のinlierに含める範囲[m]
box_ortho_angle_tolerance : boxフィッティングでの直交判定時の許容誤差角度[rad]
box_dist_tolerance : boxフィッティング時のinlierに含める範囲[m]
box_require_point_ratio : boxフィッティング時、boxの表面に必要な点密度(2個目以降のboxフィッティング)。この値が大きければ、2個目以降に密度が低いboxが選ばれなくなります。
show_fitting_shapes : True時フィッティング結果が表示。
show_fitting_points : デバッグ用。フィッティング結果(ポイントクラウド)を表示。
do_box_fitting : True時Boxフィッティングを実行
do_cylinder_fitting : True時Cylinderフィッティングを実行
学習結果を用いると、ハンドのアプローチ上に障害物(ポイントクラウド)があっても識別器により、成功する確率が高ければ、解として選択されます。
学習結果を用いない場合は、ハンドのアプローチ上に障害物(ポイントクラウド)がない解を選択します。
ばら積みピッキング学習(訓練)データ作成(固定デプスセンサ)で識別器ファイルを作成することで、学習データに基づいたピッキング計画が行えます。
使用方法は学習結果を使用しない時と同様に、上記実行手順を行います。
動作計画時に学習結果の使用するかどうかは、識別器ファイルの有無で判断されます。
識別器ファイルはext/graspPlugin/PickAndPlacePlanenr/learning_data/”ハンド名”_”オブジェクト名”_"識別器メソッド名"_model.txtです。
また、動作計画実行時にメッセージウィンドウの表示で学習結果を使用しているか判断できます。
使用時:Start learning based bin picking planning...
未使用時:Start bin picking planning....
ここでは把持姿勢補間・配置位置探索について説明します。
この機能はピックアンドプレースに失敗したときに使用することを想定しています。
1.対象ロボットに"SetRobot",対象オブジェクトに"SetObject"をしておきます。
配置させたい位置をCtrl+クリックします。(対象位置に矢印が表示されます)
2.この状態でManipバーのDebugボタンをクリックします。
把持姿勢補間・配置位置探索が実行されます。
終了するまで数分掛かります。
3.配置可能な位置の探索に成功するとextplugin/graspPlugin/ObjectPlacePlannner/data/data_put.matに位置情報が記録され、以降のピックアンドプレースではこの情報を利用することになります。
把持姿勢補間に成功すると対象オブジェクトの把持DBにピックアンドプレース可能な把持姿勢が追加されます。
把持姿勢補間・配置位置探索に失敗した場合は、ピックアンドプレースに失敗した原因がメッセージウィンドに表示されます。
把持姿勢補間・配置位置探索は下図の手順となっています。
アイテムタブからパラメータを変更したいロボットを選択しPlannerバーのSetRobotをクリックし、パラメータ変更を行うアームを選択します。
DataGenバーのParamボタンをクリックします。
下図のような設定画面が表示されます。
1つのタブで1つの設定を表しています。
各パラメータの意味は以下の通りです。
Name:設定名
GRC Position: GRCの位置(Palmからの相対位置)。
GRC RPY: GRCの姿勢。
GRC max Edge: GRCの辺長。
Angle: 対象指関節の基準姿勢時の関節角。
Contact: 対称物と接触するリンクにチェックをいれます。
CompLink: どの関節を使ってリンクの位置を補償するかを定義します。
Close: 補償する関節の刻み幅を指定します。
Min/Max Load: 把持対象物の最大/最小の重量。
Use Palm Close: 対象物とPalmを接触させる場合チェックを入れます。またclose directionにPalmを移動させる方向を指定します。
「Display」ボタンをクリックすることで、下図のようにGRCと指関節の基準姿勢を確認できます。
「Grasp」ボタンをクリックすることで、設定したパラメータで把持計画を行います。(事前に対象オブジェクトに対し、SetObjectを実行しておく必要があります。)
変更が完了したら「Save ALL」をクリックしてください。変更した全ての設定が更新されます。
新しくパラメータを追加する場合は、パラメータ設定画面を表示し「Add」ボタンをクリックします。
設定画面上に新しいタブが表示されるので、パラメータを設定し「Save All」ボタンをクリックします。
graspPluginSetting: - dataFilePath: (フォルダ名) prehensionList: [prehension]
パラメータ設定画面を表示させ、削除したいパラメータのタブを選択します。
"Remove"ボタンをクリックします。
最後に”Save All"ボタンをクリックします。
対象としているロボットに、別のYAMLファイルからパラメータを追加するには以下の手順を実行します。
SetRobotが実行済みであると、アイテムビューの対象ロボットの子アイテムにアームがあります。また各アームには子アイテムとしてパラメータがついています。
この中から対象のアームを選択します。
次に、メニューからファイル->読み込み->Prehension parameter YAMLをクリックします。
ファイル選択ダイアログが表示されるので、読み込むYAMLを選択します。
アイテムビューにYAMLファイルから読み込んだパラメータが追加されます。
パラメータ設定画面を表示させ、変更・削除等を行い、最後に「Save All」ボタンをクリックしパラメータを保存します。
grasp プラグイン(Choreonoid プラグイン)プログラミングのちょっとしたテクニック集です。
BodyItem::load を使って VRML モデルファイルを読み込むことができるが、プロジェクトファイルにすでにそのモデルが存在する場合、同じモデルが二つ出てきてしまう。
この状態でプロジェクトをセーブして、またChoreonoid を起動すると、今度は三つのモデルが現れる。
これを回避するには、RootItem::findItem メソッドで、モデルが存在しないことを確認すれば良い。
以下は、名前 name を持つモデルが存在しない場合、ファイル wrlhrpfile からモデルを読み込む関数 getItem の例。
BodyItem * getItem(string name, string wrlhrpfile) { // ルートアイテムをチェック BodyItem * item = RootItem::instance()->findItem8 (name); if (item == NULL) { // なければ登録 item = new BodyItem(); if (item->load(wrlhrpfile, "OpenHRP-VRML-MODEL")) { mout << "load: " << wrlhrpfile << endl; RootItem::mainInstance()->addChildItem(item); ItemTreeView::mainInstance()->checkItem(item, true); } } return item; }
graspPlugin1.4以降のzipファイルをダウンロードできます.以前のバージョンはここです.
graspPlugin-ros-tms-1.4 for Choreonid 1.4
※ros-tmsとの連携専用のバージョンです.一般的なgraspPlugin-1.4は,後ほどリリースします.
Ubuntu 10.04 上に graspPlugin の環境を構築するには、下のソフトウェアのインストール手順を順に実行します。GraspPlugin の構築が完了したら、GraspConsumer プラグインによる把持動作計画チュートリアルでハンドロボット PA10 に缶をつかむ動作をさせることができます。なお、GraspConsumerプラグインを使わないのであれば、Choreonoid とgraspPlugin のインストールとビルドで十分です.
一括セットアップスクリプトを使って、OpenRTM を Ubuntu 12.04 にインストールする。
ここでは、インストール先を~/src/choreonoid-1.3.0 とする。
OpenRTM-aist-1.0.0-RELEASE から Ubuntu 用一括セットアップスクリプト pkg_install_ubuntu.sh をダウンロードする。
(注:ウェブサイト上では pkg_install_ubuntu.sh と表示されているが、実際にダウンロードするファイルは pkg_install_ubuntu100.sh となる。ただし、2013年9月現在このファイルは存在しないようです。結果的にOpenRTM1.1をインストールすることになると思いますが、問題なく動作することを確認しています。)
ブラウザを使わずに、wget でダウンロードする場合は以下の通り。(以降、囲み内で %で始まる行は端末に打ち込むコマンドを示す。最初の%は含まない)
% wget http://svn.openrtm.org/OpenRTM-aist/trunk/OpenRTM-aist/build/pkg_install_ubuntu.sh
% sudo sh pkg_install_ubuntu.sh
OpenRTM-aist のリポジトリが登録されていません。 Source.list に OpenRTM-aist のリポジトリ: deb http://www.openrtm.org/pub/Linux/ubuntu/ lucid main を追加します。よろしいですか?(y/n)
この操作後に追加で XXXkB のディスク容量が消費されます。 続行しますか [Y/n]?
警告: 以下のパッケージは認証されていません! (注:ここに表示されるパッケージ名は、その都度異なります) 検証なしにこれらのパッケージをインストールしますか [y/N]?
OpenRTM のサンプルのソースは /usr/share/OpenRTM-aist/examples にインストールされる。
% cp -pr /usr/share/OpenRTM-aist/examples ~/workspace
2012/2/20現在、apt-getでOpenRTM1.0系をインストールしようと思っても、1.1系がインストールされてしまいます。この場合、/etc/apt/preferencesを編集します。OpenRTMのサイトにも書いてありますが、preferencesに以下の記述を追加してください。
--<ここから>--
Package: openrtm-aist
Pin: version 1.0.*
Pin-Priority: 1001
Package: openrtm-aist-dev
Pin: version 1.0.*
Pin-Priority: 1001
Package: openrtm-aist-doc
Pin: version 1.0.*
Pin-Priority: 1001
Package: openrtm-aist-example
Pin: version 1.0.*
Pin-Priority: 1001
Package: openrtm-aist-python
Pin: version 1.0.*
Pin-Priority: 1001
Package: openrtm-aist-python-example
Pin: version 1.0.*
Pin-Priority: 1001
--<ここまで>--
ロボット動作振り付け統合ソフトウェア Choreonoid の導入手順を以下に記す。ここでは一度にgraspPluginの導入はせず、Choreonoidのみをビルドして実行する。なお、graspPluginはChoreonoidのバージョン1.0から1.3.1に対応しています。バージョン1.4への対応は少しお待ちください。
Choreonoid ホームページ のダウンロードページから、ソースパッケージ http://choreonoid.org/_downloads/choreonoid-1.3.1.zip をダウンロードして、展開する。
展開先に~/srcを指定することで、~/src/choreonoid-1.3.1ディレクトリが作られ、ビルドに必要なファイルが展開される。
端末を立ち上げ、この~/src/choreonoid-1.3.1ディレクトリをカレントディレクトリとする。
% cd choreonoid-1.3.1
% cd choreonoid/misc/script % sudo ./install-requisities-ubuntu-1*.**.sh (中略) 続行しますか [Y/n]?
% sudo apt-get update % sudo ./install-requisities-ubuntu-1*.**.sh
% sudo apt-get install libgstreamermm-0.10-dev libqt4-phonon-dev
% sudo apt-get install libeigen3-dev
% cd ../..
Makefile を生成するため、ccmake を起動する。
% ccmake .
% make
make が完了したら、 choreonoid を実行する。
% bin/choreonoid
glaspPlugin のインストール手順を以下に記す。
加えて、wx-common パッケージをインストールする。
% sudo apt-get install wx-common
これらがすでにインストール済みであれば、この項はスキップして graspPlugin インストール から作業を始めることができる。
graspPlugin は Choreonoid のプラグインとして提供される。
以下のサイトにブラウザからアクセスする。
https://code.google.com/p/grasp-plugin/downloads/list
graspPluginのソースを ~/src/choreonoid-1.3.1 の extplugin 以下に展開する。
% cd ~/src/choreonoid-1.3.1/extplugin % unzip graspPlugin-1.3.zip
% cd ~/src/choreonoid-1.3.1/extplugin/graspPlugin % unzip PRM-1.3.zip
% ./graspPlugin/Grasp/install-requisities-ubuntu.sh
graspPlugin をプラグインとして導入するため、ccmake で Choreonoid のビルド設定を変更する。
% cd ~/src/choreonoid-1.3.1 % ccmake .
2度cキーを押して configure すると、GRASP_PLUGINS と GRASP_ROBOT_MODEL_PLUGINS の二つの項目ができる。
となる。目的とするプラグインが依存しているプラグインについては、各プラグインの解説の中に依存プラグインとして記述している。
値を編集したら、「c」キーを押して configure を行う。
数秒後に configure が無事に終了すると、画面の下のキーメニューに新しく「Press [g] to generate and exit」の項目が出る。
「g」キーを押すと、makefileが生成され、ccmake が終了する。
% make
choreonoid を実行する。
% bin/choreonoid
graspPlugin を組み込んだことによって、ツールバーの種類が増えている。
また、Messageタブにもプラグインファイルを読み込み、起動した旨のメッセージが表示されている。
把持動作計画モジュール GraspPlannerComp のコンパイル方法について、以下に記す。
ここでは、graspPlugin のインストールとビルド まですでに完了しているとする。
GraspPlanner 本体のソースのあるディレクトリに移動し、make する。
% cd ~/workspace % cd Choreonoid/extplugin/graspPlugin/GraspConsumer/GraspPlan20100623/ % make
`rtm-config --idlc` `rtm-config --idlflags` -I`rtm-config --prefix`/include/rtm/idl GraspController.idl rtm-skelwrapper --include-dir="" --skel-suffix=Skel --stub-suffix=Stub --idl-file=GraspController.idl GraspControllerSkel.h was generated. GraspControllerSkel.cpp was generated. GraspControllerStub.h was generated. GraspControllerStub.cpp was generated. rm -f GraspPlanner.o g++ `rtm-config --cflags` -I. -c -o GraspPlanner.o GraspPlanner.cpp rm -f GraspPlanningImpl.o g++ `rtm-config --cflags` -I. -c -o GraspPlanningImpl.o GraspPlanningImpl.cpp rm -f GraspControllerSkel.o g++ `rtm-config --cflags` -I. -c -o GraspControllerSkel.o GraspControllerSkel.cpp rm -f GraspControllerSVC_impl.o g++ `rtm-config --cflags` -I. -c -o GraspControllerSVC_impl.o GraspControllerSVC_impl.cpp rm -f GraspPlanner.so g++ -shared -o GraspPlanner.so GraspPlanningImpl.o GraspPlanner.o GraspControllerSkel.o GraspControllerSVC_impl.o `rtm-config --libs` -L/usr/lib -L/usr/local/lib rm -f GraspPlannerComp.o g++ `rtm-config --cflags` -I. -c -o GraspPlannerComp.o GraspPlannerComp.cpp g++ -o GraspPlannerComp GraspPlanningImpl.o GraspPlanner.o GraspControllerSkel.o GraspControllerSVC_impl.o GraspPlannerComp.o `rtm-config --libs` -L/usr/lib -L/usr/local/lib
% ./GraspPlannerComp
Eclipse 全部入り(OpenRTM Eclipse tools)を動作させるには、Ubuntu 標準の Open JDK ではなく、Sun製の JDK が必要である。
Sun製の JDK sun-java6-jdk をインストールするには、まず apt のリポジトリを追加したうえで、パッケージリストを取得する。
% sudo add-apt-repository "deb http://archive.canonical.com/ lucid partner" % sudo apt-get update
% sudo apt-get install sun-java6-jdk アップグレード: 0 個、新規インストール: 8 個、削除: 0 個、保留: 0 個。 57.0MB のアーカイブを取得する必要があります。 この操作後に追加で 168MB のディスク容量が消費されます。
続いて、以下の画面が表示されたら「はい」を選択する。
インストールが終了したら、デフォルトのjavaをsun-java6に設定する。
% sudo update-alternatives --config java
There is only one alternative in link group java: /usr/lib/jvm/java-6-sun/jre/bin/java Nothing to configure.
There are 3 choices for the alternative java (providing /usr/bin/java). Selection Path 優 Status ------------------------------------------------------------ 0 /usr/lib/jvm/java-6-openjdk/jre/bin/java 1061 auto mode * 1 /usr/bin/gij-4.4 1044 manual mode 2 /usr/lib/jvm/java-6-openjdk/jre/bin/java 1061 manual mode 3 /usr/lib/jvm/java-6-sun/jre/bin/java 63 manual mode Press enter to keep the current choice[*], or type selection number:
OpenRTM Eclipse tools 1.0-RELEASE から、 Linux用全部入りパッケージ をダウンロードして、(例として)ホームディレクトリのworkspace 以下に展開する。
% cd workspace % wget http://www.openrtm.org/pub/OpenRTM-aist/tools/1.0.0/eclipse342_rtmtools100release_linux_ja.tar.gz % tar xzf eclipse342_rtmtools100release_linux_ja.tar.gz % cd eclipse
そこで、以下のコマンドラインで起動する。
% ./eclipse -clean -vmargs -Dorg.eclipse.swt.browser.XULRunnerPath=/usr/lib/xulrunner-1.9.2.13/xulrunner
Eclipse のRT System Editor 上で、Name Server View が有効にならない場合があります。
これは、 /etc/hosts の IPv6 関係の項目(The following lines are以降の行)をコメントアウトすることで回避できます。
% sudo gedit /etc/host
として、/etc/hosts を編集してください。
/etc/hosts の例
127.0.0.1 localhost
127.0.1.1 yourservername # 注:この項はサーバ名(PC によって異なる)
# The following lines are desirable for IPv6 capable hosts
#::1 localhost ip6-localhost ip6-loopback
#fe00::0 ip6-localnet
#ff00::0 ip6-mcastprefix
#ff02::1 ip6-allnodes
#ff02::2 ip6-allrouters
#ff02::3 ip6-allhosts
Eclipse 上で作成した Choreonoid プロジェクトに対して、細かい設定を行う。
インクルードパスを設定すると、不明な型にカーソルを合わせてF3を押すだけで宣言部を参照できるようになる。
まずプロジェクトのプロパティを開く。プロジェクト名を右クリックしてコンテキストメニューを表示し、その一番下の「プロパティ」を選択すると、プロパティ画面が現れる。等がある。
Choreonoid では、ccmake で Makefile を生成しているので、Eclipse からもこれを呼び出せるようにしたい。
インクルードパスの場合と同様にプロジェクトのプロパティを開き、C/C++ Build を選択すると、以下のような画面が出る。
[inline:CppBuild.png]
ここで、「Generate Makefiles automatically」のチェックを外すと、Build directory のロックが外れるので、「${workspace_loc:/Choreonoid}」に修正する。
また、マルチコアCPUを積んだPCであれば、並列コンパイルでトータルビルド時間を短縮できる。
Behaviourタブを開いて、「Use parallel build」をチェック、「Use parallel jobs」にコア数に応じた適切な値を設定するだけでよい。
「適切な値」はPCによって異なる。CPUのコア数、デュアル版・クアッド版、ハイパースレッディング機能の有無、等々。
ひとまず コア数(一般に2~8程度)+1~2 として、パフォーマンスに余裕があるなら増やし、なければ減らすことになるだろう。
また、まれに並列コンパイルを行うと問題の発生するプロジェクトもある。
[inline:BuildBehaviour.png]
エラーが出たら即make を中断する「Stop on first build error」は並列コンパイルとは相性がよくないので、通常は外しておくが、上流のコンパイルエラーが下流に響いて大量のエラーが出るときなどはチェックした方がよい。
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行目辺りにあるコメントを外すことで可能になります。
Choreonoid 1.3から以下の点が変更されています。
干渉チェックColdetLinkPairUpdateCheckPtr -> cnoid::ColdetLinkPairPtr
#include <boost/make_shared.hpp> using namespace boost;
#ifdef CNOID_10_11_12_13 arm()->palmObjPair = new ColdetLinkPair(palm(),object() ); #else arm()->palmObjPair = make_shared< ColdetLinkPair >(body(), palm(), bodyItem->body(), object() ); #endif
ColdetLinkPairUpdateCheckPtr temp= new ColdetLinkPairUpdateCheck(bodyItemRobot()->body()->link(i),bodyItemRobot()->body()->link(j));
#ifdef CNOID_10_11_12_13 ColdetLinkPairPtr temp= new ColdetLinkPair(bodyItemRobot()->body()->link(i),bodyItemRobot()->body()->link(j)); #else ColdetLinkPairPtr temp = make_shared< ColdetLinkPair >(bodyItemRobot()->body(), bodyItemRobot()->body()->link(i), bodyItemRobot()->body(), bodyItemRobot()->body()->link(j) ); #endif
robotObjPairs.push_back(new ColdetLinkPairUpdateCheck(bodyItemRobot()->body()->link(j), object() ));
#ifdef CNOID_10_11_12_13 robotObjPairs.push_back(new ColdetLinkPair(bodyItemRobot()->body()->link(j), object() )); #else robotObjPairs.push_back(make_shared< ColdetLinkPair >(bodyItemRobot()->body(),bodyItemRobot()->body()->link(j),targetObject->bodyItemObject->body(),object())); #endif
#ifdef CNOID_10_11_12_13 if(j==nJoints-1) linkObjPair[j] = new ColdetLinkPair(tip, bo->body()->link(0)); else linkObjPair[j] = new ColdetLinkPair(fing_path->joint(j), bo->body()->link(0)); #else if(j==nJoints-1) linkObjPair[j] = make_shared< ColdetLinkPair >(body, tip, bo->body(), bo->body()->link(0)); else linkObjPair[j] = make_shared< ColdetLinkPair >(body, fing_path->joint(j), bo->body(), bo->body()->link(0)); #endif
#include <cnoid/JointPath>
#ifdef CNOID_10_11_12_13 fing_path = body->getJointPath(palm, tip); #else fing_path = make_shared< JointPath >(palm, tip); #endif
#ifndef CNOID_10_11_12_13 #include <cnoid/ItemList> #endif
#ifdef CNOID_10_11_12_13 BodyItem* item = prevSelected[i]; #else BodyItem* item = prevSelected.get(i); #endif