Choreonoid

〜 コレオノイド 〜

目次

Choreonoid の使い方の基礎 Basic usage of Choreonoid

Choreonoid をソースからビルド Building Choreonoid from Source Package

基本的には,ソースのアーカイブ(*.zip)を展開したディレクトリにある INSTALL を参考にする.

Basically, follow the instruction written in INSTALL included in the source archive.

Windows版

ツール/ライブラリのインストール Installing Tools/Libraries

  • Visual Studio (VS) をインストール (ここでは 2010 を仮定). Install Visual Studio (here, we assume the VS 2010).
  • Qt libraries 4.8.0 for Windows (VS 2010) をインストール. Install Qt libraries 4.8.0 for Windows (VS 2010).
  • VS のバージョンを合わせておかないと,エラーが発生することがある. Error may arise if you install a library inconsistent with VS version.
  • 適当なディレクトリに解凍する. Expand the archive into a proper directory.
  • "C:\OpenSceneGraph" においたと仮定. We assume that the archive is expanded in "C:\OpenSceneGraph".

CMake

  • cmake-gui を起動. Launch cmake-gui.
  • Source: Choreonoid のソースアーカイブを展開したディレクトリ (CMakeLists.txt がある) を入力. Input to Source: the directory into which the source archive was expanded; there should be CMakeLists.txt.
  • Build: ソースディレクトリ/build を入力. Input to Build: SourceDirectory/build.
  • Configure をクリック. Click Configure.
  • Visual Studio 10 を選択. Select Visual Studio 10.
  • 「OpenSceneGraph が見つからない」などのエラーが出るので,OSG_DIR に,OpenSceneGraph を解凍したディレクトリのパスを指定する. You may see an error: "Can't find OpenSceneGraph", then specify the directory into which the OpenSceneGraph was expanded for OSG_DIR.
  • バックスラッシュを / に変更したほうがいいかもしれない. Maybe, backslash should be changed to slash.
  • e.g. C:/OpenSceneGraph
  • 再度 Configure をクリック. Again, click the Configure button.
  • Configuring done と表示されたら成功. Successful if the message "Configuring done" is displayed.
  • その他の設定: The other configurations:
  • CMAKE_INSTALL_PREFIX がデフォルトのままだとインストーラ版と紛らわしいから,C:/Program Files (x86)/CnoidX など適当に変更. The default value of CMAKE_INSTALL_PREFIX is confusing with the installer version, so change it to C:/Program Files (x86)/CnoidX or something like that.
  • INSTALL_DEPENDENCIES をチェック. Check INSTALL_DEPENDENCIES.
  • BUILD_GROBOT_PLUGIN をチェック. Check BUILD_GROBOT_PLUGIN.
  • これらの変更をしたら再度 Configure をクリック. After these changing, click Configure again.
  • Generate をクリック. Click the Generate button.
  • Generating done と表示されたら成功. Successful if the message "Generating done" is displayed.
  • cmake-gui を終了. Exit cmake-gui.

Visual Studio でコンパイル Compile with Visual Studio

  • ソースディレクトリ/build/Choreonoid.sln を VS で開く. Open SourceDirectory/build/Choreonoid.sln with VS.
  • 構成マネージャを開いて,リリースモードに変更する(デフォルトはデバッグモード). Open Configuration Manager and change to Release mode (the default is debug mode).
  • ALL_BUILD をビルド. Build ALL_BUILD.
  • ソースの修正(Choreonoid 1.0.0 のみ): Modify the source code (only Choreonoid 1.0.0):

file: src\GuiBase\PluginManager.cpp line:139:

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    nameToActivePluginInfoMap.insert(make_pair("Base", info));

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    nameToActivePluginInfoMap.insert(make_pair(std::string("Base"), info));
  • ALL_BUILD を再ビルド. Build ALL_BUILD again.
  • このビルドは成功するはず. This build should be successful.
  • INSTALL をビルドすると,インストールが行われる. Build INSTALL, then the installation will be done.
  • ただし,インストール先にシステムディレクトリ (e.g. C:/Program Files (x86)/CnoidX) を指定している場合,一般ユーザにインストール権限がないのでエラーとなる.そこで,一般ユーザに C:/Program Files (x86)/CnoidX に対する書き込み権限を与えておく. NOTE: If you are specifying a system directory for the install directory (e.g. C:/Program Files (x86)/CnoidX), an error may arise if an ordinary user has no authority to install. So, set writable that directory for the user.
  • C:/Program Files (x86)/CnoidX/bin/choreonoid.exe を実行. Launch C:/Program Files (x86)/CnoidX/bin/choreonoid.exe.
  • サンプルプロジェクト GR001Sample.cnoid を読み込んでみよう. Load the sample project GR001Sample.cnoid.
  • バランサプラグインをコピーする. Copy the Balancer Plugin.
  • ソースディレクトリ\proprietary\BalancerPlugin\CnoidBalancerPlugin.dll を,インストールディレクトリ\lib\choreonoid-1.1\ にコピーする. Copy SourceDirectory\proprietary\BalancerPlugin\CnoidBalancerPlugin.dll into InstallDirectory\lib\choreonoid-1.1\ .

(ビルド完了) (Build has been completed)

Linux版

ツール/ライブラリのインストール Installing Tools/Libraries

LibraryPackage
CMakecmake cmake-curses-gui cmake-qt-gui
Qtlibqt4-dev
OpenSceneGraph (OSG)libopenscenegraph80 libopenscenegraph-dev
libpnglibpng12-0 libpng12-dev
yamllibyaml-0-2 libyaml-dev

パッケージマネージャか apt で入れる.

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
sudo apt-get -f install PACKAGE_NAMES

CMake

  • 以下の順で実行:
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    unzip choreonoid-1.3.1.zip
    cd choreonoid-1.3.1/
    mkdir build
    cd build/
    cmake-gui ..
  • Click: Configure
  • Click: Generate
    Configuring done
    Generating done
  • build ディレクトリで make を実行
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    make
  • テスト実行:
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    bin/choreonoid
  • バランサプラグインのインストール:
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    cd lib/choreonoid-1.3/
    ln -s ../../../proprietary/BalancerPlugin/libCnoidBalancerPlugin.x86.so libCnoidBalancerPlugin.so
    64ビットOSの場合は x86 を x64 に変更する.

Choreonoid のプラグイン開発ガイド Guide for Making Choreonoid Plugins

詳細な解説は,本家のサイトに記述されている.

The detailed explanation is written in the official site:

手っ取り早く動かしてみたい人は,以下のクイックチュートリアルを参照.

Refer to the following quick tutorial.

クイックチュートリアル Quick Tutorial

cmake-gui で, BUILD_HELLO_WORLD_SAMPLE および BUILD_SAMPLE1_SAMPLE にチェックを入れて Configure, Generate を実行し,Visual Studio でビルド,インストールすると,これらのプラグインがインストールされる.

Launch cmake-gui, check BUILD_HELLO_WORLD_SAMPLE and BUILD_SAMPLE1_SAMPLE, execute Configure and Generate, build it with Visual Studio, and build INSTALL, then, these plugins are installed.

Choreonoid を実行すると,メッセージパネルに以下のように表示される.

Launching Choreonoid, its message panel goes as follows:

 ...
 HelloWorld-plugin has been activated.
 ...
 Sample1-plugin has been activated.
 ...

HelloWorld は,メニューバーの View メニューに "Hello World" という項目を追加する.

The HelloWorld plugin add the "Hello World" item in the View menu.

  • クリックしてみよう.メッセージパネルに "Hello World!" と表示されるはずだ. Click it, then "Hello World" is printed in the message panel.

Sample1 プラグインは,選択中のロボットの関節角をインクリメントしたりデクリメントしたりするボタンを,ツールバーに追加する.

The Sample1 plugin adds two buttons that increments and decrements the joint angles of the selected robot respectively.

  • サンプルプロジェクトを読み込み,ロボット (GR001) を選択. Load the sample project, and select the robot (GR001) in the item panel.
  • ツールバーの Increment / Decrement をクリック. Click Increment / Decrement on the tool bar.
  • ロボットのすべての関節角が増えたり減ったりするはずだ. Then, the every joint angle of the robot increases or decreases.

Sample1 プラグインのソースを適当に変更して INSTALL をビルドすれば,オリジナルのプラグインを作れる.

You can create an original plugin quickly by modifying the source code of the Sample1 plugin, then build INSTALL.

独自プラグインの作成 / Creating an Original Plugin

  • Ver01: Ver.1向けガイド Guide for Ver.1

Sample1 プラグインをベースにして,ロボットのポーズを操作するプラグインを作ってみる.まずは,数値を記入したテキストファイルからポーズデータをロードするというもの.

Based on the Sample1 plugin, we create a new plugin that manipulates the pose of the robot. First, we make one that loads a pose data from a text file in which some values are written.

  • ひな形を作成する. Making a template.
  • extplugin 以下に LoadFromFilePlugin というディレクトリを作る. Make a directory named LoadFromFilePlugin under the extplugin directory.
  • ext/sample/Sample1Plugin/Sample1Plugin.cpp をそのディレクトリにコピー. Copy ext/sample/Sample1Plugin/Sample1Plugin.cpp into that directory.
  • このとき名前は LoadFromFilePlugin.cpp にする. Here, change the name of the copied file to LoadFromFilePlugin.cpp.
  • ext/sample/Sample1Plugin/CMakeLists.txt をそのディレクトリにコピーする.名前は同じでよい. Copy ext/sample/Sample1Plugin/CMakeLists.txt into that directory with the same name.
  • この CMakeLists.txt を以下のように編集する: Edit this CMakeLists.txt as follows:
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
option(BUILD_MYPLUGIN_LOADFROMFILE "Building a LoadFromFile plugin \"LoadFromFilePlugin\"" ON)

if(BUILD_MYPLUGIN_LOADFROMFILE)
  set(target CnoidLoadFromFilePlugin)
  add_cnoid_plugin(${target} SHARED LoadFromFilePlugin.cpp)
  target_link_libraries(${target} CnoidBodyPlugin)
  apply_common_setting_for_plugin(${target})
endif()
  • cmake-gui
  • 起動して Configure をクリックすると BUILD_MYPLUGIN_LOADFROMFILE が表示されるから,チェックが付いていることを確認する. Launch cmake-gui, and clock Configure; then you will find the BUILD_MYPLUGIN_LOADFROMFILE item. Confirm that item is checked.
  • 再度 Configure を実行し,Generate を実行. Execute Configure again, then execute Generate.
  • Build:
    • Visual Studio:
  • 起動して build\Choreonoid.sln を開く. Open build\Choreonoid.sln with Visual Studio.
  • G++:
  • build ディレクトリで make を実行. Execute make at the build directory.
  • INSTALL をビルドすれば,とりあえず LoadFromFilePlugin プラグインが追加されるはず. Build INSTALL, then LoadFromFilePlugin should be added.
  • ただしこのまま Choreonoid を実行しても,既存の Sample1 と競合するというメッセージが表示される. But, if you launch Choreonoid, you will see a message saying that the new plugin conflicts with the existing Sample1 plugin.
 The plugin conflicts with plugin ...
  • LoadFromFilePlugin.cpp を編集しよう. Edit the LoadFromFilePlugin.cpp file.
  • 実装例 / Sample implementation (fileLoadFromFilePlugin.cpp):
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    #include <cnoid/Plugin>
    #include <cnoid/ItemTreeView>
    #include <cnoid/BodyItem>
    #include <cnoid/ToolBar>
    #include <cnoid/MessageView>
    #include <boost/bind.hpp>
    #include <sstream>
    #include <fstream>
    
    using namespace boost;
    using namespace cnoid;
    using namespace std;
    
    void onLoadFromFilePluginButtonClicked(void)
    {
        ItemList<BodyItem> bodyItems =
            ItemTreeView::mainInstance()->checkedItems<BodyItem>();
    
        if(bodyItems.size()==0)
        {
          MessageView::mainInstance()->putln("No body checked");
          return;
        }
    
        string filename("C:\\Users\\akihiko\\Documents\\test1.dat");
        ifstream ifs(filename.c_str());
        if(!ifs)
        {
          MessageView::mainInstance()->putln("File not found: "+filename);
          return;
        }
    
        for(size_t i=0; i < bodyItems.size(); ++i)
        {
            BodyPtr body = bodyItems[i]->body();
            stringstream ss;ss<<"body "<<i<<" : ";
            for(int j=0; j < body->numJoints(); ++j)
            {
                ifs >> body->joint(j)->q;
                ss<<" "<<body->joint(j)->q;
            }
            bodyItems[i]->notifyKinematicStateChange(true);
            MessageView::mainInstance()->putln(ss.str());
        }
    }
    
    class LoadFromFilePlugin : public Plugin
    {
    public:
    
        LoadFromFilePlugin() : Plugin("LoadFromFile") {
            depend("Body");
        }
    
        virtual bool initialize() {
    
            ToolBar* bar = new ToolBar("LoadFromFile");
            bar->addButton("Load")
                ->sigClicked().connect(bind(&onLoadFromFilePluginButtonClicked));
            addToolBar(bar);
    
            return true;
        }
    };
    
    CNOID_IMPLEMENT_PLUGIN_ENTRY(LoadFromFilePlugin)
  • filename は環境に合わせて好きに変更する. Modify filename as you like it.
  • このファイルには以下のようなデータが保存されている必要がある (関節角を表す). In this file, the following data should be stored (denoting the joint angles):
 0 0 0 0.5 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0
  • 実行方法. How to execute:
  • INSTALL をビルドして Choreonoid を実行. Build INSTALL and launch Choreonoid.
  • サンプルプロジェクトを読み込む. Load the sample project.
  • アイテムパネルの GR001 にチェックが付いていることを確認し,Load をクリック. Assure that the GR001 is checked in the item panel, then click the Load button.
  • 上記 filename に保存されているポーズが読み込まれ,ロボットに適用される. The pose data stored in the above filename is loaded, then it is applied to the robot.

リアルタイムにロボットのポーズを変化させるプラグイン A Plugin Manipulating the Pose of a Robot in Real-time

ロボットのポーズをリアルタイムに変化させるには,スレッド処理を行う必要がある.以下の実装例では,Qt ライブラリのスレッドクラス QThread を継承して自分のクラスを作り,さらにスレッドセーフにロボットのポーズを変化させるために callSynchronously 関数を実行しているのがポイント.callSynchronously は対象関数の実行が終わるまで待つが,callLater は対象関数の終了を待たない.

In order to modify the pose of the robot in real-time, we need to use a threading program. In the following example, we create a class by inheriting the thread class in the Qt library, QThread, and use the callSynchronously function to modify the pose of the robot with the thread-safe manner. The function callSynchronously waits the target function ends, while the callLater function does not wait.

  • 実装例 Sample implementation (fileTestMotionPlugin.cpp):
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    #include <cnoid/Plugin>
    #include <cnoid/ItemTreeView>
    #include <cnoid/BodyItem>
    #include <cnoid/ToolBar>
    #include <cnoid/MessageView>
    #include <cnoid/LazyCaller>
    #include <cnoid/Sleep>
    #include <boost/bind.hpp>
    #include <QThread>
    #include <cmath>
    
    using namespace boost;
    using namespace cnoid;
    using namespace std;
    
    
    void FillAngles(BodyItem *body_item, const double &angle)
    {
      BodyPtr body = body_item->body();
      for(int j=0; j < body->numJoints(); ++j)
      {
        body->joint(j)->q= angle;
      }
      body_item->notifyKinematicStateChange(true);
    }
    
    class TMotionThread : public QThread
    {
    public:
      virtual void run();  // overwriting QThread's
    };
    
    void TMotionThread::run()
    {
      ItemList<BodyItem> bodyItems =
          ItemTreeView::mainInstance()->checkedItems<BodyItem>();
    
      if(bodyItems.size()==0)
      {
        MessageView::mainInstance()->putln("No body checked");
        return;
      }
    
      for(double t(0.0);t<100;t+=0.1)
      {
        double angle= 0.5*std::sin(0.1*t);
        MessageView::mainInstance()->cout()<<"angle= "<<angle<<endl;
        // callLater(bind(&FillAngles, bodyItems[0], angle));
        callSynchronously(bind(&FillAngles, bodyItems[0], angle));
        // msleep(10);
      }
    }
    
    class TestMotionPlugin : public Plugin
    {
    public:
    
        TestMotionPlugin() : Plugin("TestMotion") {
            depend("Body");
        }
    
        virtual bool initialize() {
    
            ToolBar* bar = new ToolBar("TestMotion");
            bar->addButton("Motion")
                ->sigClicked().connect(bind(&TMotionThread::start,&thread,QThread::InheritPriority));
            addToolBar(bar);
    
            return true;
        }
    private:
      TMotionThread thread;
    };
    
    CNOID_IMPLEMENT_PLUGIN_ENTRY(TestMotionPlugin)

Choreonoid で kinect プラグインを作る Making a Kinect Plugin for Choreonoid

Kinect でスケルトンを取得し,ロボットのポーズを操作するプラグインを作ってみよう.

Let's make a plugin that observes skeleton with Kinect and modifies the pose of the robot.

作り方は,LoadFromFilePlugin とほぼ同様.異なる点は,CMakeLists.txt と,プラグインのソースファイル HelloKinectPlugin.cpp のみ.

Make the plugin with the same manner as LoadFromFilePlugin. Differences are CMakeLists.txt and the source code of the plugin, HelloKinectPlugin.cpp.

以下は,CMakeLists.txt の例:

The following is a sample of the CMakeLists.txt:

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
option(BUILD_MYPLUGIN_HELLOKINECT "Building an original plugin \"HelloKinectPlugin\"" ON)

# Kinect SDK
set(KINECT_SDK_DIR ${KINECT_SDK_DIR} CACHE PATH "set the directory of the Kinect SDK library")
if(MSVC)
  if(NOT KINECT_SDK_DIR)
    message(FATAL_ERROR "Please specify the Kinect SDK's top directory to KINECT_SDK_DIR.")
  endif()
endif()
if(KINECT_SDK_DIR)
  include_directories(${KINECT_SDK_DIR}/inc)
  link_directories(${KINECT_SDK_DIR}/lib/x86)
endif()

if(BUILD_MYPLUGIN_HELLOKINECT)
  set(target CnoidHelloKinectPlugin)
  set(srcdir ${PROJECT_SOURCE_DIR}/src/MyPlugin)
  add_library(${target} SHARED ${srcdir}/HelloKinectPlugin.cpp)
  target_link_libraries(${target} CnoidBodyPlugin MSRKinectNUI.lib)
  apply_common_setting_for_plugin(${target})
endif()
  • cmake-gui
  • 起動して Configure をクリックすると BUILD_MYPLUGIN_HELLOKINECT が表示されるから,チェックが付いていることを確認する. Launch cmake-gui and click Configure, then BUILD_MYPLUGIN_HELLOKINECT is listed; assure it is checked.
  • KINECT_SDK_DIR に Kinect SDK のインストールディレクトリ (e.g. C:/Program Files/Microsoft SDKs/Kinect/v1.0 Beta2) を指定. Specify the directory in which the Kinect SDK is installed to KINECT_SDK_DIR (e.g. C:/Program Files/Microsoft SDKs/Kinect/v1.0 Beta2).
  • もう一度 Configure を実行. Click Configure again.
  • Generate を実行. Click Generate.
  • Visual Studio
  • HelloKinectPlugin.cpp を編集して,Kinectからスケルトンデータを取得し,ロボットに反映させるコードを書こう. Edit HelloKinectPlugin.cpp so that observing skeleton data and applying it to the robot.
  • 実装例 ):
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    #include <cnoid/Plugin>
    #include <cnoid/ItemTreeView>
    #include <cnoid/BodyItem>
    #include <cnoid/ToolBar>
    #include <cnoid/MessageView>
    #include <boost/bind.hpp>
    #include <QThread>
    #include <cmath>
    #include <map>
    
    #include <windows.h>
    #include <MSR_NuiApi.h>
    #include <MSR_NuiSkeleton.h>
    
    using namespace boost;
    using namespace cnoid;
    using namespace std;
    
    bool ConnectToKinect()
    {
      HRESULT hr= NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON);
      if(FAILED(hr))
      {
        MessageView::mainInstance()->cout()<<"kinect is not initialized!! kinect connected?"<<endl;
        return false;
      }
      return true;
    }
    
    template <typename T>
    inline T Sq(const T &x) {return x*x;}
    
    template <typename T>
    inline T CalcAngle(
        const T &x0,const T &y0,const T &z0,
        const T &x1,const T &y1,const T &z1,
        const T &x2,const T &y2,const T &z2 )
    {
      const T a2= Sq(x1-x0)+Sq(y1-y0)+Sq(z1-z0);
      const T b2= Sq(x2-x0)+Sq(y2-y0)+Sq(z2-z0);
      const T c2= Sq(x2-x1)+Sq(y2-y1)+Sq(z2-z1);
      return std::acos((a2+b2-c2)/(2.0*std::sqrt(a2)*std::sqrt(b2)));
    }
    
    void ApplyJointAngles(BodyItem *body_item, const std::map<int,double> &angles)
    {
      BodyPtr body = body_item->body();
      for(std::map<int,double>::const_iterator itr(angles.begin()),last(angles.end()); itr!=last; ++itr)
      {
        body->joint(itr->first)->q= itr->second;
      }
      body_item->notifyKinematicStateChange(true);
    }
    
    class TCaptureThread : public QThread
    {
    public:
      virtual void run();  // overwriting QThread's
    };
    
    void TCaptureThread::run()
    {
      if(!ConnectToKinect())  return;
    
      ItemList<BodyItem> bodyItems =
          ItemTreeView::mainInstance()->checkedItems<BodyItem>();
    
      if(bodyItems.size()==0)
      {
        MessageView::mainInstance()->cout()<<"No body checked"<<endl;
        return;
      }
    
      NUI_SKELETON_FRAME SkeletonFrame;
    
      MessageView::mainInstance()->cout()<<"Start capturing..."<<endl;
    
      int body_id(0);
      std::map<int,double> joint_angles;
      for(int t(0);t<300;++t)
      {
        NuiSkeletonGetNextFrame(0, &SkeletonFrame);
        int i(0);
        for(; i<NUI_SKELETON_COUNT; ++i)
          if(SkeletonFrame.SkeletonData[i].eTrackingState==NUI_SKELETON_TRACKED)
            break;
        if(i==NUI_SKELETON_COUNT)
        {
          --t;
          continue;
        }
    
        const NUI_SKELETON_DATA &s_data(SkeletonFrame.SkeletonData[i]);
    #define XYG(x_index) s_data.SkeletonPositions[x_index].x, s_data.SkeletonPositions[x_index].y, s_data.SkeletonPositions[x_index].z
        // NUI_SKELETON_POSITION_SHOULDER_RIGHT, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_ELBOW_RIGHT
        double angle_sh_r= -(CalcAngle(XYG(NUI_SKELETON_POSITION_SHOULDER_RIGHT),XYG(NUI_SKELETON_POSITION_SHOULDER_LEFT),XYG(NUI_SKELETON_POSITION_ELBOW_RIGHT))-M_PI/2.0);
        double angle_sh_l= (CalcAngle(XYG(NUI_SKELETON_POSITION_SHOULDER_LEFT),XYG(NUI_SKELETON_POSITION_SHOULDER_RIGHT),XYG(NUI_SKELETON_POSITION_ELBOW_LEFT))-M_PI/2.0);
    #undef XYG
        MessageView::mainInstance()->cout()<<"sholder angle (L,R)= "<<angle_sh_l<<", "<<angle_sh_r<<endl;
    
        joint_angles[15]= angle_sh_r;
        joint_angles[18]= angle_sh_l;
        callSynchronously(bind(&ApplyJointAngles, bodyItems[body_id], joint_angles));
    
        NuiTransformSmooth(&SkeletonFrame,NULL);
      }
    
      MessageView::mainInstance()->cout()<<"...Finished"<<endl;
    }
    
    class HelloKinectPlugin : public Plugin
    {
    public:
    
      HelloKinectPlugin()
          : Plugin("HelloKinect")
        {
          depend("Body");
        }
    
      virtual bool initialize()
        {
          ToolBar* bar = new ToolBar("HelloKinect");
          bar->addButton("CaptureFromKinect")
              ->sigClicked().connect(bind(&TCaptureThread::start,&thread,QThread::InheritPriority));
          addToolBar(bar);
    
          return true;
        }
    
    private:
      TCaptureThread thread;
    };
    
    CNOID_IMPLEMENT_PLUGIN_ENTRY(HelloKinectPlugin)

行列計算 Matrix Operations

Choreonoid は Eigen を行列計算に利用しているため,プラグイン開発ではヘッダを include するだけで Eigen が利用できるようになる.

Choreonoid uses Eigen for matrix operations, so, in developing its plugins, we can use Eigen by just including the header files.

Example :

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
#include <Eigen/Dense>
using namespace Eigen;

SOME_FUNCTION
{
  MatrixXd m(2,3);
  m<< 1.0,  2.0,  3.0,
    -1.0, -2.0, -3.0;
  MessageView::mainInstance()->cout()<<"m= "<< m<<std::endl;
}

GUIを使ったプラグイン A Plugin using GUI

スライダなどのGUI要素を追加する例を紹介する.GUI要素はQtを使って実現される.GLGearsPlugin.cpp を見れば,ビューの追加方法やOpenGLの利用方法がわかる.BodyPlugin の BodyLinkView.cpp あたりを見れば,スライダやラベルの追加方法がわかる.

An example using GUI elements such as a slider is introduced. Qt is employed to make GUI. See GLGearsPlugin.cpp to know how to add a view and how to use the OpenGL. See BodyLinkView.cpp of BodyPlugin to know how to add a slider or a label.

  • 実装例 Sample implementation (fileGUITestPlugin.cpp):
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    #include <cnoid/Plugin>
    #include <cnoid/ItemTreeView>
    #include <cnoid/BodyItem>
    #include <cnoid/ToolBar>
    #include <cnoid/MessageView>
    #include <cnoid/Slider>
    #include <QLabel>
    #include <boost/bind.hpp>
    
    using namespace boost;
    using namespace cnoid;
    
    void on_qSliderChanged(int Value, QLabel *Label)
    {
      Label->setText(QString("Index: %1").arg(Value));
    }
    
    class TestView : public cnoid::View
    {
    public:
      TestView()
        {
          setName("TestGUI");
    
          QVBoxLayout* vbox = new QVBoxLayout();
    
          sliderLabel.setText(QString("Index: 0"));
          vbox->addWidget(&sliderLabel);
    
          qSlider.setOrientation(Qt::Horizontal);
          qSlider.setRange(1,10);
          vbox->addWidget(&qSlider);
          qSlider.sigValueChanged().connect(
            bind(&on_qSliderChanged, _1,&sliderLabel));
    
          setLayout(vbox);
        }
    
    protected:
      Slider qSlider;
      QLabel sliderLabel;
    
    private:
    };
    
    
    class GUITestPlugin : public Plugin
    {
    public:
    
      GUITestPlugin() : Plugin("GUITest") {
          depend("Body");
        }
    
      virtual bool initialize()
        {
          addView(new TestView);
          return true;
        }
    };
    
    CNOID_IMPLEMENT_PLUGIN_ENTRY(GUITestPlugin)

逆運動学ソルバを利用するプラグイン A Plugin using an Inverse Kinematics Solver

Choreonoid には逆運動学を解く機能が用意されている.この機能を利用する例を紹介する.BodyLinkViewImpl クラスの doInverseKinematics 関数を見れば,使用方法がわかる.

Choreonoid provides an inverse kinematics solver. An example using it is introduced. See the doInverseKinematics function of the BodyLinkViewImpl class to know how to use the IK solver.

  • 実装例 Sample implementation (fileTestIKPlugin.cpp):
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    #include <cnoid/Plugin>
    #include <cnoid/ItemTreeView>
    #include <cnoid/BodyItem>
    #include <cnoid/ToolBar>
    #include <cnoid/MessageView>
    #include <cnoid/LazyCaller>
    #include <cnoid/Sleep>
    #include <cnoid/PenetrationBlocker>
    #include <boost/bind.hpp>
    #include <QThread>
    #include <cmath>
    
    using namespace boost;
    using namespace cnoid;
    using namespace std;
    
    
    void MoveLink(BodyItem *body_item, int index, const Vector3 &base_pos, const double &time)
    {
      BodyPtr body = body_item->body();
    
      Link *target_link= body->link(index);
      Vector3 p= base_pos+Vector3(0.01*std::sin(0.2*time), 0.0, 0.01*(1.0-std::cos(0.2*time)));
      Matrix3 R= target_link->R;
    
      // ref: BodyLinkViewImpl::doInverseKinematics
      InverseKinematicsPtr ik = body_item->getCurrentIK(target_link);
    
      if(ik)
      {
        body_item->beginKinematicStateEdit();
    
        // block penetration:
        // PenetrationBlockerPtr blocker= body_item->createPenetrationBlocker(target_link, true);
        // if(blocker)
        //   blocker->adjust(p, R, Vector3(p - target_link->p));
    
        if(ik->calcInverseKinematics(p, R))
        {
          body_item->notifyKinematicStateChange(true);
          body_item->acceptKinematicStateEdit();
        }
      }
    }
    
    class TIKMotionThread : public QThread
    {
    public:
      virtual void run();  // overwriting QThread's
      int Index;
    };
    
    void TIKMotionThread::run()
    {
      ItemList<BodyItem> bodyItems =
          ItemTreeView::mainInstance()->checkedItems<BodyItem>();
    
      if(bodyItems.size()==0)
      {
        MessageView::mainInstance()->putln("No body checked");
        return;
      }
    
      Vector3 base_pos= bodyItems[0]->body()->link(Index)->p;
      for(double t(0.0);t<M_PI*20.0;t+=0.2)
      {
        MessageView::mainInstance()->cout()<<"time= "<<t<<endl;
        // callLater(bind(...));
        callSynchronously(bind(&MoveLink, bodyItems[0], Index, base_pos, t));
        // msleep(10);
      }
    }
    
    class TestIKPlugin : public Plugin
    {
    public:
    
      TestIKPlugin() : Plugin("TestIK") {
          depend("Body");
        }
    
      virtual bool initialize()
    	{
          ToolBar* bar = new ToolBar("TestIK");
          bar->addButton("IK0")->sigClicked().connect(bind(&TestIKPlugin::Start,this,0));
          bar->addButton("IK17")->sigClicked().connect(bind(&TestIKPlugin::Start,this,17));
          bar->addButton("IK20")->sigClicked().connect(bind(&TestIKPlugin::Start,this,20));
          bar->addButton("IK6")->sigClicked().connect(bind(&TestIKPlugin::Start,this,6));
          bar->addButton("IK12")->sigClicked().connect(bind(&TestIKPlugin::Start,this,12));
          addToolBar(bar);
    
          return true;
        }
    private:
      TIKMotionThread thread;
      void Start(int index)
        {
    	  thread.Index= index;
    	  thread.start(QThread::InheritPriority);
        }
    };
    
    CNOID_IMPLEMENT_PLUGIN_ENTRY(TestIKPlugin)

新規アイテムの作成

ファイルメニューの「新規」に新しい項目を追加する.これにより,プロパティパネルから値を変更したりできるようになる.

以下は,ポーズが収録されているファイルのパスを指定するアイテムと,そのアイテムからファイルのパスを取得し,読み込んでロボットのポーズを変化させるプラグインの例.

  • 実装例 Sample implementation (fileLoadFromFilePlugin2.cpp):
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    #include <cnoid/Plugin>
    #include <cnoid/ItemTreeView>
    #include <cnoid/BodyItem>
    #include <cnoid/ToolBar>
    #include <cnoid/MessageView>
    #include <cnoid/ItemManager>  // for itemManager
    #include <boost/bind.hpp>
    #include <iostream>
    #include <sstream>
    #include <fstream>
    
    using namespace boost;
    using namespace std;
    
    namespace cnoid {
    
    // new item object
    class LoadFromFileItem : public Item
    {
    public:
      LoadFromFileItem() :
          pose_file_("/home/guest/HRI201201/test1.dat")
        {
        }
      LoadFromFileItem(const LoadFromFileItem& org) :
          Item(org),
          pose_file_(org.pose_file_)
        {
        }
      ~LoadFromFileItem()  {}
    
      const std::string& poseFileName() const {return pose_file_;}
    
    protected:
      virtual ItemPtr doDuplicate() const
        {
          return new LoadFromFileItem(*this);
        }
    
      // add properties
      virtual void doPutProperties(PutPropertyFunction& putProperty)
        {
          using namespace boost;
          putProperty("PoseFile", pose_file_,
                      bind(&LoadFromFileItem::onPoseFilePropertyChanged, this, _1));
        }
    
      bool onPoseFilePropertyChanged(const std::string &new_val)
        {
          MessageView::mainInstance()->putln("pose_file_: changed from "+pose_file_+" to "+new_val);
          pose_file_= new_val;
          return true;
        }
    
    private:
      std::string pose_file_;
    
    };
    
    
    // plugin to add Load2 tool bar
    class LoadFromFile2Plugin : public Plugin
    {
    public:
    
      LoadFromFile2Plugin() :
          Plugin("LoadFromFile2")
        {
          depend("Body");
        }
    
      virtual bool initialize()
        {
    
          ToolBar* bar = new ToolBar("LoadFromFile2");
          bar->addButton("Load2")
              ->sigClicked().connect(bind(&LoadFromFile2Plugin::loadFromFile, this));
          addToolBar(bar);
    
          // add LoadFromFileItem to menu::file>new
          itemManager().registerClass<LoadFromFileItem>("LoadFromFileItem");
          itemManager().addCreationPanel<LoadFromFileItem>();
    
          return true;
        }
    
      void loadFromFile()
        {
          ItemList<BodyItem> bodyItems =
              ItemTreeView::mainInstance()->checkedItems<BodyItem>();
          ItemList<LoadFromFileItem> loadFromFileItems =
              ItemTreeView::mainInstance()->checkedItems<LoadFromFileItem>();
    
          if(bodyItems.size()==0)
          {
            MessageView::mainInstance()->putln("No body checked");
            return;
          }
          if(loadFromFileItems.size()==0)
          {
            MessageView::mainInstance()->putln("No LoadFromFileItem checked");
            return;
          }
    
          const string &filename(loadFromFileItems[0]->poseFileName());
          ifstream ifs(filename.c_str());
          if(!ifs)
          {
            MessageView::mainInstance()->putln("File not found: "+filename);
            return;
          }
    
          for(size_t i=0; i < bodyItems.size(); ++i)
          {
            BodyPtr body = bodyItems[i]->body();
            stringstream ss;ss<<"body "<<i<<" : ";
            for(int j=0; j < body->numJoints(); ++j)
            {
                ifs >> body->joint(j)->q;
                ss<<" "<<body->joint(j)->q;
            }
            bodyItems[i]->notifyKinematicStateChange(true);
            MessageView::mainInstance()->putln(ss.str());
          }
        }
    
    private:
    
    };
    
    
    } // end of cnoid
    
    CNOID_IMPLEMENT_PLUGIN_ENTRY(cnoid::LoadFromFile2Plugin)

ポイントは,Item クラスから継承して追加したいアイテムを作ることと,プラグインクラス側で,itemManager を使ってアイテムをメニューに登録することである.

指定したモーションの実行プラグイン

モーション (PoseSeqItem) を登録しておくと,IDを使ってモーションを再生できるようになるプラグインの実装例.

  • 実装例 Sample implementation (fileMotionList.cpp):
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
    #include <cnoid/Plugin>
    #include <cnoid/ItemTreeView>
    #include <cnoid/BodyItem>
    #include <cnoid/BodyMotionItem>
    #include <src/PoseSeqPlugin/PoseSeqItem.h>  // for PoseSeqItem
    #include <cnoid/ToolBar>
    #include <cnoid/MessageView>
    #include <cnoid/ItemManager>  // for itemManager
    #include <cnoid/Archive>  // save/load properties
    #include <boost/bind.hpp>
    #include <boost/thread/thread.hpp>
    #include <boost/date_time/posix_time/posix_time.hpp>
    #include <QThread>
    #include <iostream>
    #include <sstream>
    #include <fstream>
    
    using namespace boost;
    using namespace std;
    
    namespace cnoid {
    
    static const int NUM_MOTIONS(20);
    
    struct MotionInfo
    {
      MotionInfo() : angles(NULL), timeStep(0.0), numJoints(0), numFrames(0)
        {
        }
      MotionInfo(const MotionInfo &m) : angles(NULL), timeStep(m.timeStep), numJoints(m.numJoints), numFrames(m.numFrames)
        {
          angles= new double[numJoints * numFrames];
          std::copy(m.angles, m.angles + numJoints * numFrames, angles);
        }
      ~MotionInfo()
        {
          clear();
        }
      void clear()
        {
          if(angles)  delete[] angles;
          angles= NULL;
          numJoints= 0;
          numFrames= 0;
          timeStep= 0.0;
        }
      double* angles;
      double timeStep;
      int numJoints;
      int numFrames;
    };
    
    void SetAngles(BodyItem *body_item, const double *angle_begin, const double *angle_end)
    {
      BodyPtr body = body_item->body();
      for(int j=0; angle_begin!=angle_end && j<body->numJoints(); ++j,++angle_begin)
        body->joint(j)->q= *angle_begin;
      body_item->notifyKinematicStateChange(true);
    }
    
    class TMotionThread : public QThread
    {
    public:
      TMotionThread() : mInfo(NULL) {}
      virtual void run();  // overwriting QThread's
      MotionInfo *mInfo;
      BodyItem *bodyItem;
    };
    
    void TMotionThread::run()
    {
      if(mInfo==NULL)
      {
        MessageView::mainInstance()->putln("TMotionThread: mInfo is not assigned");
        return;
      }
      if(bodyItem==NULL)
      {
        MessageView::mainInstance()->putln("TMotionThread: bodyItem is not assigned");
        return;
      }
      if(mInfo->angles==NULL)
      {
        MessageView::mainInstance()->putln("TMotionThread: motion is not loaded");
        return;
      }
    
      for(int t(0); t<mInfo->numFrames; ++t)
      {
        callSynchronously(bind(&SetAngles, bodyItem, &mInfo->angles[t*mInfo->numJoints], &mInfo->angles[(t+1)*mInfo->numJoints]));
        this_thread::sleep(posix_time::microseconds(1.0e6*mInfo->timeStep));
      }
    }
    
    
    // new item object
    class MotionListItem : public Item
    {
    public:
      MotionListItem() : bodyItem(NULL)
        {
        }
      MotionListItem(const MotionListItem& org) :
          Item(org),
          bodyItem(NULL)
        {
        }
      ~MotionListItem()  {}
    
      // based on: GRobotControllerItem.cpp:bool GRobotControllerItem::onPlaybackInitialized(double time)
      void updateMotionList()
        {
          if(!getBodyItem())
          {
            MessageView::mainInstance()->putln("MotionListItem: should be a subitem of BodyItem");
            return;
          }
          ItemList<BodyMotionItem> motionItems= bodyItem->getSubItems<BodyMotionItem>();
          for(int i(0);i<NUM_MOTIONS;++i)
          {
            if(motion_ids_[i]=="")  continue;
            stringstream ss;
            ss<<"Updating motion "<<i<<": "<<motion_ids_[i]<<"..";
            MessageView::mainInstance()->putln(ss.str());
    
            bool found(false);
            for(int m(0);m<motionItems.size();++m)
            {
              if(motion_ids_[i]==motionItems[m]->parentItem()->name())
              {
                MultiValueSeqPtr qseq = motionItems[m]->jointPosSeq();
                if(qseq->numFrames() > 0)
                {
                  found= true;
                  if(setMotion(&qseq->at(0, 0), qseq->numParts(), qseq->numFrames(), qseq->getTimeStep(),i))
                  {
                    #if 0
                    TimeBar* timeBar= TimeBar::instance();
                    playbackConnections.add(
                        timeBar->sigPlaybackStarted().connect(
                            bind(&GRobotControllerItem::onPlaybackStarted, this, _1)));
                    playbackConnections.add(
                        timeBar->sigPlaybackStopped().connect(
                            bind(&GRobotControllerItem::onPlaybackStopped, this)));
                    #endif
                  }
                }
    
              }
            }
            if(!found)
            {
              ss<<".. not found: "<<motion_ids_[i];
              MessageView::mainInstance()->putln(ss.str());
            }
    
          }
    
        }
    
      void playMotion(int m_id)
        {
          stringstream ss;
          ss<<"Playing motion "<<m_id<<": "<<motion_ids_[m_id]<<"..";
          MessageView::mainInstance()->putln(ss.str());
    
    //       ItemList<BodyItem> bodyItems =
    //           ItemTreeView::mainInstance()->checkedItems<BodyItem>();
    //       if(bodyItems.size()==0)
    //       {
    //         MessageView::mainInstance()->putln("No body checked");
    //         return;
    //       }
    
          mthread_.bodyItem= bodyItem;
          mthread_.mInfo= &motions_[m_id];
          mthread_.start();
        }
    
    protected:
      virtual ItemPtr doDuplicate() const
        {
          return new MotionListItem(*this);
        }
    
      void onPositionChanged()
        {
          getBodyItem();
        }
    
      // add properties
      virtual void doPutProperties(PutPropertyFunction& putProperty)
        {
          using namespace boost;
          for(int i(0);i<NUM_MOTIONS;++i)
          {
            stringstream ss;ss<<"Motion"<<i;
            putProperty(ss.str(), motion_ids_[i], bind(&MotionListItem::onMotionPropertyChanged, this, i, _1));
          }
        }
      // save and load properties to/from file
      virtual bool store(Archive& archive)
        {
          for(int i(0);i<NUM_MOTIONS;++i)
          {
            stringstream ss;ss<<"Motion"<<i;
            archive.write(ss.str(), motion_ids_[i]);
          }
          return true;
        }
      virtual bool restore(const Archive& archive)
        {
          for(int i(0);i<NUM_MOTIONS;++i)
          {
            stringstream ss;ss<<"Motion"<<i;
            motion_ids_[i]= archive.get(ss.str(), motion_ids_[i]);
          }
          return true;
        }
    
      bool onMotionPropertyChanged(int m_id, const std::string &new_val)
        {
          motion_ids_[m_id]= new_val;
          return true;
        }
    
    private:
      std::string motion_ids_[NUM_MOTIONS];
      BodyItem *bodyItem;
    
      TMotionThread mthread_;
    
      std::vector<MotionInfo> motions_;
    
      bool getBodyItem()
        {
          bodyItem= findOwnerItem<BodyItem>();
          if(bodyItem)  return true;
          return false;
        }
    
      bool setMotion(double* angles, int numJoints, int numFrames, double timeStep, int id)
        {
          // FOR FUTURE EXTENSION:
          // poseSendingMutex.lock();
          // if(mode == CONTINUOUS_POSE_SENDING)
          // {
          //     poseSendingMutex.unlock();
          //     return false;
          // }
    
          if(id >= static_cast<int>(motions_.size()))
          {
              motions_.resize(id + 1);
          }
          MotionInfo& motion= motions_[id];
          motion.clear();
          motion.angles= new double[numJoints * numFrames];
          std::copy(angles, angles + numJoints * numFrames, motion.angles);
          motion.numJoints= numJoints;
          motion.numFrames= numFrames;
          motion.timeStep= timeStep;
    cerr<<"motions_.size():"<<motions_.size()<<endl;
    cerr<<"&motions_[0]:"<<&motions_[0]<<endl;
    
          // FOR FUTURE EXTENSION:
          // poseSendingMutex.unlock();
          return true;
        }
    
    };
    
    
    class MotionListPlugin : public Plugin
    {
    public:
    
      MotionListPlugin() :
          Plugin("MotionList")
        {
          depend("Body");
        }
    
      virtual bool initialize()
        {
    
          ToolBar* bar = new ToolBar("MotionList");
          bar->addButton("Update")
              ->sigClicked().connect(bind(&MotionListPlugin::updateMotionList, this));
          bar->addButton("Play0")
              ->sigClicked().connect(bind(&MotionListPlugin::playMotion, this, 0));
          bar->addButton("Play1")
              ->sigClicked().connect(bind(&MotionListPlugin::playMotion, this, 1));
          bar->addButton("Play2")
              ->sigClicked().connect(bind(&MotionListPlugin::playMotion, this, 2));
          bar->addButton("Play3")
              ->sigClicked().connect(bind(&MotionListPlugin::playMotion, this, 3));
          addToolBar(bar);
    
          // add MotionListItem to menu::file>new
          itemManager().registerClass<MotionListItem>("MotionListItem");
          itemManager().addCreationPanel<MotionListItem>();
    
          return true;
        }
    
      void updateMotionList()
        {
          MotionListItem *mlist= getCheckedMotionListItem();
          if(mlist) mlist->updateMotionList();
        }
      void playMotion(int m_id)
        {
          MotionListItem *mlist= getCheckedMotionListItem();
          if(mlist) mlist->playMotion(m_id);
        }
    
      static MotionListItem* getCheckedMotionListItem()
        {
          ItemList<MotionListItem> motionListItems =
              ItemTreeView::mainInstance()->checkedItems<MotionListItem>();
    
          if(motionListItems.size()==0)
          {
            MessageView::mainInstance()->putln("No MotionListItem checked");
            return NULL;
          }
          return motionListItems[0];
        }
    
    private:
    
    };
    
    
    } // end of cnoid
    
    CNOID_IMPLEMENT_PLUGIN_ENTRY(cnoid::MotionListPlugin)

Boost::Threadを利用しているので,以下のようにリンクするライブラリを追加する必要がある.

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
option(BUILD_MYPLUGIN_MOTIONLIST "Building a MotionList plugin" ON)

if(BUILD_MYPLUGIN_MOTIONLIST)
  set(target CnoidMotionListPlugin)
  add_cnoid_plugin(${target} SHARED MotionList.cpp)
  target_link_libraries(${target} CnoidBodyPlugin ${Boost_THREAD_LIBRARY} pthread)
  apply_common_setting_for_plugin(${target})
endif()

使い方

  • ロボット (BodyItem) のサブアイテムとして,MotionList を追加する(ファイルメニュー->新規->MotionList).
  • MotionList にチェックを入れておく.
  • MotionList の Motion# にモーション (PoseSeqItem) を追加する.
  • Update をクリックする.
  • Play# で登録したモーションを再生できる.
  • MotionListItem をほかのプラグインから呼び出して,playMotion(ID) でモーションを実行することもできる.
  • 注意: Update をクリックする前に,登録したモーションに対応するボディーモーション (BodyMotionItem) を,「ボディーモーションの生成」ボタンをクリックして,生成しておく必要がある.

キネクトのスケルトントラッキングの結果からモーションを生成するプラグイン

「リアルタイムにロボットのポーズを変化させるプラグイン」を使って人の関節角をロボットに反映させ,リアルタイムにモーションを生成するプラグイン.

準備

choreonoidはver.1.3.1を使用していることを想定している.
  • OpenNIの環境設定 インストールの手順はKinectのページを参照.
    ※注意:OpenNI2.0.0はKinectに対応していないようなので、Kinectを用いる場合は旧バージョンをHistoryページやgitのページから手に入れる必要があることに注意.
  • ディレクトリの作成
  1. choreonoid1.3.1/extplugin に開発するプラグイン用のディレクトリを作成する.ここでは"SkeletonPlugin"というディレクトリを作成したとして進めていく.
    cd ~/choreonoid1.3.1/extplugin
    mkdir SkeletonPlugin
  2. SkeletonPluginのディレクトリに移動
    cd SkeletonPlugin
  3. CMakeLists
    OpenNIをを利用するだけであれば以下のファイルをそのままSkeletonPluginディレクトリに入れる.ビューワやその他ライブラリを利用する場合は適宜中身を書き換える.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
    length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
     1 option(BUILD_MYPLUGIN_SKELETON "Building a Skeleton plugin \"SkeletonPlugin\"" ON)
     2
     3 if(NOT BUILD_SIMPLE_CONTROLLER_PLUGIN)
     4   return()
     5 endif()
     6
     7
     8 ####OpenNI#####
     9
    10 ## Add current Source Dir
    11 #INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
    12 ## Add folder /include
    13 #INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/include)
    14 ###############################################################################
    15 # Find OpenNI
    16 #
    17 # This sets the following variables:
    18 # OPENNI_FOUND - True if OPENNI was found.
    19 # OPENNI_INCLUDE_DIRS - Directories containing the OPENNI include files.
    20 # OPENNI_LIBRARIES - Libraries needed to use OPENNI.
    21 # OPENNI_DEFINITIONS - Compiler flags for OPENNI.
    22
    23 find_package(PkgConfig)
    24 if(${CMAKE_VERSION} VERSION_LESS 2.8.2)
    25  pkg_check_modules(PC_OPENNI openni-dev)
    26 else()
    27  pkg_check_modules(PC_OPENNI QUIET openni-dev)
    28 endif()
    29
    30 set(OPENNI_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER})
    31
    32 #add a hint so that it can find it without the pkg-config
    33 find_path(OPENNI_INCLUDE_DIR XnStatus.h
    34 HINTS /usr/include/ni ${NESTK_ROOT_DIRS_HINTS} ${PC_OPENNI_INCLUDEDIR} ${PC_OPENNI_INCLUDE_DIRS}  /usr/include/openni
    35 PATHS "$ENV{PROGRAMFILES}/OpenNI/Include" "$ENV{PROGRAMW6432}/OpenNI/Include"
    36 PATH_SUFFIXES openni)
    37 #add a hint so that it can find it without the pkg-config
    38 find_library(OPENNI_LIBRARY
    39 NAMES OpenNI64 OpenNI
    40 HINTS ${NESTK_ROOT_DIRS_HINTS} ${PC_OPENNI_LIBDIR} ${PC_OPENNI_LIBRARY_DIRS} /usr/lib
    41 PATHS "$ENV{PROGRAMFILES}/OpenNI/Lib${OPENNI_SUFFIX}" "$ENV{PROGRAMW6432}/OpenNI/Lib${OPENNI_SUFFIX}"
    42 PATH_SUFFIXES lib
    43 )
    44 find_library(NITE_LIBRARY
    45 NAMES XnVNite XnVNITE_1_3_1 XnVNITE_1_4_0 XnVNite_1_4_2 XnVNite_1_5_2
    46 HINTS ${NESTK_ROOT_DIRS_HINTS} ${PC_OPENNI_LIBDIR} ${PC_OPENNI_LIBRARY_DIRS} /usr/lib
    47 PATHS "$ENV{PROGRAMFILES}/PrimeSense/NITE/Lib${OPENNI_SUFFIX}" "$ENV{PROGRAMW6432}/PrimeSense/NITE/Lib${OPENNI_SUFFIX}"
    48 PATH_SUFFIXES lib
    49 )
    50
    51 find_path(NITE_INCLUDE_DIR XnVSessionManager.h
    52 HINTS ${NESTK_ROOT_DIRS_HINTS} ${PC_OPENNI_INCLUDEDIR} ${PC_OPENNI_INCLUDE_DIRS} /usr/include/openni /usr/include/nite
    53 PATHS "$ENV{PROGRAMFILES}/PrimeSense/NITE/Include" "$ENV{PROGRAMW6432}/PrimeSense/NITE/Include"
    54 PATH_SUFFIXES openni)
    55
    56 set(OPENNI_INCLUDE_DIRS ${OPENNI_INCLUDE_DIR} ${NITE_INCLUDE_DIR})
    57 if(APPLE)
    58 set(OPENNI_LIBRARIES ${OPENNI_LIBRARY} ${NITE_LIBRARY} usb)
    59 else()
    60 set(OPENNI_LIBRARIES ${OPENNI_LIBRARY} ${NITE_LIBRARY})
    61 endif()
    62
    63 include(FindPackageHandleStandardArgs)
    64 find_package_handle_standard_args(OpenNI DEFAULT_MSG
    65 OPENNI_LIBRARY OPENNI_INCLUDE_DIR)
    66
    67 mark_as_advanced(OPENNI_LIBRARY OPENNI_INCLUDE_DIR)
    68 if(OPENNI_FOUND)
    69 include_directories(${OPENNI_INCLUDE_DIRS})
    70 link_directories(${OPENNI_LIBRARY})
    71 add_definitions(${OPENNI_DEFINITIONS})
    72 message(STATUS "OpenNI found (include: ${OPENNI_INCLUDE_DIR}, lib: ${OPENNI_LIBRARY})")
    73 endif(OPENNI_FOUND)
    74
    75
    76
    77 if(BUILD_MYPLUGIN_SKELETON)
    78   set(target CnoidSkeletonPlugin)
    79   set(sources SkeletonPlugin.cpp)
    80
    81   add_cnoid_plugin(${target} SHARED ${sources})
    82   target_link_libraries(${target} CnoidBodyPlugin ${OPENNI_LIBRARY})
    83   apply_common_setting_for_plugin(${target})
    84 #install_external_libraries(${ODE_LIBRARY})
    85 endif()
  4. SkeletonPlugin.cpp
    以下のサンプルソースをSkeletonPluginディレクトリに入れる.
  5. SamplesConfig.xml
    KinectのconfigfileをSkeletonPluginディレクトリに入れる
  6. テスト
    cd ~/choreonoid1.3.1/build
    cmake-gui ..
    BUILD_MYPLUGIN_SKELETONにチェックしてビルド
    choreonoidを実行し,画面に"Skeleton"ボタンが追加されていることを確認する.

使い方

  • サンプルソースはRGB-Dカメラから骨格三次元情報を取得し,間接角を計算、それに合わせてロボットの関節角をリアルタイムで更新するプログラムとなっている.
  • 動作生成するロボットのプロジェクト等を読み込む(例:ファイルメニュー->プロジェクトの読み込み->choreonoid1.3.1/share/project/GR001Sample.cnoid)
  • 「Skeleton」ボタンを押してしばらくするとロボットのすべての関節角が0に初期化される.
  • 人のトラッキングが行なわれると、それに合わせてロボットが動く.
    • サンプルでは腕を回す動作、おじぎ、屈伸がトラッキング可能
    • ロボットの切り替えは,SkeltonPlugin.cppの"#define ROBOT"の値を使いたいロボットに切り替えて再度コンパイルする.(0:GROBOT,1:NAO)
    • 新しくロボットの動作を追加する場合は,332行目以降のように,そのロボットに対応した関節のIDに対して角度を入力するようにプログラムを追加する.

キネクトのスケルトントラッキングの結果からモーションを実行させるプラグイン

「指定したモーションの実行プラグイン」とキネクトスケルトントラッキングを組み合わせたもの. 全身のポーズによって指定したモーションを実行させるようなプラグイン.

OpenNI,OpenGL,GLUT,を利用できるようにCmakeLists.txtを変更している.

使い方

  • MotionListに実行させる動作を登録
  • 「Start skeleton Tracking」をクリックすると別ウインドウでデプス画像が表示される.
  • 全身が写ると骨格が検知されてトラッキングが開始される.
  • このとき,ターミナルにポーズの状態が番号で表示され続けている.
  • 「Get Pose and Play Motion」をクリックすると,その時のポーズの状態の番号と同じ番号に登録されたMotionが実行される.

Choreonoid 用モデル作成 Model Making for Choreonoid

  • ここではサンプルのGR001モデルファイルを編集して新たなロボットモデルを作成する方法について説明する.
  • モデル作成の流れは
    • 1 デバッグ用にロボットの各パーツの3Dモデルファイル作成(適当なプリミティブでよい)
    • 2 ロボットの動力学モデルファイル(サンプルではGR001.wrl)の編集
    • 3 choreonoid用設定ファイル.yamlの編集
    • 4 choreonoidで動力学モデルの確認を行いながら,モデルのデバッグ
    • 5 パーツの3Dモデルを詳細なものに編集
  • のようになる

モデルファイルの構成

  • choreonoidを~/cnoidにインストールした場合,サンプルモデルファイルは~/cnoid/share/model/GR001にある
  • GR001/GR001.wrl
    • ロボットの動力学モデルファイル.VRML2.0をOpenHRP用に拡張している.そのためヒューマノイドロボットの記述が簡潔に行えるようになっている.動力学モデルを入れ子構造で記述するのが特徴.
  • GR001/GR001.yaml
    • GR001.wrlをchoreonoidで扱うための情報を記述するファイル.
  • GR001/parts
    • 描画に用いられる3Dモデルが入っているディレクトリ.動力学モデルがパーツごとに記述されるのに合わせて,3Dモデルもパーツ単位で作成する.

Edit VRML File

  • GR001.wrlをテキストエディタで編集する.複雑な入れ子構造を記述する必要があるため,オートインデントや対括弧のジャンプ,対括弧内の非表示機能などの機能のあるエディタで編集を行った方がよい.

ロボットのルート

  • サンプルのモデルはパーツごとの入れ子構造になっている.ロボットのモデルのルートは次のようになっている.MyRobotがロボットの名前である.ロボットのパーツはhumanoidBodyの中に追加していくことになる.また,追加したパーツの名前はjoints,またはsegmentsにリストアップする必要がある.
    DEF MyRobot Humanoid{
        humanoidBody [
           (省略)
        ]
        joints [(省略)]
        segments [(省略)]
    }

パーツの記述

  • 関節の記述は以下の用に行う
  • MyJointがjointの名前である.それ以下に続くのがMyJointのパラメータとなる.
  • children内に関節の先に取り付けられるパーツを記述していく
    DEF MyJoint Joint {
       jointType "rotate"    // 関節タイプ
       jointId 12    // 関節番号(重複しないように)
       jointAxis 0.0 1.0 0.0     // 関節軸
       translation 0.0 0 0.1256    // 親パーツとの相対位置
       ulimit [2.0857]    // 関節角上限
       llimit [-2.0857]    // 関節角下限
       uvlimit [ ]    // 関節角速度上限
       lvlimit [ ]    // 関節角速度下限
       rotorInertia 1.0    // モーターの慣性?
       rotorResistor 1.0    // モーターの粘性?
       children[省略]
    }
  • リンクの記述は以下のように行う
  • MyLinkがリンクの名前である.
    DEF MyLink Segment{
        centerOfMass -0.00001 0.00014 -0.02742    // 親に対するリンクの重心位置
        mass 0.06442    // リンクの質量
        momentsOfInertia [    // リンクの慣性モーメント
            0.00007499295 0.00000000157-0.00000001834
            0.00000000157 0.00007599995 -0.00000005295
            -0.00000001834 -0.00000005295 0.00000553373 ]
            children[ Inline { url "parts/MyLink.wrl" } ]    // リンクの3Dモデルへの相対パス
        }
    }

Edit .yaml File

  • GR001.yamlファイルを基に,パーツの名前を自作の.wrlで追加したロボットのパーツに置き換えていく.
    • OpenHRP規格のVRMLファイル名
      modelFile: "NAO.wrl"
    • Choreonoidの標準ポーズアイコンを押したときのロボットのポーズ.JointIDの順で指定
      standardPose: [0,0,....]
    • Choreonoidのポーズで利用されるロボットのパーツの木構造
      linkGroup: 
       - name: UPPER-BODY
         links: 
           - name: NECK
             links: [ NECK_Y, NECK_P ]
           - name: ARMS
             links:
               - name: R-ARM
                 links: [ R_SHOULDER_P, R_SHOULDER_R, R_ELBOW_Y, R_ELBOW_R, R_WRIST_Y ]
               - name: L-ARM
                 links: [ L_SHOULDER_P, L_SHOULDER_R, L_ELBOW_Y, L_ELBOW_R, L_WRIST_Y ]
       - WAIST
       - name: LOWER-BODY
         links:
           - name: LEGS
             links:
             - name: R-LEG
               links: [ R_HIP_R, R_HIP_YP, R_HIP_P, R_KNEE_P, R_ANKLE_P, R_ANKLE_R ]
             - name: L-LEG
               links: [ L_HIP_R, L_HIP_YP, L_HIP_P, L_KNEE_P, L_ANKLE_P, L_ANKLE_R ]

Making 3D Parts Model

  • Blender2.49で作成したwrlファイル(パーツの3Dモデルファイル,ロボットの動力学用のwrlでないことに注意)がchoreonoidで表示できることを確認している.モデル作成の詳細に関してはwikiのBlenderの項参照.BlenderのバージョンによってはVRML出力が標準で付いていないものもあるため注意が必要.ただし,プラグインとしてVRML出力を使いできるらしい.また,Blenderはバージョン2.5前後でGUIが大きく変わっているので,自分の使うバージョンに適宜読み替えてモデルを作成する.
  • blenderの使い方はここ

Model Check

  • 作成したモデルのチェックはChoreonoidで簡単に行える.
  • Choreonoidで作成したモデルを読み込み(.yamlの方),関節の位置や可動域,また,関節の名前と回転軸の対応などを確認する.この時,モデルビュー画面で関節をマウスで直接操作した場合可動域外まで動いてしまうようなので,関節のスライダで動作チェックを行った方がよい.また,Choreonoidの逆運動学でポーズを作る際にも,得意姿勢付近では計算が不安定になるため,.yamlのstanderd poseに登録した姿勢や初期姿勢などで特異姿勢を避ける工夫が必用である.

Nao for Choreonoid

  • no containts

Naoをコントロールするプラグイン A plugin using Nao


トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2018-08-30 (木) 07:17:04 (75d)