graspPlugin

graspPlugin for Choreonoid

graspPluginは、アームの先端にハンドが取り付けられた構成のロボットシステムに対して、把持計画、軌道計画、作業計画など、種々の計画問題を解くことが出来るソフトウェアです。種々のロボットに対して種々の計画問題を解くために、ソフトウェアは再利用性が高いことが要求されます。例えば、グリッパをとってみても、平行グリッパ、回転グリッパ、片側可動、両側可動など種類が異なる多種のグリッパが存在します。graspPluginはロボットの仕様や作業目的に応じて拡張や機能をカスタマイズできるように、ロボット動作振り付け統合ソフトウェア Choreonoid をフレームワークとして用い,このプラグインとして個々のソフトウェアモジュールを開発しています。

なお、軌道計画を行うプラグインとしてスタンフォード大学で開発されたMPK(Motion Planning Kit)をポートしたもの、PRM, RRT, RRT*をgraspPlugin for Choreonoidでは用いることができます。このMPKはJean-Claude Latombe教授のご厚意でサイトにアップロードしているもので、そのライセンスはMPK本体のライセンスに従います。

詳しい使い方は、ContentsのgraspPluginのリンクをクリックすることでみることができます。

開発チーム
辻 徳生(金沢大)
原田 研介(大阪大学/産総研)

hrp3.jpg
pa10.jpg
rh1.jpg

ライセンス

MIT

導入方法

graspPluginのインストール方法 をご覧ください。

チュートリアル

以下に様々な情報がありますので、必要に応じてみてください。

公開レポジトリ

graspPluginのFree VersionはGitHubで公開しています。以下です。

https://www.github.com/kensuke-harada/graspPlugin

graspPluginのインストール方法

1. Choreonoidのインストール

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に応じてファイル名が異なります)

2. graspPluginのインストール

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

4. OpenRTMのインストール(オプション)

OpenRTMを使用するプラグインを使う場合(RobotInterfaceなど)を使う場合にインストールする.詳細はOpenRTMのサイトで確認する.

OpenRTMのサイト http://www.openrtm.org/openrtm/ja/content/tools より、eclipse toolsの全部入りをインストールする。

5. コンパイル

 % cd ~/src/choreonoid
 % ccmake .

ここで、色々なオプションが出てくるので、目的に合わせて設定する。得に、GRASP_PLUGINS, GRASP_ROBOT_MODEL_PLUGINSはgraspPluginの目的に応じて適切に設定する。また、hrgPluginをインストールした場合はHRGPLUGINS, HRGRobotModelsPluginも同様に設定する。例えば、戦略デモの場合は

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

Ubuntu16.04でPCLプラグインを使用する場合:
Qt5を使用する必要があります。ccmake時にUSE_QT5という項目があるので、これをONにしてください。

6. コンパイルのTips

Ubuntu 14.04でChoreonoid1.4をコンパイルする際は、choreonoid/src/Util/Reference.hの以下の2行をコメント
//#include <boost/atomic.hpp>
//#define CNOID_REFERENCED_USE_ATOMIC_COUNTER

Ubuntu 14.04でCMakeでENABLE_OSGをONにするには、OpenSceneGraph3.0をインストールする必要がある。
http://openscenegraph.alphapixel.com/osg/downloads/free-openscenegraph-binary-downloads )からLinux 3 .0 .1 64bitの所にある
OpenSceneGraph-3.0.1-Ubuntu-x64-release-12741.7zをダウンロードする。これを適当なディレクトリに展開する。
展開して生成されたディレクトリ中にlib64ディレクトリがあるので、この名前をlibに変更する。
ccmake時のOSG_DIR に展開して生成されたディレクトリのパスを設定する。
※7zが展開できない時はp7zipをインストール
 % sudo apt-get instlall p7zip

7. その他のTips

Ubuntu16.04、Choreonoid1.5、Qt5の環境で、Choreonoid上のシーンタブでダブルクリックをしてもオブジェクトを選択できない現象が起こりました。
以下の方法で解決ができます。
まず、シーンバーの設定ダイアログを開きます。(下図中赤枠)
choreonoid_bar.png
「ピッキングにOpenGLピクセルバッファを使う」のチェックをはずし、「了解」ボタンを押します。
scene_dialog.png

以上

graspPluginのWindowsへのインストール方法 (Choreonoid 1.5)

windows10 64bitへのchorenoid1.5とgraspPluginのインストール方法を説明する


1.choreonoid1.5のインストール

以下のツール・ライブラリのバージョンでの動作を確認した
ツール
ライブラリ

1.1 ツール等のインストール

1.1.1 Visual Studio Express 2015 for Windows Desktop

https://www.visualstudio.com/ja/vs/visual-studio-express/
ページ下へ移動し、"Express for Desktop"をクリック。
ダウンロードしたファイルを実行し、インストールを開始。
install_vs01.png

1.1.2 CMake

https://cmake.org/download/
ページ中央部のLatest ReleaseのWindows win64-x64 Installerのリンクをクリックしダウンロード。
install_cmake01.png

ダウンロードしたファイルを実行し、インストールを開始。
インストール中に以下の画面が表示されるので"Add CMake to the system PATH for all user" もしくは"Add CMake to the system PATH for the current user"
を選択する。
install_cmake02.png

1.1.3 Boost

https://sourceforge.net/projects/boost/files/boost-binaries/
インストール対象のバージョンをクリック。
install_boost01.png
choreonoidをビルドするVisualStudioのバージョンとビット数が一致するファイルのリンクをクリック。
例えばboostのバージョンが1.63.0、VisualStudioのバージョンが2015で64bitならば
  boost_1_63_0-msvc-14.0-64.exe
を選択する。
install_boost02.png

ダウンロードしたファイルを実行し、インストール。
インストール先はバージョン1.63.0では、デフォルトでC:\local\boost_1_63_0となる。

※環境変数の設定方法は
 1)画面左下のwindowsアイコンを右クリック。メニューが表示されます。
 2)システムをクリック。システムウィンドウが表示されます。
 3)左側のシステム詳細設定をクリック。システムプロパティウィンドウが表示されます。
 4)環境変数ボタンをクリック。あとは新規作成する場合は新規ボタンを、既存の変数を編集する場合は変更ボタンを押して設定します。

1.1.4 Qt

https://www.qt.io/download-open-source/
ページ中のDownload Nowをクリックし、インストーラをダウンロード。

install_qt01.png

1.1.5 Eigen

http://eigen.tuxfamily.org/index.php?title=Main_Page
対象バージョンのzipファイルをダウンロード。
install_eigen.png

ダウンロードしたファイルを解凍。
生成されたフォルダをeigen-"バージョン"(バージョン3.2.10ではeigen-3.2.10)に名前を変更し、C:\localの下へ移動。

1.2 Choreoniodのビルド

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)を設定。
install_choreonoid01_0.png

2)Configureをクリック
3)"Specify the generator for this project"に"Visual Studio 14 2015 Win64"を選択し、Finishをクリック。
install_choreonoid02_0.png
4)"EIGEN_DIR"にEigenフォルダ(C:\local\eigen-3.2.10)を指定し、Configureをクリック。
install_choreonoid03_0.png
5)"CMAKE_INSTALL_PREFIX"をchoreonoidをchoreonoidのフォルダ(C:\local\choreonoid-1.5.0)に変更し、Configureをクリック。
install_choreonoid04_0.png
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を選択します。
install_choreonoid05.png
3)画面上部メニューから[ビルド]->[ソリューションのビルド]でビルドを実行します。
4)ビルドが成功したら、右側のソリューションエクスプローラにある"INSTALL"を右クリックします。
 ポップアップメニューが表示されるので、その中にある"ビルド"をクリックします。
 choreonoidがC:\local\choreonoid-1.5.0\binにインストールされます。
 (構成をReleaseでビルドした場合はchoreonoid.exe、構成をDebugでビルドした場合はchoreonoid-debug.exeが実行ファイルとなります。)
install_choreonoid06.png


2.graspPluginのインストール

以下のライブラリのバージョンで動作を確認しました。

2.1 ライブラリ等のインストール

2.1.1 Qhull

http://www.qhull.org/download/
Download: Qhull 2015.2 for Windows 10, 8, 7, XP, and NT
をクリックし、ソースをダウンロードし、適当なフォルダで解凍。
install_qhull01.png

1)CMake GUIを起動
2)Where is the source codeとWhere to build the binariesに"解凍先フォルダ"を指定
3)Configureをクリック。
4)MAKE_INSTALL_PREFIXに適当なフォルダを選択。この先C:\local\qhullを指定したとして説明を進めます。
5)Configureをクリック。
6)Add Entryボタンをクリックし、以下のように設定

install_qhull02.png
install_qhull03.png
7)Configureをクリックし、Generateをクリック。"解凍先フォルダ"にqhull.slnが生成されます。
8)CMakeを終了

"解凍先フォルダ"にqhull.slnを開きます。
1.2 ChoreoniodのビルドのVisualStudioによるビルドと同様の手順でビルドとインストールを行う。
※choreonoidをDebugでビルドするときはQhullもDebugバージョンが必要になるので、ソリューション構成をDebugにしてDebugバージョンもビルド・インストールを行います。

2.1.2 freeglut

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バージョンもビルド・インストールを行います。

2.1.3 Python

https://www.python.org/downloads/windows/
Latest Python2 Releaseをクリック。
Windows x86-64 MSI installerをクリックしてダウンロード。
install_python01.png
install_python02.png

ダウンロードしたファイルを実行しインストール。
途中で以下の画面になるので、Add python.exe to Pathをインストールするに変更します。
install_python03.png

2.1.4 LAPACK

HIRO/Pluginを使用する場合にビルド・インストールが必要となります。
LAPACKをビルドをするにはMinGWがインストールされている必要があります。

MinGWのインストール

https://mingw-w64.org/doku.php/download
ページ中段のMingw-buildsのリンクをクリック。
Sourceforgeをクリックするとダウンロードが始まります。
install_mingw01.png
install_mingw02.png

ダウンロードしたファイルを実行し、インストールを開始します。
途中の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の部分はインストールするバージョンにより変わります。)
install_mingw03.png

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の部分はインストールするバージョンにより変わりますので適宜変更してください。)

LAPACKのインストール

http://www.netlib.org/lapack/#_previous_release
lapack-3.5.0.tgzをクリックしダウンロードし解凍。(3.7.0はWindowsではビルドができませんでした。)
install_lapack01.png

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をクリック。
install_lapak02.png
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

2.1.5 PCL

PCLプラグインを使用する場合にインストールが必要となります。
PCLプラグインをインストールするにはMicrosoft Visual C++ 2008 Service Pack 1 再頒布可能パッケージがインストールされている必要があります。

Microsoft Visual C++ 2008 Service Pack 1 再頒布可能パッケージ

Windowsキー+xを押すと左下にメニューが表示される。プログラムと機能をクリック
インストールされているプロラムが表示されるので、その中からMicrosoft Visual C++ 2008 Redistributeable を探す。
install_vcredist01.png
見つからない場合は以下の手順でインストールを行う。

https://www.microsoft.com/ja-jp/download/details.aspx?id=26368
ページ中のダウンロードをクリック
ダウンロードするプログラムの選択画面が表示されるので、vcredist_x86.exeとvcredist_x64.exeにチェックを入れ、
次へを押すとダウンロードが開始される。
install_vcredist02.png
install_vcredist03.png
ダウンロードしたファイルそれぞれを実行し、インストールを行う。

PCLのインストール

http://unanancyowen.com/pcl18/
PCL 1.8.0 All-in-one Installer MSVC2015 x64をクリックしダウンロード
install_pcl01.png

ダウンロードしたファイルを解凍し、インストーラを実行
途中で環境変数の設定をきかれるのでAdd PCL to the system PATH for all users もしくは Add PCL to the system PATH for current userにチェックする。
install_pcl02.png

2.1.6 OpenRTM

RobotInterfaceプラグインを使用する場合にインストールが必要となります。

http://www.openrtm.org/openrtm/ja/node/6034
OpenRTM-aist-1.1.2-RELESE_x86_64.msiをクリックしダウンロード

ダウンロードしたファイルを実行

2.2 graspPluginのビルド

2.2.1取得

リポジトリから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

※リポジトリに関しては開発者用レポジトリを参照

2.2.2 extpluginリンクの作成

管理者としてコマンドプロンプトを起動。
 起動方法はwindowsボタンとXを同時押すとメニューが表示されるので、コマンドプロンプト(管理者)をクリック。
以下コマンドでchoreonoidのフォルダへ移動し、リンクを作成します。
 > cd C:\local\choreonoid
 > mklink /D extplugin ext

2.2.3 ビルド

途中まで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プラグイン

1.基本情報

プラグイン名:CnoidRos
依存プラグイン:Grasp

2.概要

PRMやMotionFileで生成した動作をROSで通信する。
ROSノードはpythonスクリプトで書かれており、CnoidRosプラグインとPythonスクリプトはメモリマップドファイルでプロセス間通信を行う。
以下のフォーマットで通信する。
 (命令1);(命令2);.......(命令n)
命令は以下の通り
 goInitial()
 goOff()
 joints[0,0,0,....0]:1.0 (関節角を設定.[]内は関節角列を:のあとは動作時間をあらわしている)
 rhandClose
 lhandClose
 rhandOpen
 lhandOpen

3.動作確認方法

3.1Ubuntuマシン内での動作

3.1.1準備

・ROSのインストール
http://wiki.ros.org/ja/indigo/Installation/Ubuntu を参考にROSをインストールします。
次にHironx/NEXTAGE OPEN APIを以下のコマンドでインストールします。(http://wiki.ros.org/rtmros_nextage/Tutorials/Install%20NEXTAGE%20OPEN%20software%20on%20your%20machine )
 $ sudo apt-get update && sudo apt-get install ros-indigo-rtmros-nextage ros-indigo-rtmros-hironx

・CnoidRos.pyのセットアップ
まずROSのワークスペースを作成します。(http://wiki.ros.org/ja/ROS/Tutorials/InstallingandConfiguringROSEnvironment
 $ mkdir -p ~/catkin_ws/src
 $ cd ~/catkin_ws/src
 $ catkin_init_workspace
 $ cd ~/catkin_ws/
 $ catkin_make

次にワークスペースをOverlayします。
 $ source devel/setup.bash

次にパッケージを作成します
 $ catkin_create_pkg CnoidRos std_msgs rospy

これでCnoidRosパッケージが作成されるのでCnoidRosプラグイン下のCnoidRos.pyをコピーします。
 $ cp "graspPluginのパス"/CnoidRos/ROS/CnoidRos.py ~/catkin_ws/src/CnoidRos/src/

CnoidRos.pyに実行権限を付加します。
 $ chmod a+x ~/catkin_ws/src/CnoidRos/src/CnoidRos.py

3.1.2実行

1.choreonoidを起動します。
2.ターミナルを起動しroscoreを実行

 $ roscore

3. ターミナルを起動しシミュレータを起動
 $ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch

4.ターミナルを起動しCnoidRos.pyを起動
 $ source ~/catkin_ws_devel/setup.bash
 $ rosrun CnoidRos CnoidRos.py

5.choreonoid上で操作。
chorenoidのCnoidRosバーのボタンをクリックするとシミュレータが動作します。

3.2UbuntuとWindowsの通信

3.2.1準備

・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

・Windowsマシン
 -ROS2のインストール
 https://github.com/ros2/ros2/wiki/Windows-Development-Setup を参考にソースからビルドします。(バイナリバージョンは正常に動作しませんでした)
 また上記URL(2016/3/10時点)の説明通りではうまくビルドできませんでした。
  ページ中のBuilding the ROS 2 CodeとInstall a binary distribution of OpenCV compatible with Visual Studio 2015の順番を入れ替えてインストールを行ってください。

 ※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フォルダにコピーします。

3.2.2実行

・Ubuntuマシン
 以下の手順でCnoidRosTalkerを起動します。
 ターミナルを起動します

 $ . ros2_install/ros2-linux/local_setup.bash
 $ CnoidRosTalker.py

・Windowsマシン
 以下の手順でCnoidRosListenerを起動します。
 コマンドプロンプトを管理者権限付きで起動します。
 > cd C:\dev\ros2\install
 > local_setup.bat
 > python bin\CnoidRosListener.py

・choreonoid起動
 Ubuntuマシンでchoreonoideを起動します
 chorenoidのCnoidRosバーのボタンをクリックするとWindowsのコマンドプロンプトにメッセージが表示されます。

・pythonスクリプトについて
 CnoidRosバーのボタンをクリックするとそれに応じてWindowsにメッセージが表示される。Windows側での表示処理はnextage_manipulator.pyに記述されている。
 このファイルを書き換えることで処理内容を変更できる。
 CnoidRosバーのボタンが押されたときに呼ばれるnextage_manipulator.pyの関数を以下に示す。
  [Home] -> goInitial()
  [OffPose]->goOffPose()
  [Move]->動作に応じてrhandOpen(),rhandClose(),lhandOpen(),lhandClose(),setJointAngles(angle,time)が呼ばれる。

ConstraintIKプラグイン

1. 基本情報

プラグイン名:ConstraintIK
依存プラグイン:Grasp;GeometryHandler

2. 主な機能・説明

・障害物を避ける、関節角度制約付きのIK。参考文献[1]の3章を実装。
・このプラグインの実装にはEQuadProg++[2]を使用しています。
・障害物は楕円体で近似します。この情報はyamlファイルに記述します。
・ロボットの楕円体と環境の(SetEnvした)オブジェクトの楕円体が衝突しないように制約をかけIKを解きます。
・カメラを設定してあると、アームとカメラを同時にIKを解きます。

2.1. メニュー

- SaveObstacleShape
 対象物の楕円体情報を保存する
- DisplayObstacleShape
 楕円体を表示
- HideObstacleShape
 楕円体表示のクリア
-SetCamera
 IKを解く時のカメラを選択する

2.2. 使用方法

2.2.1. IK呼び出し方法

まず、ConstrainIKプラグインを使用できるようにします。
ConstraintIK.hをインクルードします。
 #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;

2.2.2. オブジェクトへの障害物(楕円体)の設定

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ファイルパスと違う場合は修正してください。

2.2.3. ロボットへの障害物(楕円体)の設定

「3.1.2.yamlファイル形式」を参考にし、手動でロボットのyamlファイルに楕円体情報を追加してください。

3.詳細な説明

3.1.yamlファイル形式

3.1.1.オブジェクト

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

collisionShape:以下が楕円体情報です。
-center: 楕円体の中心座標
-rpy: 楕円体の回転行列(rpy)
-length: 楕円体の半径
-security_distance: 許容する最小楕円体間距離(文献[1]の(3)式のd_sに相当)
-effective_distance: 楕円体間の拘束条件が有効となる距離(文献[1]の(3)式のd_iに相当)
-xi:(文献[1]の(3)式のξに相当)

3.1.2.ロボット

下記のような記述をロボットの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]

-jointName: 対象ジョイント名
-center: 楕円体の中心座標
-rpy: 楕円体の回転行列(rpy)
-length: 楕円体の半径
座標、回転行列はローカル座標系で指定します。

3.1.3.カメラ

カメラ情報は下記のような記述をロボットの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]

-name: カメラ名
-type: カメラパスの種類(HEAD:頭部カメラ、HAND:ハンドアイカメラ)
-base: カメラパスのベースリンク名
-cameraLink: カメラが設置されているリンク名
-direction: カメラの向き
-focal_distance: カメラの焦点距離(ハンドアイカメラのみ)
-pos: カメラの位置
direction、posはローカル座標系で指定します。

3.2.アルゴリズム

目標位置・姿勢まで20分割して文献[1]のIKを使用して解いていきます。
このままでは、目標位置・姿勢までとの誤差が大きいことがあります。
そのため微調整のため、文献[1]のIKを誤差が十分小さくなるまで繰り返し解きます。

4.動作確認

4.1.障害物(楕円体)の設定

choreonoidを起動します。
extplugin/graspPlugin/ConstraintIK/project/generate_ellipsoid_test.cnoidを読み込みます(「ファイル」->「プロジェクトの読み込み」)。
ConstraintIKバーの「SaveObstacleShape」を押します。
ConstraintIKBar.png
下記ダイアログが表示されるので、「Clustering:」は「Number of Clusters」を選択し、「Approximation type:」はboundingboxを選択し、「show」ボタンをクリックします。
ConstraintIKDialog.png
そうすると下図画像のように、対象の箱のエッジに障害物となる楕円体が表示されます。
ObstacleShapes.png
ダイアログの「write」ボタンをクリックすると「box3.yaml」が生成されます。
生成された「box3.yaml」ファイルをextplugin/graspPlugin/ConstraintIK/project/に移動します。
choreonidを終了します。

4.2.障害物(楕円体)の表示

choreonoidを起動します。
extplugin/graspPlugin/ConstraintIK/project/box3.yamlを読み込みます(「ファイル」->「読み込み」->「OpenHRPモデルファイル」)。
アイテムタブのbox3を選択し、Plannerバーの「SetEnv」をクリックします。
PlannerBar.png
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プラグイン

基本情報

プラグイン名: GeometryHandler
依存プラグイン: Grasp

使用方法1: 把持計画に用いる対象物のパラメータファイルの作成

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]を参考にしてください。

使用方法2: 対象物の配置計画のためのパラメータファイルの生成

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の定義のコメントを活かしてください。

使用方法3:組み立て作業計画

実行コマンド

 % ~/src/Choreonoid$ bin/choreonoid extplugin/graspPlugin/GeometryHandler/project/assemble.cnoid 

コマンド:
マウスを対象物に置いた状態で
L: クラスタ生成
N: シミュレーション結果を基に、接触点軌跡を表示
O: グラフの構築
を実行する。

特に、Lを押すと対象物がクラスタ毎に違った色で表示される。なお、上記コマンドは、GeometryHandlerPlugin.cppに書いてある。

choreonoid1.5でのインターフェース変更

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 プラグイン解説

GraspプラグインはgraspPlugin for Choreonoidの中で最も基礎となるプラグインです。本プラグインが提供する機能は多指ハンドに対する把持計画と、他のプラグインが共通に利用する関数です。

基本情報

プランナーバー

「=Planner=」というタイトルを持つプランナーバーを使って、把持計画を作成します。
PlannerBar.png
詳細は プランナーバーヘルプ をご参照ください。

チュートリアル

動作計画について

Grasp プラグインは、把持や設置のために適切なグリッパの位置を求めるもので、アーム全体をどのように動かすかについては考慮されていません。周囲の環境に干渉しないアームの動作計画を行うには、PRM プラグインを用います。PRM プラグイン解説をご覧ください。

把持計画アルゴリズム

本プラグインでは様々な種類のロボットに対応できるように、ハンドの把持領域と対象物の両方にバウンディングボックスモデルを仮定した把持計画手法を採用しています。本手法を用いることで、例えば多指ハンドであれば3指の指先把持、4指の指先把持、あるいは包み込み把持など様々な把持形態を実現することができます。
ここで、ハンドの把持領域のバウンディングボックスをGrasping Rectangular Convex (GRC)、対象物のバウンディングボックスをObect Convex Polygon (OCP)と呼び、これらの幾何学的関係により大まかな手首の位置姿勢を求め、その上でランダムサンプリングにより最終的な把持姿勢を求めます。
また、把持計画を高速化するために、摩擦円錐の楕円近似を用いたForce Closureの判定手法を用いています。ただし、Force Closure判定の従来手法もプラグインの関数として用意しています。アルゴリズムの詳細については下記リファレンスを参照してください.

References

[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.

Graspプラグインにおいて新しいロボットのモデルを導入する方法

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指6リンクあるのですが、これらは連動して動きます。この連動を定義するのが、このエントリです。

(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プラグインを使える準備が整いました。

GRCmax2.jpg GRCmax
GRCdes2.jpg GRCdes

アームの軸構成に応じて予めちゃんと動く逆運動学の関数を用意しないと、必要以上に把持計画の実装に苦労することになります。graspPluginではArm.cppの中にデフォルトの逆運度学を用意しています。この関数で十分な場合は多いですが、デフォルトの逆運動学を使わずにロボット固有の逆運動学を用いる場合、その関数はgraspPlugin/RobotModels/ModelName/Pluginに置きます。

大まかなパームの姿勢は求まるが、指の姿勢がなかなか定まらない場合は、Finger0CloseやFinger1Closeのパラメータの値を小さくしてみてください。

GraspConsumer プラグイン解説

把持動作計画モジュールのコンシューマであるGraspConsumerプラグイン の操作について解説します。

基本情報

把持計画アルゴリズム

知能化プロジェクトで把持動作計画モジュールを公開しています。これは把持計画を行うRTコンポーネントなのですが、実際には対象物座標系から見た手首位置・姿勢の集合をデータファイルとして置いておき、視覚センサなどから対象物の位置・姿勢が与えられた時にデータファイルを探索することで手首の位置・姿勢を求めています。GraspConsumer プラグインは、Grasp プラグインと同様に把持計画を行うプラグインですが、把持動作計画モジュールのコンシューマとして実装しています。

GraspRTCバー

GraspRTCバーは、ツールバーの以下の部分です。
GraspRTCBar.png
各ボタンの機能については、GraspRTCバーヘルプ をご参照ください。

チュートリアル

GraspConsumer プラグインによる把持動作計画チュートリアル をごらんください。

把持動作計画モジュールについて

GraspConsumerプラグインのプロバイダである把持動作計画モジュールについては、下記をご参照ください。

Graspプラグイン把持姿勢DB生成機能

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で対象ハンドをアタッチする。
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となります。

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.把持データベースへの把持姿勢追加の5と同様です。

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.探索方法の変更

ばねなどのモデルでは、領域をきれいに分割することが難しいことがあります。ばねを全体を1つの領域とし、その領域内をすべて探索することで把持点を得ることができます。
変更方法は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を押します。
bbdialog.png
下図のようにバウンディングボックスが表示されます。
boundingbox.png
choreonoidを終了します。

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

choreonoidを起動します。
extplugin/graspPlugin/GraspDataGen/project/generateDB_HIRO.cnoidを読み込みます(「ファイル」->「プロジェクトの読み込み」)。
extplugin/graspPlugin/Samples/Object/mug2hrp.wrlを読み込みます(「ファイル」->「読み込み」->「OpenHRPモデルファイル」)。
アイテムタブからmug2を選択し、Plannerバーの「SetOject」をクリックします。
DataGenバーの「GenerateGraspPattern」ボタンをクリックすると下記ダイアログが表示されますのでOKを押します。
GenerateGraspPatternDialog.png
把持姿勢探索が始まります。探索が完了するまで30秒から1分程度要します。
メッセージウィンドに

 GraspPattern generating is finished

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

次に把持姿勢を表示します。
DataGenバーの「Grasp」ボタンをクリックすると下記ダイアログが表示されるのでOKボタンを押します。
graspdialog.png
下記画像のようにmug2を把持する姿勢に変化することを確認してください。
grasp1.png
ダイアログの「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が描画されます。
contact_region_obj.png

次に、ハンド側の接触領域を表示します。
DataGenバーの「Grasp」ボタンをクリックし、ダイアログの、「show contact cluster」にチェックをはずし、「show hand contact cluster」にチェックをいれ、OKボタンを押します。
下記画像のように、接触領域が表示されたハンドが描画されます。
contact_region_hand.png
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を押しながらクリックします。すると下図画像のように矢印が現れます(オブジェクトを置く位置を指定しています)。
place.png
PlannerバーのGraspボタンをクリックします。
メッセージウィンドウに「Success: 0」と出ます。(もしfailと出るようならばオブジェクトを置く位置を移動するか、mugを移動してください)
PathPlanバーのStartをクリックします。
メッセージウィンドウに「Trajectory Planning is finished」と表示されれば成功です。
ツールバーのアニメーション開始ボタンを押すと、ピックアンドプレースのモーションが表示されます。
choreonoidを終了します。

4.4. 腰位置探索

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

以下は古い情報です。
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ボタンを押します。
searchwaistpos.png
メッセージウィンドウに"success!"と表示され、ロボットと箱とが干渉状態ではなければ成功です。
choreonoidを終了します。

MotionFileのコマンド

ここではMotionFileで使えるコマンドそれぞれについてまとめる。


全コマンドの書式

左手用コマンドはRARM, RHANDをLARM, LHANDにする

※時刻の単位はsec, 位置の単位はm, 姿勢の単位はdegree

ロボットの構成定義

ロボットのVRMLファイルで定義したジョイント達の、どこが腕や手にあたるかをYAMLファイルで記述する。


各コマンドの詳細

・HEAD_FORWARD

動作内容)
[要確認]
書式)時刻 HEAD_FORWARD
例)
 0  HEAD_FORWARD


・PAUSE

動作内容)
[要確認]
書式)時刻 PAUSE
例)
 1  PAUSE


・JOINT_ABS

動作内容)
ロボットの全関節角を指定した角度にする。
書式)開始時刻 終了時刻 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

0秒 initial.png → 1秒 JOINT_ABS.png
角度が指定した値になっている JOINT_ABS_angles.png


・JOINT_REL

動作内容)
全関節角を現在の関節角から指定した角度動かす。
書式)開始時刻 終了時刻 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

1秒 JOINT_ABS.png → 2秒 JOINT_REL.png
指定した分角度が増減 JOINT_REL_angles.png


・RARM_XYZ_ABS/LARM_XYZ_ABS

動作内容)
手先を指定した位置・姿勢(world座標)に動かす。
書式)開始時刻 終了時刻 RARM_XYZ_ABS x座標 y座標 z座標 roll pitch yaw
例)

 0  1  LARM_XYZ_ABS   -0.12  0.3  0.274   145  -60  -170

0秒 initial.png → 1秒 LARM_XYZ_ABS.png
指定した位置姿勢になっている LARM_XYZ_ABS_pos.png


・RARM_XYZ_REL/LARM_XYZ_REL

動作内容)
手先を現在の位置・姿勢から指定した距離・角度分動かす。
書式)開始時刻 終了時刻 RARM_XYZ_REL x座標の増分 y座標の増分 z座標の増分 rollの増分 pitchの増分 yawの増分
例)

 1  2  LARM_XYZ_REL  0.1  -0.05  0.05  90  0  0

1秒 LARM_XYZ_ABS.png → 2秒 LARM_XYZ_REL.png
指定した分位置姿勢が変化 LARM_XYZ_REL_pos.png


・RARM_LINEAR_ABS/LARM_LINEAR_ABS

動作内容)
手先を指定した位置・姿勢(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

LARM_LINEAR_ABSを使った場合:
0.2 0.4 0.6 0.8 1.0
LINEAR_0.2.png LINEAR_0.4.png LINEAR_0.6.png LINEAR_0.8.png LINEAR_1.0.png
LARM_XYZ_ABSを使った場合:
0.2 0.4 0.6 0.8 1.0
XYZ_0.2.png XYZ_0.4.png XYZ_0.6.png XYZ_0.8.png XYZ_1.0.png


・RARM_LINEAR_REL/LARM_LINEAR_REL

動作内容)
手先を現在の位置・姿勢から指定した距離・角度分動かす。
後はARM_LINEAR_ABS同様。
書式)開始時刻 終了時刻 RARM_LINEAR_REL x座標の増分 y座標の増分 z座標の増分 rollの増分 pitchの増分 yawの増分 分割数
例)
 1  2  LARM_LINEAR_REL  0  0.30  0  0  0  0  100


・RARM_JNT_ABS/LARM_JNT_ABS

動作内容)
腕の関節角を指定した角度にする。
最初のパラメータはIKを解くベースリンクの角度。指定したパラメータの数が不足しているとき、手先の方の関節角は0になる。
書式)開始時刻 終了時刻 RARM_JNT_ABS (右腕の関節数分の関節角度(degree))
例)

 0  1  LARM_JNT_ABS  0  30  -45  -90  90  0  135

0秒 initial.png → 1秒 LARM_JNT_ABS.png
1秒時点でLARMの関節角が指定した値になっている LARM_JNT_ABS_angles.png


・RARM_JNT_REL/LARM_JNT_REL

動作内容)
腕の関節角を現在の関節角から指定した角度動かす。
書式)開始時刻 終了時刻 RARM_JNT_REL (右腕の関節数分の関節角度の増分(degree))
例)

 1  2  LARM_JNT_REL  -30  15  -15  -30  -45  -30  -45

1秒 LARM_JNT_ABS.png → 2秒 LARM_JNT_REL.png
1秒時点の関節角から指定した値だけ変化 LARM_JNT_REL_angles.png


・RHAND_JNT_ABS/LHAND_JNT_ABS

動作内容)
手の関節角を指定した角度にする。
書式)開始時刻 終了時刻 RHAND_JNT_ABS (右手の関節数分の関節角度(degree))
例)

 0  1  LHAND_JNT_ABS   85  45  -85  -45

0秒 HAND_initial.png → 1秒 LHAND_JNT_ABS.png
指の関節角が指定した値になっている LHAND_JNT_ABS_angles.png


・RHAND_JNT_REL/LHAND_JNT_REL

動作内容)
手の関節角を現在の関節角から指定した角度動かす。
書式)開始時刻 終了時刻 RHAND_JNT_REL (右手の関節数分の関節角度の増分(degree))
例)

 1  2  LHAND_JNT_REL  -50  -30  50  30

1秒 LHAND_JNT_ABS.png → 2秒 LHAND_JNT_REL.png
1秒時点の関節角から指定した値だけ変化 LHAND_JNT_REL_angles.png


・RARM_OBJECT/LARM_OBJECT

動作内容)
手先を対象物の座標系から指定した相対位置・姿勢に配置させる[要確認]。
書式)開始時刻 終了時刻 RARM_OBJECT x座標の増分 y座標の増分 z座標の増分 rollの増分 pitchの増分 yawの増分 # 対象物からの相対位置姿勢
例)
 0  1  RARM_OBJECT  0 0 0  0 0 0 


・RHAND_JNT_OPEN/LHAND_JNT_OPEN

動作内容)
[要確認]
書式)開始時刻 終了時刻 RHAND_JNT_OPEN # 値の指定なし
例)
 0  1  RHAND_JNT_OPEN


・RHAND_JNT_CLOSE/LHAND_JNT_CLOSE

動作内容)
[要確認]
書式)開始時刻 終了時刻 RHAND_JNT_CLOSE (右手の関節数分の関節角度(degree))
例)
 0  1  RHAND_JNT_CLOSE


・RHAND_GRASP/LHAND_GRASP

動作内容)
[要確認]
書式)開始時刻 終了時刻 RHAND_GRASP # 値の指定なし
例)
0 1 RHAND_GRASP

MotionFileプラグイン

プラグイン名:MotionFile
依存プラグイン:Grasp,PRM

概要
情報としては少し古いですので、下記を参考にしてください。
  1. MotionFileのコマンド
  2. MotionFileプラグイン
  3. Motionタブの使用方法(choreonoid 1.5)

graspPlugin/MotionFile/data/motion.datに動作コマンドを記述します。
次に、このプラグインを有効にすることでツールバーが現れ、そこにLoadボタンとClearボタンがあります。
Loadボタンを押すとChoreonoid内でロボットが動作コマンドのとおりに動きます。また、RobotInterfaceプラグインを
併用することで、実際のロボットも動作させることができます。なお、個々のコマンドについては、
MotionFileController.cppの中身を見てください。

サンプルの実行方法
graspPlugin/MotionFile/project/3dprinter.cnoidをプロジェクトファイルとしてChoreonoidを起動してください。
3Dプリンタからロボットがupper caseを取り出す動作が生成されます。

Motionタブの使用方法(choreonoid 1.5)

ここではchoreonoid 1.5のgraspPlugin でのMotionタブの使用方法を説明する。


1. コンパイル


ccmakeでGRASP_PLUGINSにMotionFileを追加

1_ccmake_setting.png

choreonoidにMotionタブが追加される:

2_GUI_tab.png


2. モーションの作成

・新規作成

タブ下方のカウンターで追加する行数を指定し、Appendボタンで追加。
3_append.png

・ファイルを読み込み

Loadボタンで作成済みのファイルを指定して読み込む。
6_load.png
Loadボタンを押し、読み込むファイルを選択する。
7_Open_LoadFile.png
読み込んだファイルの内容が表示される。
8_motion_file_opened.png

・モーションの編集

コマンドによってフォーマットが異なる。コマンドごとの詳細はMotionFileのコマンドを参照。
行の追加: Appendボタンで指定行数を追加して中身を編集する(末尾のみ。途中に挿入したいときはファイルを直接エディターで編集する)。
行の削除: コマンドをNoneにして保存し、再度ロードすれば消える。

例) 2行目を削除するとき
9_delete_line.png
コマンドをNONEに変更する。
10_delete_line_none.png
ファイルに保存したときにNONEの行は出力されないので、再読み込みすると表示上からも削除される。
12_delete_line_none_reload.png

編集時の注意:
・入力ボックスにマウスオーバーした状態でマウスホイールを回すと中の値が変わってしまうので、スクロールするときは何もないところにカーソルを置く。
・座標の絶対/相対の指定は、実際には座標指定のないコマンドとの組み合わせでも指定できてしまうので注意する。

・編集したモーションの保存

Saveボタンで編集した内容をファイルに保存する。
18_save.png
ファイルをロード済みであればダイアログが出る
13_save_file_diag.png
Noを選んだ場合は名前を付けて保存する
14_no_overwrite.png

※もともとロードしたファイルにコメント行があった場合、上書きするとコメントがなくなるので注意。


3. モーションの反映

Playボタンを押すと(その前にsetRobot, setObjectを実施しておく)タブ内の編集内容が関節角に変換されてアイテムに読み込まれる。
15_play_button.png

4. ツールバーからの読み込み

16_MotionFileBar.png
ツールバーのPlayメニューを使用した場合は、
choreonoid/ext/graspPlugin/MotionFile/data/motionfile.dat
が存在する場合、その中で指定されたパスのファイルを読み込む。
motionfile.datが存在しない場合は、choreonoid/ext/graspPlugin/MotionFile/data/motion.datが読み込まれる。
17_motionfile.png

Motionファイルの作成

マニュアルでMotionファイルを作成する方法についてまとめる。


概要

Motionファイルに動作コマンドを記述し指定時刻での腕の先端の位置姿勢を指定する。
それから動作(フレームごとの関節角)を生成する

手先はIKを計算するリンク
動作の初期姿勢はsetRobotを実行したときのロボットの位置姿勢

手順

Motionファイルを作成
setRobot, setObject
ロボットのyamlファイルにgraspPluginSetting: を記述しておく必要がある
ツールバーのPlayボタン(choreonoid 1.4ではLoad)

Motionファイルの記法

Motionファイルの作成

プロジェクトファイルでロボットとオブジェクトの配置が決まっている
ロボットが手先に持ったものを動かしたいときはMotionファイルで指定する位置(手先)と
ものの位置が異なることを考慮する

Motionファイルの読み込み

時刻の指定をそのまま反映するには
PRM/TrajectoryPlanner.cpp で
USE_EXACT_TIMEのdefineを有効にする

PCL, PCL.rtcプラグイン

PCLプラグインはPoint Cloud Libraryを利用してポイントクラウドを表示するプラグインで、PCL.rtcプラグインはrtcpclを利用してOpenRTM経由でポイントクラウドを表示させるプラグインです。

PCLプラグイン

プラグイン名: PCL
依存プラグイン: Grasp, GeometryHandler

プラグインを利用する前に以下のコマンドを実行してください。Ubuntuのバージョンにより異なります。

Ubuntu16.04の場合

 $ sudo apt-get install libpcl-dev
 $ sudo apt-get install libproj-dev

Ubuntu16.04以前の場合

 % sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
 % sudo apt-get update
 % sudo apt-get install libpcl-1.7-all-dev

なお、これはextplugin/graspPlugin/PCL/install-requisities-ubuntu.sh に同じ内容があります。

次に、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

この場合は、e → c → e → g か、e → g でMakefileが生成されます。そして、Choreonoidをコンパイルし、Choreonoidを起動してください。PCLバーが追加されていると思います。このバーに含まれるボタンはそれぞれ以下のような機能を持っています。

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プラグイン

プラグイン名: PCL.rtc
依存プラグイン: Grasp, GeometryHandler

本プラグインを利用する前に、以下のコマンドを実行してください。

 % cd ~/src/Choreonoid/extplugin/graspPlugin/PCL.rtc
 % ./install-resuisities-ubuntu.sh

これにより、Point Cloud Library version 1.1がインストールされます。次に、このシェルスクリプトのメッセージに従って、rtcpclをインストールします。rtcpclのインストールの詳細についてはRTCPCL のコンパイルを参照して下さい。ただ、このリンクの中で記述してあるソースの改編についてはgraspPlugin/PCL/rtcpcl以下に置いたソースは全て適用済です。

次に、ccmake においてプラグインとしてGraspとPCL.rtcを指定してコンパイルしてください。また、ccmake の際にBUILD_PCL_PLUGIN は ONにしてください。そして、Choreonoidをコンパイルし、Choreonoidを起動してください。PCLバーが追加されていると思います。このバーに含まれるボタンはそれぞれ以下のような機能を持っています。

Cap: ポイントクラウドをキャプチャする
MinZ:範囲を限定し、その範囲の中で座標が最少のポイントを求める
Cube:ポイントクラウドのポイントを立方体のVRMLモデルで置き換える

rtcpcl2.jpg



その他Tips

VRMLモデルからポイントクラウドへの変換は、PCLプラグインのPointCloudUtility.hにある PointCloudUtil::modelToPiontCloudでおこなう。使用方法はPCL/Calibration.cppの414から421行目が参考になる。

PCLプラグイン位置・姿勢推定機能

ここでは、PCLプラグインの位置・姿勢推定機能について説明する。
PCLプラグインについてはPCL, PCL.rtcプラグインを参照のこと。

1.事前準備

CVFH特徴量を用いて位置・姿勢推定を行う場合は,事前に特徴量DBを作成する必要があります.
CVFH特徴量を用いた位置・姿勢推定方法:[inline:ObjectPoseRecognitionAlgorithm20150311.pdf]
セグメンテーションアルゴリズム:[inline:SegmentationAlgorithm.pdf]

1.1.PartialCloudGenratorのビルド

PartialCloudGeneratorをビルドしていない場合,以下の手順でビルドを行います.

 $ cd (choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/PartialCloudGenerator
 $ ccmake .

ccmakeが起動するので「c」を2回押し,「g」を押します.そして
 $ make

でビルドします.

1.2.部分モデルの作成

対象モデルのある視点から見た部分モデルをポイントクラウド化したものを複数個作成します.
まず,モデルデータを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 (保存先ディレクトリ)

保存先ディレクトリにmodel_X.pcd,pose_X.txt(Xは0から41)が生成されます.
 model_X.pcd:部分モデルのポイントクラウド
 pose_X.txt:姿勢行列

1.3.特徴量DBの作成

部分モデルに対して,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:ダウンサンプリングした部分モデルポイントクラウド

1.4.YAMLファイルの作成

YAMLファイルの例です.これを参考にし,YAMLファイルを作成します.
 # VRML(HRP)ファイルを指定します
 modelFile: modelHRP.wrl
 # CVFH特徴量ファイルがあるディレクトリを指定します
 descriptorDir: ./descriptor/model/ 

2.位置・姿勢推定

2.1使用方法

(1)Choreonoidを起動します.
(2)推定したいオブジェクトをSetObjectします.
 ※CVFHを用いた位置・姿勢推定を行う場合は1.4の方法で作成したYAMLファイルを指定してください.
(3)ハンドに取り付けたカメラ等を使用する場合はSetCameraで使用カメラを選択します。
(4)PCLバーの「Est」ボタンを押します.
(5)パラメータを変更する場合は2.2の情報を参考に変更します.
(6)「OK」ボタンを押すと位置・姿勢推定が実行されます.
 成功したら対象オブジェクトが移動します.
 失敗した場合,対象オブジェクトは原点に移動します.
 失敗した場合は,探索範囲の変更,平面除去回数の変更等を試してください.

2.2パラメータ

「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タブ」
 キャリブレーション用です(後述).

3.設定ファイル

(choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/config.iniを変更することで,特徴量DB作成,位置・姿勢推定時のデフォルトパラメータを変更できます.
ファイル内の [ObjPoseEstimate]から下が位置・姿勢推定,[GenerateDescriptors]から下が特徴量DB作成のパラメータです.
 [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

以下は古い情報です。

4.キャリブレーション方法

(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を作成します.

パラメータの設定

重要と思われるソースコード中のパラメータの設定方法を説明します。
初期位置合わせ(SAC_IA)で用いる法線算出時のビューポイント:
 ビューポイントは法線をどこからみて算出するかを決めるパラメータです。適切な値に設定しないと、同一平面の点でも法線の向きがばらばらになることがあります。
 
SAC_IAEstimatorクラスestimateメソッド内(ObjectPoseEstimator.cpp:l.820辺り)
 Vector3 view_point(0.0, 0.0, 1.0);
でオブジェクトのポイントクラウドの法線算出時のビューポイントを設定しています。
 

法線の可視化

初期位置合わせ時の法線を見る方法を説明します。
まず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ファイルのパス”

上記のコマンドを入力すると法線付きのポイントクラウドが表示されます。
[ubuntuの場合]
まず、PCLのアプリケーションをインストールします
 $ sudo apt-get install libpcl-1.6-bin

pcdファイルを表示します

 $ 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ボタンを押します。
下図のようなポイントクラウドが表示されます。
scene.png
アイテムタブに「110418_ELPA_Merged」が追加されているのでこれにチェックをいれます。下図のように箱の真ん中にリモコンが表示されます(向き等は下図と違う場合があります。)
object.png
choreonoidを終了します。

PRM プラグイン解説

graspPlugin が内蔵しているアームの動作計画プラグインである PRM プラグインは、環境との干渉を回避しながらアームを初期姿勢から目標姿勢まで到達させるための動作計画を行います。以下、PRMプラグイン の操作について解説します。

基本情報

パスプランバー

動作計画の作成(例)

動作計画作成チュートリアル をご参照ください。

プログラムについて

動作計画アルゴリズム

このプラグインはスタンフォード大のJ.C. Latombe教授らのグループで開発されたMotion Planning Kit (MPK) [1]で使われている動作計画アルゴリズムを graspPlugin プラグインとしてポーティングしたものです。アルゴリズム自体はPRMの派生版である SBL(Single-Query Bi-directional Lazy-collision checking) probabilistic roadmap planner を用いています。詳細は下記リファレンスを参照してください.

References

[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の基本的な情報・操作方法は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にすることを推奨します。

その他Tips

軌道計画に成功すると、アイテムタブの対象ロボット、オブジェクトアイテムの下の階層にGraspPoseSeqItemX(Xは数字)が生成されます。
生成されたモーションを再生するには、ロボットのGraspPoseSeqItemXと、オブジェクトのGraspPoseSeqItemXの下の階層のmotionを選択した状態であることが必要です。
Ctrlを押しながら左クリックで、2つ以上のアイテムを選択できます。
軌道計画実行直後は選択された状態ですが、軌道計画後にアイテムタブを操作すると選択が解除されてしまいます。

PickAndPlacePlannerプラグイン

基本情報

プラグイン名: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

そして、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] : 基準姿勢から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 プラグイン解説

RobotInterface プラグインは、視覚認識とロボットの操作を行うプラグインです。サーバを通じて視覚認識を行ったり、RTコンポーネントとして接続した単腕ロボット・双腕ロボットをコントロールします。ここで、単腕のロボットを操作するためのインタフェースとしてはACT共通IFを用い、双腕のロボットをロボットを操作するためのインタフェースとしては双腕ロボットACT共通IFを用いています。また視覚認識についてはスクリプトベースですが、もし視覚認識についても共通IFに準拠したものを接続したい場合はVisualTriggerプラグインを使ってください。

基本情報

プラグイン名
RobotInterface
依存プラグイン
Grasp, PRM
接続を確認したRTコンポーネント
PA10Provider, HiroNXInterface(HiroNXProvider)

とりあえず、共通IFに準拠しているロボットは何でも繋げるはずです。

インターフェースバー

「=Interface=」のタイトルを持つインターフェースバーで操作します。
詳細はインターフェースバーヘルプをご参照ください。

RobotInterfaceプラグインへのツールチェンジ機能の追加について

1.graspPlugin/RobotInterface/ArmControllerRtc/HiroNX.idl に以下のコマンドを追加

        void rhandAttach();
        void rhandDetach();
        void lhandAttach();
        void lhandDetach();

2. graspPlugin/RobotInterface/ArmControllerRtc で以下のコマンドを実行

 % omniidl -bcxx HiroNX.idl
 % make -f Makefile.ArmController

makeの時点で、リンクのエラーが出ても気にしない。

3. graspPlugin/RobotInterface/RobotInterface.cppに以下の形でコマンドを追加

 controllerRtc()->manipulator()->rhandAttach();

4. NextageInterface, THK, PortDuplicatorでHiroNX.idlをコピー

5. idlコンパイル

 % sh idlcomplie.sh

6. HiroNX_idl_example.pyの編集

NextageInterfaceの場合、rhandAttach()とrhandDetach()の項目を
追加し、そこに
HiroNXManipulator.rhandAttach()

HiroNXManipulator.rhandDetach()
を追加

HiroNXManipulator.pyにrhandAttach()とrhandDetach()の項目を追加し、ここにrobot._hand....とかのハンドを外すコマンドとつけるコマンドを記述

RtcGraspPathPlanプラグイン

基本的情報

プラグイン名: RtcGraspPathPlan
依存プラグイン: Grasp, PRM

使用方法

1. モジュールの起動方法
 % cd ~/src/choreonoid
 % bin/choreonoid extplugin/hrgPlugin/hrp2.cnoid

2. コンシューマの作成方法

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プラグイン

1. 基本情報

プラグイン名:SoftFingerStability
依存プラグイン:Grasp;GeometryHandler

2. 主な機能・説明

・ソフトフィンガを持つハンドの把持安定性を計算します
・Enパラメータ計算方法の説明([inline:EnCalculationAlgorithm.pdf])

2.1. 使用方法

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]);        

3. 既知のバグ

ハンドのポリゴンモデルに閉じたポリゴンモデル以外を使用すると、把持安定性評価値が正しく計算されません。

kinectキャリブレーション

ロボットアームに基準を設置し、アームを動かしポイントクラウドを撮像することにより、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/MotionFile/data/motion.datを開き編集します。
モーション例としてextplugin/graspPlugin/MotionFile/data/motion.dat.HIROcalibがあるので参考にしてください。
※kinectと基準が近い(50cm以内)ポーズでは基準が撮像できないので注意してください。

■キャリブレーション行列の設定
あらかじめ、おおまかなキャリブレーション行列を設定しておきます。
編集ファイルは固定カメラ使用時は、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

pythonスクリプトpythonインターフェース

Chorenoid、graspPluginでpythonインターフェースが用意されている機能は、pythonスクリプトとしてchoreonoid上で実行可能となります。
ここでは、pythonスクリプトのサンプルと使用方法、pythonインターフェース作成方法を説明します。

準備

Choreonoidのビルド時のCmakeで以下の項目をONにしてビルドする必要あります。
- BUILD_PYTHON_PLUGIN
- ENABLE_PYTHON

Pythonスクリプトの使用方法

Choreonoid起動時にPythonスクリプトファイルを読込む方法

コマンドラインでChorenoidを起動するときにオプションで実行するPythonスクリプトを指定します。

  $ choreonoid [project-file] -p <pythonscriptfile >

Pythonスクリプトファイルにモデルファイルのロード等すべての処理を記述している場合は、上記の[project-file]部分
は不要です。
例として以下のコマンドを実行してみてください。
 $ cd <chorenoidのパス>
 $ bin/chorenoid -p ext/graspPlugin/Grasp/python/script/grasp_example1.py

grasp_exmaple1.pyは、PA10とahiruを読み込みahiruを把持計画する処理が記述されています。

次は、プロジェクトファイルを読み込み、その後Pythonスクリプトを実行する例です。

 $ cd <chorenoidのパス>
 $ bin/chorenoid ext/graspPlugin/Grasp/python/script/grasp_example.cnoid \
   -p ext/graspPlugin/Grasp/python/script/grasp_example2.py

grasp_example2.pyは、PA10がahiruを把持計画する処理が記述されています(PA10、ahiruの読込処理は含まれていません)。

プロジェクトに追加する方法

まず、実行方法を説明します。
プロジェクトに使用するPythonスクリプト情報を保存したファイルとして、
ext/graspPlugin/Grasp/python/script/grasp_example_withpython.cnoid
があります。
これをコレオノイドで起動します。

 $ bin/chorenoid ext/graspPlugin/Grasp/python/script/grasp_example_wihtpython.cnoid

スクリプト実行ボタンを押すとPA10がahiruを把持します。
pythonscript.png

プロジェクトにPythonスクリプト情報を追加する方法を説明します。
 ・まず、もととなるプロジェクトファイル(cnoid)を読み込む、もしくは、まっさらな状態からモデル等をロードしておきます。
 ・メニューの[ファイル]->[読み込み]->[Pythonスクリプト]をクリックすると、ファイル選択ダイアログが表示されるので、対象となるPythonファイルを選択します。
 ・アイテムタブに対象ファイル名のアイテムが追加されます。対象アイテムにチェックが入っていることを確認してください。
 チェックが入っていないとスクリプトが実行されません。
 ・メニューの[ファイル]->[名前を付けてプロジェクトを保存]で保存します。

上記の例では、スクリプト実行タイミングがスクリプト実行ボタンをクリックした時ですが、以下の手順でプロジェクトが読み込まれたとき
に実行するようにできます。
 ・アイテムタブの対象Pythonスクリプトアイテムを選択します。
 ・アイテムタブの下にプロパティタブがあり、その中の「読み込み時に実行」をTrueに変更します。

サンプルPythonスクリプトの説明

- 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()

- grasp_exmaple2.py
PA10がahiruを把持計画する処理

 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インタフェースの追加方法を説明します。
既にpythonインタフェースがあるプラグインで追加・修正を行う場合は
1. 2. は行う必要はありません。

1.CMakeLists.txtの末尾に以下を追加

  if(ENABLE_PYTHON)
   add_subdirectory(python)
  endif()

2.pythonディレクトリを作成

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)

4.ソースファイルの追加修正
Graspプラグインを参考にして、追加修正を行ってください。
Boost pythonを使ってPythonインタフェースを作成しているので、
プログラミングの詳細はBoost pythonのドキュメントを参照してください。
ここではGraspプラグインのファイルを例に簡単に説明します。

-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();
 }

-PyGrasp.cpp

 #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);
 }

exportGrasp()内のdef関数でpythonに関数をエクスポートしています。
第1引数はPythonでの関数名、第2引数は処理する関数のポインタです。
def("grasp", Grasp);
はpythonでgrasp()が呼ばれたときGrasp()を実行することを表しています。

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]

1.使用方法

対象とするオブジェクトをSetObjectします。
PCLバーにある「Fitting」ボタンを押します。
フィッテイングされたBox,Cylinderが表示されます。
※表示されるBox,Cylinderはオブジェクトの位置・姿勢がすべて0の状態を仮定したものです。

2.パラメータ設定

パラメータ変更は(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フィッティングを実行

固定されたデプスセンサでのばら積みピッキング

事前準備

  1. ext/graspPlugin/PickAndPlacePlanner/ManipController.cppを開き,
    24行目付近の#define OBJ_ENV_CONTACTをコメントアウトします。
  2. ビルドを行います。必要なプラグラインは
    Grasp;PRM;GeometryHandler;ObjectPlacePlanner;PickAndPlacePlanner;PCL:ConstraintIK;SoftFingerStability;GraspDataGenです。
    また、USE_DB_IN_PICKANDPLACEをONに設定しておきます。
  3. 把持データベースを作成しておきます。Graspプラグイン把持姿勢DB生成機能
  4. CVFH特徴量を用いて位置・姿勢推定を行う場合はPCLプラグイン位置・姿勢推定機能に記述されている事前準備を行っておきます。
  5. キャリブレーション行列ext/graspPlugin/PCL/calibtools/calibmat.txtを実験環境用に書き換えてください。

実行手順

  1. コレオノイドを起動し、プロジェクトファイルの作成/読込を行います。
    ※把持対象オブジェクトはwrlファイルではなくYAMLファイルであること(PCLプラグイン位置・姿勢推定機能を参照)
  2. 対象ロボットにSetRobot,対象オブジェクトにSetObjectをし、プレース場所のオブジェクトをSetEnvします。
  3. Ctrl+左クリックでプレース位置を指定します。
  4. 位置・姿勢推定を行います。使用方法はPCLプラグイン位置・姿勢推定機能の使用方法を参照)
  5. ピッキング動作計画を行います。ManipバーのStartボタンをクリックします。
    ダイアログが表示されるので、Cylinder grasping modeを選択しOKをクリックすると動作計画が実行され、
    メッセージウィンドウにSuccessと表示されれば成功です。

学習結果を用いたピッキング

学習結果の使用による違い

学習結果を用いると、ハンドのアプローチ上に障害物(ポイントクラウド)があっても識別器により、成功する確率が高ければ、解として選択されます。
学習結果を用いない場合は、ハンドのアプローチ上に障害物(ポイントクラウド)がない解を選択します。

学習結果を用いたピッキング使用方法

ばら積みピッキング学習(訓練)データ作成(固定デプスセンサ)で識別器ファイルを作成することで、学習データに基づいたピッキング計画が行えます。
使用方法は学習結果を使用しない時と同様に、上記実行手順を行います。
動作計画時に学習結果の使用するかどうかは、識別器ファイルの有無で判断されます。
識別器ファイルはext/graspPlugin/PickAndPlacePlanenr/learning_data/”ハンド名”_”オブジェクト名”_"識別器メソッド名"_model.txtです。
また、動作計画実行時にメッセージウィンドウの表示で学習結果を使用しているか判断できます。
 使用時:Start learning based bin picking planning...
 未使用時:Start bin picking planning....

把持姿勢補間・配置位置探索

ここでは把持姿勢補間・配置位置探索について説明します。
この機能はピックアンドプレースに失敗したときに使用することを想定しています。

1.使用方法

 1.対象ロボットに"SetRobot",対象オブジェクトに"SetObject"をしておきます。
  配置させたい位置をCtrl+クリックします。(対象位置に矢印が表示されます)
 2.この状態でManipバーのDebugボタンをクリックします。
  把持姿勢補間・配置位置探索が実行されます。
  終了するまで数分掛かります。
 3.配置可能な位置の探索に成功するとextplugin/graspPlugin/ObjectPlacePlannner/data/data_put.matに位置情報が記録され、以降のピックアンドプレースではこの情報を利用することになります。
  把持姿勢補間に成功すると対象オブジェクトの把持DBにピックアンドプレース可能な把持姿勢が追加されます。
  把持姿勢補間・配置位置探索に失敗した場合は、ピックアンドプレースに失敗した原因がメッセージウィンドに表示されます。

2.把持姿勢補間・配置位置探索方法

2.1.全体フロー

把持姿勢補間・配置位置探索は下図の手順となっています。

failure_analysis_flow.png

2.2.把持姿勢補間

2.3.配置位置探索

2.4.失敗原因解析

把持計画パラメータ設定方法

設定画面表示

アイテムタブからパラメータを変更したいロボットを選択しPlannerバーのSetRobotをクリックし、パラメータ変更を行うアームを選択します。
DataGenバーのParamボタンをクリックします。
下図のような設定画面が表示されます。

prehensionparam.png

パラメータ変更

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と指関節の基準姿勢を確認できます。

DisplayGRC.png

「Grasp」ボタンをクリックすることで、設定したパラメータで把持計画を行います。(事前に対象オブジェクトに対し、SetObjectを実行しておく必要があります。)

変更が完了したら「Save ALL」をクリックしてください。変更した全ての設定が更新されます。

パラメータ追加

新しくパラメータを追加する場合は、パラメータ設定画面を表示し「Add」ボタンをクリックします。
設定画面上に新しいタブが表示されるので、パラメータを設定し「Save All」ボタンをクリックします。

注意:ロボットのYAMLファイルに以下の記述がなければ,「Save All」でファイルが出力されずコアダンプする.
  graspPluginSetting: 
  -
    dataFilePath: (フォルダ名)
    prehensionList: [prehension]

パラメータ削除

パラメータ設定画面を表示させ、削除したいパラメータのタブを選択します。
"Remove"ボタンをクリックします。
最後に”Save All"ボタンをクリックします。

YAMLファイルからパラメータ追加

対象としているロボットに、別のYAMLファイルからパラメータを追加するには以下の手順を実行します。
SetRobotが実行済みであると、アイテムビューの対象ロボットの子アイテムにアームがあります。また各アームには子アイテムとしてパラメータがついています。
この中から対象のアームを選択します。

itemviewpng.png

次に、メニューからファイル->読み込み->Prehension parameter YAMLをクリックします。

inputyaml.png

ファイル選択ダイアログが表示されるので、読み込むYAMLを選択します。
アイテムビューにYAMLファイルから読み込んだパラメータが追加されます。
パラメータ設定画面を表示させ、変更・削除等を行い、最後に「Save All」ボタンをクリックしパラメータを保存します。

Choreonoid プラグインプログラミング Tips

grasp プラグイン(Choreonoid プラグイン)プログラミングのちょっとしたテクニック集です。

VRMLモデルファイルを読み込む

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;
    }

(了)

Downloads

graspPlugin1.4以降のzipファイルをダウンロードできます.以前のバージョンはここです.

graspPlugin-ros-tms-1.4 for Choreonid 1.4
※ros-tmsとの連携専用のバージョンです.一般的なgraspPlugin-1.4は,後ほどリリースします.

古い情報

graspPlugin 環境構築

Ubuntu 10.04 上に graspPlugin の環境を構築するには、下のソフトウェアのインストール手順を順に実行します。GraspPlugin の構築が完了したら、GraspConsumer プラグインによる把持動作計画チュートリアルでハンドロボット PA10 に缶をつかむ動作をさせることができます。なお、GraspConsumerプラグインを使わないのであれば、Choreonoid とgraspPlugin のインストールとビルドで十分です.

OpenRTM のインストール

一括セットアップスクリプトを使って、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

実行中、以下のようにスクリプトから何度か問い合わせがあるが、すべての問い合わせに対して y と答えて続行する。

問い合わせ例 1

 OpenRTM-aist のリポジトリが登録されていません。
 Source.list に OpenRTM-aist のリポジトリ:
    deb http://www.openrtm.org/pub/Linux/ubuntu/ lucid main
 を追加します。よろしいですか?(y/n)

問い合わせ例 2

 この操作後に追加で XXXkB のディスク容量が消費されます。
 続行しますか [Y/n]?

問い合わせ例 3

 警告: 以下のパッケージは認証されていません!
  (注:ここに表示されるパッケージ名は、その都度異なります)
 検証なしにこれらのパッケージをインストールしますか [y/N]?

[Y/n]と問われているときは、Enter キーを押すだけでyを入力したとみなされインストールが行われるが、[y/N]と問われた場合は明示的にyキーのあとEnterを押さないと、nを入力したとみなされインストールがスキップされてしまう。
もし誤ってインストールをスキップした場合は、もう一度 pkg_install_ubuntu.sh を実行すればよい。

サンプルプログラムを取得

OpenRTM のサンプルのソースは /usr/share/OpenRTM-aist/examples にインストールされる。

 % cp -pr /usr/share/OpenRTM-aist/examples ~/workspace

などとして、ユーザーのワークスペースにサンプルをコピーできる。

OpenRTM 1.1系がインストールされてしまう場合

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 のインストールとビルド

ロボット動作振り付け統合ソフトウェア Choreonoid の導入手順を以下に記す。ここでは一度にgraspPluginの導入はせず、Choreonoidのみをビルドして実行する。なお、graspPluginはChoreonoidのバージョン1.0から1.3.1に対応しています。バージョン1.4への対応は少しお待ちください。

Choreonoid インストール

ソースの入手

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

必須パッケージのインストール

misc/script に、一括インストールスクリプトが用意されているので、実行する。
 % cd choreonoid/misc/script
 % sudo ./install-requisities-ubuntu-1*.**.sh
 (中略)
 続行しますか [Y/n]? 

「*」の部分は対応するバージョン名を入れる。相当量のパッケージをインストールするが、途中で失敗するなら update してから、再度スクリプトを実行する。
 % sudo apt-get update
 % sudo ./install-requisities-ubuntu-1*.**.sh

libgstreamermm-0.10-dev と libqt4-phonon-dev も必要な場合は、手動でインストールする(2013年9月現在必要なし)。
 % sudo apt-get install libgstreamermm-0.10-dev libqt4-phonon-dev 

Ubuntu10.04にChoreonoid1.3.1をインストールする場合は、eigenは自力でインストールする。
 % sudo apt-get install libeigen3-dev

完了したら、Choreonoid ディレクトリに戻る。
 % cd ../..

ccmake を使ったビルド

Makefile を生成するため、ccmake を起動する。

 % ccmake .

ccmake は、最初の起動では EMPTY CACHE とだけ表示される。
emptycache.png
ここで「c」キーを押して configure を行うと、数秒後にキーと値のメニューが表示される。
configure1.png
もう一度cを押すと、画面の下のキーメニューに新しく「Press [g] to generate and exit」の項目が出る。
configure2.png
そこで「g」キーを押すと、ccmake が終了する。これで Makefile が生成されているので、make を行う。
 % make

PCによっては時間がかかる。

Choreonoid の起動

make が完了したら、 choreonoid を実行する。

 % bin/choreonoid

以下のような Choreonoid 起動画面が出たら成功である。
Choreonoid_2.png

graspPlugin のインストールとビルド

glaspPlugin のインストール手順を以下に記す。

必須ソフトウェアのインストール

glaspPlugin のインストールに先立ち、OpenRTM と Choreonoid をインストールする必要がある。
  1. OpenRTM のインストール
  2. Choreonoid のインストールとビルド

加えて、wx-common パッケージをインストールする。

 % sudo apt-get install wx-common 

wx-commonパッケージがないと、ccmakeのconfigureにおいて、wxWidgets_wxrc_EXECUTABLE が NOT FOUND になって失敗する。

これらがすでにインストール済みであれば、この項はスキップして graspPlugin インストール から作業を始めることができる。

graspPlugin インストール

graspPlugin は Choreonoid のプラグインとして提供される。

graspPlugin ソースの展開

以下のサイトにブラウザからアクセスする。

 https://code.google.com/p/grasp-plugin/downloads/list

ここで、graspPlugin-1.3.zip ならびに、PRM-1.3.zipをダウンロードする。

graspPluginのソースを ~/src/choreonoid-1.3.1 の extplugin 以下に展開する。

 % cd ~/src/choreonoid-1.3.1/extplugin
 % unzip graspPlugin-1.3.zip

PRMプラグインのソースを~/src/choreonoid-1.3.1 の extplugin/graspPlugin 以下に展開する。
 % cd ~/src/choreonoid-1.3.1/extplugin/graspPlugin
 % unzip PRM-1.3.zip

なお、開発者は https://grasp-plugin.googlecode.com/svn/trunk/graspPlugin を利用できる。

必須パッケージをインストール

graspPlugin のインストールスクリプトを実行する。(ユーザーパスワードが必要)
 % ./graspPlugin/Grasp/install-requisities-ubuntu.sh

ccmake を使ったビルド

graspPlugin をプラグインとして導入するため、ccmake で Choreonoid のビルド設定を変更する。

 % cd ~/src/choreonoid-1.3.1
 % ccmake .

graspPlugin をビルドするには、ここでccmake で項目を編集する必要がある。
ccmake では、画面の左半分に白地で表示される項目がキーで、右半分に黒地で表示される項目がキーに対する値となる。
上下のカーソルキーを使って目当ての項目に移動したら、エンターキーを押すと値を編集することができる。
値を修正したら、もう一度エンターキーを押すと、再びカーソルキーを上下に動かせるようになる。

2度cキーを押して configure すると、GRASP_PLUGINS と GRASP_ROBOT_MODEL_PLUGINS の二つの項目ができる。
ccmake.png

それぞれに対して、以下の値を設定する。この設定は使うプラグインやロボットのモデルによって異なる。
例えば、双腕ロボットHIROによるピックアンドプレースの場合、
キー GRASP_PLUGINS
値 Grasp;PRM;GripperManipulation;VisionTrigger;RobotInterface
キー GRASP_ROBOT_MODEL_PLUGINS
値 HIRO/Plugin
と設定する。また、例えばPA10によってGraspプラグインの把持計画を使う場合
キー GRASP_PLUGINS
値 Grasp;PRM
キー GRASP_ROBOT_MODEL_PLUGINS
値 PA10/Plugin

となる。目的とするプラグインが依存しているプラグインについては、各プラグインの解説の中に依存プラグインとして記述している。

値を編集したら、「c」キーを押して configure を行う。
数秒後に configure が無事に終了すると、画面の下のキーメニューに新しく「Press [g] to generate and exit」の項目が出る。
pressg.png
「g」キーを押すと、makefileが生成され、ccmake が終了する。

ここで make を実行して、Choreonoid とプラグインをビルドする。
 % make

実行

choreonoid を実行する。

 % bin/choreonoid

以下のような画面が出たら成功。
Choreonoid.png

graspPlugin を組み込んだことによって、ツールバーの種類が増えている。
また、Messageタブにもプラグインファイルを読み込み、起動した旨のメッセージが表示されている。

GraspPlannerComp のビルド

把持動作計画モジュール 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 を実行できれば、成功。

 % ./GraspPlannerComp 

実行メッセージは特に出ない。

Eclipse 全部入りのインストール

Eclipse 全部入り(OpenRTM Eclipse tools)を動作させるには、Ubuntu 標準の Open JDK ではなく、Sun製の JDK が必要である。

sun-java6-jdk のインストール

Sun製の JDK sun-java6-jdk をインストールするには、まず apt のリポジトリを追加したうえで、パッケージリストを取得する。

 % sudo add-apt-repository "deb http://archive.canonical.com/ lucid partner"
 % sudo apt-get update

ここで JDK パッケージのインストールが可能になる。
 % sudo apt-get install sun-java6-jdk
 
 アップグレード: 0 個、新規インストール: 8 個、削除: 0 個、保留: 0 個。
 57.0MB のアーカイブを取得する必要があります。
 この操作後に追加で 168MB のディスク容量が消費されます。

途中、このような画面が表示されるので、「了解」を選択して続行する。
java6.png

続いて、以下の画面が表示されたら「はい」を選択する。
java6-2.png

インストールが終了したら、デフォルトの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.

メッセージが下のようなものだった場合、Sun Java が呼ばれるよう設定する必要がある。
 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: 

こうした場合、Pathの欄が「/usr/lib/jvm/java-6-sun/jre/bin/java」 となっているものを探す。
ここでは 3 が Sun Java ということになるため、3を押してエンターすると、デフォルトのjava がSun のものに変更される。

eclipse 全部入り本体 のインストール

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 を起動するとスプラッシュスクリーンのあと空のウインドウが表示されるだけで、正常に起動しない。

そこで、以下のコマンドラインで起動する。

 % ./eclipse -clean -vmargs -Dorg.eclipse.swt.browser.XULRunnerPath=/usr/lib/xulrunner-1.9.2.13/xulrunner

一回~数回、このオプションをつけて起動できたら、単に ./eclipse だけでも起動できるようだ。
パネルにカスタム・アプリケーションのランチャを追加して、コマンドを /home/(ユーザ名)/workspace/eclipse/eclipse のように設定すると、端末を使わずに Eclipse を起動できる。

/etc/hosts の設定

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 のビルドとインクルードの設定

Eclipse 上で作成した Choreonoid プロジェクトに対して、細かい設定を行う。

インクルードパスの設定

インクルードパスを設定すると、不明な型にカーソルを合わせてF3を押すだけで宣言部を参照できるようになる。

まずプロジェクトのプロパティを開く。プロジェクト名を右クリックしてコンテキストメニューを表示し、その一番下の「プロパティ」を選択すると、プロパティ画面が現れる。
[inline:Project-Property.png]
[inline:EclipseProperty.png]
ここで、左ペインの「C/C++ General」を開き、「Paths and Symbols」を選ぶと、インクルードパスの設定画面となる。
[inline:PathsAndSymbols.png]
Assesmbly,GNU C,GNU C++の三項目が選べるが、ここではすべてに対して一括でインクルードパスを登録する。
右の「Add」ボタンをクリックすると、ダイアログが現れる。
[inline:AddDirectoryPath.png]
そうしたらまず「Add to all languages」 をチェックし、右側の4つのボタンから適切なものを選ぶ。
Variables
Eclipse の内蔵変数を利用する場合
Workspace
ワークスペース内のプロジェクトを参照する場合
File system
/usr/include など、システムのインクルードパスを参照する場合
キャンセル
ディレクトリパスの追加を中止する
登録する主なものとして、
ワークスペース
  • /choreonoid/include
  • /choreonoid/thirdparty/eigen3
  • /choreonoid
ファイルシステム
  • /usr/include/qt4
  • /usr/include/pcl-1.3

等がある。

ビルドの設定

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 プラグイン解説

GripperManipulationプラグインはパラレルグリッパを持つロボットに対してピックアンドプレースの動作計画を行うプラグインです。把持計画手法についてはパラレルグリッパに特化した独自の手法を実装しています。この手法は非常に高速に計算できるの実時間で把持姿勢を求めることが出来ます。また、バウンディングボックスを用いないため複雑な形状の対象物にも比較的容易に適用できます。

基本情報

  • プラグイン名: GripperManipulation
  • 依存プラグイン: Grasp
  • 対応しているロボット: PA10, HIRO

プランナバー

本プラグインを導入すると、プランナバーのStartボタンを押すとGraspプラグインの把持計画が実行されるのではなく、GripperManipulationプラグインの把持計画が実行されるようになります。また、本プラグインを新たに導入するとINTENTIONバーが現れます。このバーは把持対象物の部位を指定するインテンションパラメータを指定するものです。

インストール方法・使用方法

アルゴリズム

下記文献[1]には、把持計画アルゴリズムのうち、非実時間部と実時間部の両方が記述してありますが、本プラグインではその実時間部を実装しています。非実時間部はGeometryHandlerという公開バージョンには含まれていないプラグインを使います。(追記予定)

References

[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.

GripperManipulation プラグインを RT コンポーネントに改造する

Choreonoid/extPlugin/graspPlugin/GripperManipulation というプラグインに、RTコンポーネントの「口」を取り付ける。

オリジナルの GripperManipulation について

双腕ロボットの把持計画を行う graspPlugin プラグインであり、ファイル構成は以下のようになっている。

  • CMakeLists.txt cmake 定義ファイル
  • GripperManipulationMain.cpp Choreonoidプラグイン ManipPlugin クラスのソース
  • ManipBar.cpp Choreonoid ツールバー ManipBar クラスのソース
  • ManipBar.h Choreonoid ツールバー ManipBar クラスのヘッダ
  • ManipController.cpp 把持計画コントローラ ManipController クラスのソース
  • ManipController.h 把持計画コントローラ ManipController クラスのヘッダ
  • PRM 把持計画データファイルのディレクトリ

ここで、プログラムの構造がわかりやすいよう、GripperManipulationMain.cpp を、ManipPlugin.cpp にリネームする。
合わせて CMakeLists.txt も修正して、コンパイルできることを確認する。

RTC BuilderでRTコンポーネントのスケルトンを作成する

下準備として、まず GripperManipulation の下に rtcディレクトリを作成し、ここに GraspController.idl を置く。

さらに、Eclipse を起動し、このrtc ディレクトリにダミーの新規 Eclipse プロジェクトを設定する。
ウィザード選択では「C++ Project」を選び、各項目を以下のように設定する。
  • プロジェクト名 GraspControllerRTC
  • ロケーション /home/asahi/workspace(*注)/Choreonoid/extplugin/graspPlugin/GripperManipulation/rtc
  • Project type: Executable - Empty Project

(*注) 本文では作業ディレクトリを /home/asahi/workspace として進めているので、読者はこの部分を自分の作業ディレクトリに置き換えて読むこと。

NewCppProject.png
GripperManipulationRtcWizard.png

完了ボタンを押すと、C++ パースペクティブを開きますか? というダイアログが出るが、そのまま閉じればよい。

これで下準備は完了したので、Eclipse で RTC Builder パースペクティブを開く。
RTのアイコンをクリックすると、RtcBuilder ビューが開くので、タブごとに必要な情報を設定していく。
RTCBuilder_0.png

「基本」タブ

  • モジュール名 GripperManipulation
  • モジュール概要 Gripper Manipulation
  • ベンダ名 AIST
  • モジュールカテゴリ TestInterface
  • Output Project GripperManipulationRTC (先に作成したプロジェクトを選択する)

その他の項目はデフォルトのままでよい。
RTCBuilder-GripperManipulation.png

「言語・環境」タブ

C++を選択する。

「サービスポート」タブ

Add Port ボタンを押す。
sv_nameというポートができるので、マウスでクリックして右側にプロファイル設定画面を表示する。
ポート名をPlanStartPortに設定して、今度はAdd Interfaceボタンを押し、新しくできたインターフェースに以下の情報を設定する。
PlanStart.png
  • インターフェース名 PlanStart
  • 方向 Required
  • IDLファイル /home/asahi/workspace/Choreonoid/extplugin/graspPlugin/GripperManipulation/rtc/GraspController.idl
  • インターフェース型 GraspPlanStart(選択項目)
  • IDLパス /home/asahi/workspace/Choreonoid/extplugin/graspPlugin/GripperManipulation/rtc/
同じように、ポート ResultPort とインターフェース Result を作成する。
  • インターフェース名 Result
  • 方向 Provided
  • IDLファイル /home/asahi/workspace/Choreonoid/extplugin/graspPlugin/GripperManipulation/rtc/GraspController.idl
  • インターフェース型 GraspPlanResult
  • IDLパス /home/asahi/workspace/Choreonoid/extplugin/graspPlugin/GripperManipulation/rtc/

コード生成

以上で RTC Builder の設定は完了である。
再び基本タブを開き、コード生成ボタンを押すと、Choreonoid/extPlugin/graspPlugin/GripperManipulation/rtc に、以下のファイルが作られる。
  • GraspControllerSVC_impl.cpp
  • GraspControllerSVC_impl.h
  • GripperManipulation.conf
  • GripperManipulation.cpp
  • GripperManipulation.h
  • GripperManipulationComp.cpp
  • GripperManipulationComp_vc8.vcproj
  • GripperManipulationComp_vc9.vcproj
  • GripperManipulation_vc8.sln
  • GripperManipulation_vc8.vcproj
  • GripperManipulation_vc9.sln
  • GripperManipulation_vc9.vcproj
  • Makefile.GripperManipulation
  • RTC.xml
  • copyprops.bat
  • user_config.vsprops

画面の指示にしたがって C++パースペクティブに切り替える。

このとき 一緒に保存されている RTC.xml に、一連の設定が保存されている。
RTコンポーネントの設定を変更する時は、RTC Builderの基本タブの中にある、プロファイルのインポート機能で、この設定を呼び戻すことができる。

rtc をコンパイルする

端末を開いて GripperManipulation/rtc ディレクトリに移って、以下のコマンドを実行する。

 % omniidl -bcxx GraspController.idl

GraspController.hh と GraspControllerSK.cc が生成される。
続いて、make を実行する。Makefile を指定する必要がある。
 % make -f Makefile.GripperManipulation

これによって、GraspControllerSkel.cpp, GraspControllerStub.cpp, GraspControllerSkel.h, GraspControllerStub.h が生成される。
(コンパイルは通らないが、そのまま進める)

Choreonoid プラグインとRTコンポーネントの橋渡しクラスを作成する

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

として GripperManipulation のリビジョン131をチェックアウトしたあと、RTC Builderの設定を行い、ManipRtc.h(cpp) を作成したところで、パッチファイル GripperManipulation_patch.txt をダウンロードし、
 % patch -p0 < GripperManipulation_patch.txt

上のコマンドを実行すると、変更点が一度に適用されるので、ただちにビルドを行える。

以下は、パッチファイルの変更点の概要である。

ManipRtc.h(cpp)

クラス名を ManipRtc に変更する。(Eclipseのリファクタリング機能が便利だが、誤って元のGraspConsumer まで変更しないよう、注意!)

  • GraspConsumer.h をインクルードしている部分を、rtc下のGraspController.hh と GripperManipulation.hに変更する。
  • comp の型を、GraspConsumer *から、::GripperManipulation * に変更する。
  • graspPlanResult の引数で、 const GraspPlanResult となっている部分をすべて::GraspPlanResult に変更する。
  • ついでに、インクルードガードの識別子をヘッダ名と合わせて MANIPRTC_H とする。

ManipPlugin.cpp

ManipPlugin::initialize メソッドに、RTコンポーネントをスタートさせるコードを追加する。

    virtual bool initialize() {
      //Robotics::SceneBodyManager* manager = Robotics::SceneBodyManager::instance();
        grasp::GraspController::instance(ManipController::instance());
        ManipRtc::instance()->RtcStart();
        //manage(manager->addSceneBodyFactory(create));

rtc/GripperManipulation.h

grasp::ManipRtc クラスを、GripperManipulation のフレンドクラスに設定する。

 using namespace RTC;

 namespace  grasp {
    class ManipRtc;
 }
 
 ...
 
 class GripperManipulation
  : public RTC::DataFlowComponentBase
 {
 public:
  friend class grasp::ManipRtc;

rtc/GraspControlerSVC_impl.cpp/h

RTCBuilderはIDLデータ型の引数をCORBAとして出力するが、omniidlは同じものを::CORBAとして出力するため、型が異なるとコンパイラがエラーを出してしまう。
そこで、omniidlの出力に合わせて、CORBAネームスペースの型の引数の宣言を、::CORBA に書き換える。
また、GraspPlanResultSVC_impl クラスのメソッドの引数について、GraspPlanStart ネームスペースの型の引数宣言を GraspPlanResult に書き換える。(これはRTCBuilder のバグか)

CMakeLists.txt

  • set(rtc-dir ./rtc) を追加
  • include_directories(${rtc-dir}) を追加
  • sources に ManipRtc.cpp, ${rtc-dir}/GraspControllerSVC_impl.cpp, ${rtc-dir}/GripperManipulation.cpp, ${rtc-dir}/GraspControllerSkel.cpp を追加。
  • headers に ManipRtc.h, ${rtc-dir}/GraspController.hh, ${rtc-dir}/GraspControllerSVC_impl.h, ${rtc-dir}/GripperManipulation.h, ${rtc-dir}/GraspControllerSkel.h, ${rtc-dir}/GraspControllerStub.h を追加。
  • add_definitions に -fPIC -O2 -I/usr/include -I/usr/include/rtm/idl -I. を追加。
  • target_link_libraries に uuid dl pthread omniORB4 omnithread omniDynamic4 RTC coil を追加。
  • if(UNIX)と、対になるelseif(MSVC)~endif(UNIX)までの行を削除。

ビルドと動作確認

初めの一回は、Choreonoid ディレクトリで ccmake を実行する。

 % ccmake .

GRASP_PLUGINS に GripperManipulation を追加して、configure と generate を実行すると、Makefile が作成される。
make して、lib/choreonoid-0.9/libCnoidGripperManipulationPlugin.so が無事にビルドされたら、動作確認を行う。

Choreonoid と RT System Editor を起動して、NameServer の127.0.0.1 の(ホスト名)|host_cxtの下に、GripperManipulation0|rtc があることを確認する。
これを System Diagram 上にドラッグ&ドロップできれば成功である。
SystemDiagram_0.png
(ただし、これを他の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における手首の力センサを仮定する場合

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 から Choreonoid 1.4へのアップデートについて

Choreonoid 1.3から以下の点が変更されています。

干渉チェック
  • .h


 ColdetLinkPairUpdateCheckPtr   ->   cnoid::ColdetLinkPairPtr

  • .cpp


 #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



JointPath の設定


 #include <cnoid/JointPath>



 #ifdef  CNOID_10_11_12_13
    fing_path = body->getJointPath(palm, tip);
 #else
    fing_path = make_shared< JointPath >(palm, tip);
 #endif


ItemList関連


 #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


LinkやJointクラスのメンバ変数名


pがp()に変更
RがR()に変更
qがq()に変更