概要

OpenHRPには、モデル固有のプログラムを、動的に動力学計算ライブラリに組み込む仕組みとして、"BodyCustomizer"というプラグインを用意しています。
このプラグインを使用すると、あるモデルに対して解析的な逆運動学や関節のバネダンパモデルを組み込むことができます。
ここでは、サンプルプロジェクトspringJoint.xmlを用いて、関節をバネダンパモデル化する手順を説明します。

モデルの作成

サンプルでは、2つの箱（リンク）の間に、バネダンパモデルの関節が定義されている、springJointという名前を付けたモデルを使用します。
ファイルは、(openHRP)/sample/model/springJoint.wrlです。
通常の関節と同様にバネダンパ化する関節を定義し、バネダンパの変位の方向を設定します。制御対象ではない関節なので、JointIDは設定しなくてもかまいません。
この関節には、SPRING_JOINTという名前を付けています。

DEF springJoint Humanoid {
  version "1.1"
  humanoidBody [
    DEF ROOT Joint {
        :
        :
        DEF SPRING_JOINT Joint {
            translation     0 0 0.05
            jointType "slide"
	        jointAxis 0 0 1
                :
                :

プラグインの作成

ソースコード

次に、作成したモデル用のプラグインを作成します。サンプルのソースコードは（OpenHRP)/sample/example/customizer/customizer.cppです。

static const char** getTargetModelNames()
{
    static const char* names[] = { 
        "springJoint",
        0 };
	
    return names;
}

このプラグインが、どのモデルに適用されるかを動力学ライブラリに知らせる関数です。モデル定義のファイル中に設定されている名前を羅列した配列を返します。
GrxUIで読み込んだモデルにつける名前ではありませんので注意してください。
（OpenHRPでは、同じロボットが複数台ある場合に、個々のロボットを識別するためにファイル中の名前とは別のモデル名を付けられるようになっています。
同じロボットの個々に、プラグインを指定したい場合は、モデルファイルから別に定義する必要があります。）
配列の最後には'0'を入れます。

static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
{
    Customizer* customizer = 0;
	
    string name(modelName);
    if(name == "springJoint"){
        customizer = new Customizer;
        customizer->bodyHandle = bodyHandle;
        customizer->springT = 1.0e3;    
        customizer->dampingT = 1.0e1;
        int jointIndex = bodyInterface->getLinkIndexFromName(bodyHandle, "SPRING_JOINT");
        if(jointIndex >=0 ){
            JointValSet& jointValSet = customizer->jointValSet;
            jointValSet.valuePtr = bodyInterface->getJointValuePtr(bodyHandle, jointIndex);
            jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(bodyHandle, jointIndex);
            jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(bodyHandle, jointIndex);
        }
    }

    return static_cast<BodyCustomizerHandle>(customizer);
}

createがプラグインの最初に呼び出される関数です。変数の初期化などを行います。
ここでは、プラグインから、動力学ライブラリの変数にアクセスする方法も記述されています。
動力学ライブラリからプラグインに対して提供される関数は以下の通りです。

getLinkIndexFromName関節の名前から、ライブラリ中での識別番号に変換します。
getLinkName識別番号から、関節の名前に変換します。
getJointValuePtr識別番号の示す関節の位置を表す変数のポインタを返します。
getJointVelocityPtr識別番号の示す関節の速度を表す変数のポインタを返します。
getJointForcePtr識別番号の示す関節のトルクを表す変数のポインタを返します。

static void destroy(BodyCustomizerHandle customizerHandle)
{
    Customizer* customizer = static_cast<Customizer*>(customizerHandle);
    if(customizer){
        delete customizer;
    }
}

destroyは、プラグインが破棄されるときに呼び出される関数です。後処理を行います。 

extern "C" DLL_EXPORT
NS_HRPMODEL::BodyCustomizerInterface* getHrpBodyCustomizerInterface(NS_HRPMODEL::BodyInterface* bodyInterface_)
{
    bodyInterface = bodyInterface_;

    bodyCustomizerInterface.version = NS_HRPMODEL::BODY_CUSTOMIZER_INTERFACE_VERSION;
    bodyCustomizerInterface.getTargetModelNames = getTargetModelNames;
    bodyCustomizerInterface.create = create;
    bodyCustomizerInterface.destroy = destroy;
    bodyCustomizerInterface.initializeAnalyticIk = 0;
    bodyCustomizerInterface.calcAnalyticIk = 0;
    bodyCustomizerInterface.setVirtualJointForces = setVirtualJointForces;

    return &bodyCustomizerInterface;
}

このプラグインが提供する関数を動力学ライブラリに知らせるための関数です。実装している関数名を記述した構造体を返します。
実装していない関数には、'0'を代入してください。

以上４つの関数は、プラグインのなかで必ず実装する必要があります。 

static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
{
    Customizer* customizer = static_cast<Customizer*>(customizerHandle);
    JointValSet& trans = customizer->jointValSet;
    *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr);
}

動力学ライブラリの積分ループで1ステップ毎に呼び出されます。ここに、バネダンパ化のコードを実装します。

インストール

作成したソースコードをコンパイルして、ライブラリを作成します。この時、ライブラリ名は、必ず***Customizer.so(dll)のようにします。
これを、(OpenHRP3インストール先）/share/OpenHRP-3.1/customizerの下にコピーします。
（サンプルは、OpenHRPのインストール時にインストールされます。）
動力学ライブラリは、このディレクトにある***Customizer.so(dll)という名前のプラグインをすべてロードします。

サンプルの実行

GrxUIを起動し、プロジェクトspringJoint.xmlをloadします。
シミュレーションを開始すると、床の上に落ちた後、上の箱が上下に震動します。

デフォルトのゴムブッシュ用customizer

上記のゴムブッシュをシミュレートするcustomizerサンプルはspringJoint.xml用の記述例でした。 任意の個数・任意の自由度のゴムブッシュをシミュレートするための "BushCustomizer"がデフォルトで読み込まれます。
"BushCustomizer"を使うには、
  1. 使用しているロボットモデルのVRMLファイルにブッシュ用関節を定義し、別ファイルとして保存する
  2. BushCustomizer用パラメータファイルを記述する (confファイル)
  3. BUSH_CUSTOMIZER_CONFIG_PATH環境変数で上記パラメータファイルのパスを指定する
  4. プロジェクトファイルのロボットモデルパスに、ゴムブッシュ有りVRMLを記述する（基本的にはそれ以外のロボットモデルパスはゴムブッシュなしのもので構いません）。
とすることで利用できます。
2のパラメータファイルは以下のような情報を与えてください。
robot_model_nameロボットモデル名
bush_configN個のブッシュのパラメータを空白で区切り記述します。各パラメータは、カンマで"ブッシュ関節名,バネ係数,ダンパ係数"を指定します。slide関節、rotate関節でそれぞれバネ係数は[N/m],[Nm/rad]、ダンパ係数は[N/(m/s)],[Nm/(rad/s)]です。
サンプルとなる記述例は
  • 1のVRMLファイルはsample/model/sample1_bush.wrl (sample1.wrlをゴムブッシュ追加したもの)
  • 2のパラメータファイルはsample/example/customizer/sample1_bush_customizer_param.conf
にあり、２足ロボットの両足首にそれぞれZ軸直動ブッシュ、X・Y軸回転ブッシュが定義されています。 
robot_model_name: SampleRobot
bush_config: RLEG_BUSH_Z,5.0e5,1.0e3 RLEG_BUSH_ROLL,1.0e3,2.5e1 RLEG_BUSH_PITCH,1.0e3,2.5e1 LLEG_BUSH_Z,5.0e5,1.0e3 LLEG_BUSH_ROLL,1.0e3,2.5e1 LLEG_BUSH_PITCH,1.0e3,2.5e1