ボディハンドラ¶

ボディハンドラとは ¶

ボディハンドラは、個々のボディモデルに固有の処理をC++で実装し、モデルに組み込むための仕組みです。BodyHandlerクラスを継承したクラスとして処理を実装し、それを共有ライブラリとしてビルドしておくと、モデルファイルの記述によってモデル読み込み時に共有ライブラリが読み込まれ、ハンドラがボディモデルに組み込まれます。

ボディハンドラで実装できるモデル固有の処理としては、例えば以下のようなものがあります。

特定のロボットの機構を対象とした、解析的な（数値反復によらない）逆運動学計算
閉リンク機構等を対象とした、複数の関節を連動させる処理
関節変位のGUI上での表示・入力方法のカスタマイズ

ボディハンドラはBodyライブラリのレベルで機能する拡張機構であり、Choreonoid本体やプラグインのコードに手を加えることなく、モデル単位で機能を追加できるのが特徴です。組み込まれた処理は、GUI上でのモデルの操作やシミュレーションなど、そのモデルを扱う様々な処理に共通して反映されます。

モデルファイルでの指定 ¶

ボディハンドラは、ボディモデルのファイルにおいて body_handlers キーで指定します。

body_handlers: MyRobotHandler

値にはハンドラの共有ライブラリのファイル名を、拡張子（.so、.dll）を省略した形で記述します。リストとして記述することで、複数のハンドラを指定することも可能です。

ライブラリのファイルは、Choreonoidのプラグインディレクトリ以下の "bodyhandler" ディレクトリ

[Choreonoidインストール先]/lib/choreonoid-x.y/bodyhandler

から検索されます（x.yはChoreonoidのバージョン番号です）。絶対パスで指定することも可能です。

ハンドラの読み込みに成功すると、モデルの読み込み時にメッセージビューへ以下のようなメッセージが出力されます。

ボディハンドラ HRP4CHandler が "/usr/local/lib/choreonoid-2.5/bodyhandler/HRP4CHandler.so" でみつかりました.
ボディハンドラ HRP4CHandler が HRP-4C にロードされました．

標準のハンドラインタフェース ¶

ボディハンドラの機能は、BodyHandlerを継承したインタフェースクラスの仮想関数を実装することで提供します。Bodyライブラリでは標準で以下のインタフェースが用意されており、Choreonoidの対応する処理から利用されます。

CustomJointPathHandler¶

特定のリンク間の関節パス（JointPath）について、独自のJointPathオブジェクトを提供するためのインタフェースです。主に、対象モデルの機構に特化した解析的な逆運動学計算を組み込むために使用します。組み込んだ逆運動学は、GUI上での逆運動学による姿勢操作や、プログラムからのJointPathを用いた逆運動学計算に反映されます。

独自のJointPathクラスの実装には、補助クラスであるCustomJointPathBaseを利用できます。このクラスを基底とすると、逆運動学の計算関数を登録するだけで独自のJointPathを構成することができます。実装例については、後述のサンプル HRP4CHandler が参考になります。

また、独自のJointPathクラスにおいてJointSpaceConfigurationHandlerというインタフェースを併せて実装すると、同一の手先位置・姿勢に対する関節角の解の組（腕の肘の向きや手首の反転などの「形態」）を扱えるようになり、GUI上（リンク位置ビュー等）で形態の確認や切り替えが可能となります。

LinkedJointHandler¶

ある関節の変位に応じて他の関節を連動させるためのインタフェースです。閉リンク機構や、複数の関節が機構的に連動して動くモデルを対象として、関節間の拘束を近似的に実現するために使用します。連動の処理は、関節変位ビューやシーンビュー上での関節操作、運動学シミュレータでの動作などに反映されます。

JointDisplacementPresentationHandler¶

関節の内部的な変位値（回転関節であればラジアン単位の関節角度）と、GUI上で表示・入力する値との間の変換を定義するためのインタフェースです。例えば、機構上は回転関節である軸の変位を、GUI上では機構上の2点間の距離として表示・入力させる、といったことができます。変換は関節変位ビュー等の関節変位を扱うGUIに反映されます。

なお、BodyHandlerは仮想継承を用いる設計となっており、ひとつのハンドラクラスで上記の複数のインタフェースを同時に実装することも可能です。

ボディハンドラの実装 ¶

ボディハンドラは、上記のインタフェースクラス（または直接BodyHandler）を継承したクラスとして実装します。その際、以下の関数をオーバーライドします。

initialize(Body* body, std::ostream& os)

ハンドラがモデルに組み込まれる際に呼ばれる初期化処理です。対象となるリンクの取得や検証等を行います。falseを返すとハンドラは組み込まれません。osに出力したテキストはメッセージビューに表示されます。

clone()

ハンドラオブジェクトの複製を返します。シミュレーションの実行時など、ボディモデルが内部で複製される際には、ハンドラもこの関数によって複製されて引き継がれます。

また、ソースファイルには以下のマクロを記述して、共有ライブラリからハンドラを生成するためのファクトリ関数を定義しておきます。

CNOID_IMPLEMENT_BODY_HANDLER_FACTORY(ハンドラクラス名)

このファクトリ関数では、ハンドラをビルドしたChoreonoidと読み込む側のChoreonoidのバージョンの整合性チェックも行われます。

以下は、Choreonoidのサンプルに含まれるボディハンドラ sample/general/ClosedLinkSampleHandler.cpp の実装です。これは閉リンク機構のサンプルモデル（share/model/misc/ClosedLinkSample.body）を対象に、LinkedJointHandlerを用いて3つの関節を連動させるものです。

#include <cnoid/LinkedJointHandler>
#include <cnoid/Body>
#include <fmt/format.h>

using namespace std;
using namespace cnoid;
using fmt::format;

class ClosedLinkSampleHandler : public LinkedJointHandler
{
public:
    virtual BodyHandler* clone() override;
    virtual bool initialize(Body* body, std::ostream& os) override;
    virtual bool updateLinkedJointDisplacements(Link* masterJoint, double masterJointDisplacement) override;

private:
    Link* joints[3];
};

CNOID_IMPLEMENT_BODY_HANDLER_FACTORY(ClosedLinkSampleHandler)


BodyHandler* ClosedLinkSampleHandler::clone()
{
    return new ClosedLinkSampleHandler(*this);
}


bool ClosedLinkSampleHandler::initialize(Body* body, std::ostream& os)
{
    const char* ids[3] = { "0", "1", "3" };
    for(int i=0; i < 3; ++i){
        string name(format("J{0}", ids[i]));
        joints[i] = body->link(name);
        if(!joints[i]){
            os << name << "is not found." << endl;
            return false;
        }
    }
    return true;
}


bool ClosedLinkSampleHandler::updateLinkedJointDisplacements(Link* masterJoint, double masterJointDisplacement)
{
    if(masterJoint){
        masterJoint->q() = masterJointDisplacement;
    }
    if(!masterJoint || masterJoint == joints[0]){
        joints[1]->q() = -joints[0]->q();
        joints[2]->q() = joints[0]->q();
    } else if(masterJoint == joints[1]){
        joints[0]->q() = -joints[1]->q();
        joints[2]->q() = joints[0]->q();
    } else if(masterJoint == joints[2]){
        joints[0]->q() = joints[2]->q();
        joints[1]->q() = -joints[2]->q();
    }
    return true;
}

このように、initializeで対象の関節を取得しておき、updateLinkedJointDisplacementsで基準となる関節（masterJoint）の変位に応じて他の関節の変位を更新しています。この処理によって、GUI上でいずれかの関節を操作すると、他の関節もそれに連動して動くようになります。

ビルド方法 ¶

ボディハンドラのビルドには、Choreonoidが提供するCMake関数 choreonoid_add_body_handler を使用できます。

choreonoid_add_body_handler(MyRobotHandler MyRobotHandler.cpp)

この関数によって、ハンドラが共有ライブラリとしてビルドされ、上述の "bodyhandler" ディレクトリに出力・インストールされるようになります。この関数は、Choreonoidのソースツリー内（extディレクトリ等）でのビルドに加えて、 find_package(Choreonoid) でChoreonoidを取り込んだ外部のCMakeプロジェクトでも使用できます。

なお、上述のバージョンチェックがあるため、ハンドラは使用するChoreonoid本体と同じバージョンのソース・環境でビルドしておく必要があります。

サンプル ¶

Choreonoidのソースには、ボディハンドラの実装例として以下のサンプルが含まれています。

HRP4CHandler¶

sample/HRP4C/HRP4CHandler.cpp は、ヒューマノイドロボットHRP-4Cのモデル（share/model/HRP4C/HRP4C.body）に、脚部の解析的な逆運動学を組み込むボディハンドラです。CustomJointPathHandlerとCustomJointPathBaseを使用した実装の例となっており、独自のハンドラで解析的な逆運動学を組み込む際の出発点として参考になります。HRP4C.bodyでは body_handlers キーによってこのハンドラが指定されています。

このサンプルはCMakeオプション BUILD_HRP4C_HANDLER に対応しており、これをONにしてビルドすると（デフォルトはOFF）、ハンドラの共有ライブラリがbodyhandlerディレクトリに生成されます。その状態でHRP-4Cのモデルを読み込むとハンドラがロードされ、脚部の関節パスに対する逆運動学計算で解析解が使われるようになります。 sample/HRP4C ディレクトリには、このモデルを使用するサンプルプロジェクトも含まれています。

ClosedLinkSampleHandler¶

ボディハンドラの実装で紹介した sample/general/ClosedLinkSampleHandler.cpp は、閉リンク機構のサンプルモデル（share/model/misc/ClosedLinkSample.body）の関節を連動させるLinkedJointHandlerの例です。

こちらはCMakeオプション ENABLE_SAMPLES がON（デフォルト）であれば標準でビルドされます。ClosedLinkSample.bodyのモデルを読み込んで、GUI上でいずれかの関節を操作すると、関節が連動して動くことを確認できます。

ボディハンドラ¶

ボディハンドラとは¶

モデルファイルでの指定¶

標準のハンドラインタフェース¶