SimpleControlerとBodyHandlerの併用について

初めて質問させていただきます.
大学の研究にてChoreonid-2.0.0を使わせていただいております.
SimpleControllerとBodyHandlerの併用したいと考えており,確認のためClosedLinkSampleにjoint1のみを回転させるSimpleControllerを追加してシミュレーションを実行したところ,BodyHandlerが動作せず閉リンク機構を保てなくなります.
BodyHandlerで使用しているupdateLinkedJointDisplacementsがシミュレーション中に動作していないことが原因だと思うのですが,シミュレーションを行いながらBodyHandlerを同時に用いることは可能でしょうか.
ご回答いただけると幸いです.よろしくお願いいたします.

閉リンクモデルへの対応は、現状では以下の2つに分けて考える必要があります。

  1. GUI上でモデルを操作する際の運動学計算
  2. 動力学シミュレーションを行う際の拘束

ご使用いただいているBodyHandlerというのは、sample/general ディレクトリの “ClosedLinkSampleHandler.cpp” のことかと思うのですが、こちらは上記の1に対応するものとなっています。正確には、ボディハンドラとして実装できるインタフェースクラスがいくつかあり、その中のLinkedJointHandlerというクラスを継承することで、GUI上で関節角度を変化させた際に、閉リンクとして連動して動く関節の変位を計算する処理となっています。このハンドラが有効になっていると、シーンビュー上でリンクをドラッグして動かしたり、関節変位ビュー上でスライダや数値ボックスで関節角度を変化させたりした際に、閉リンクの拘束で連動して動く関節の角度も計算され、整合性のある姿勢をとれるようになります。

一方で、動力学シミュレーションについては、これとは別のかたちで閉リンクを実現するようになっています。ClosedLinkSampleについても、その設定が入っていて、本来は動力学シミュレーションにおいても閉リンク部分もつながったまま動くようになっています。既に試されたかとは思うのですが、sample/general/ClosedLinkSample.cnoidのプロジェクトを読み込んでシミュレーション実行ボタンを押すと、このことを確認できるはずです。

そのような、動力学シミュレーションで閉リンクを実現する設定がどこに入っているかと言うと、Bodyファイルで記述されています。具体的には、ClosedLinkSampleで言うと、そのモデルのBodyファイルが"share/model/misc/ClosedLinkSample.body"となりますが、その中の最後の方に

extraJoints:
  -
    link1Name: J1
    link2Name: J3
    jointType: piston
    jointAxis: [ 0, 0, 1 ]
    link1LocalPos: [ 0.2, 0, 0 ]
    link2LocalPos: [ 0, 0.1, 0 ]

という記述があるかと思います。

ここでは追加の拘束を設定していて、J1リンクのローカル座標[ 0.2, 0, 0 ]の位置と、J3リンクのローカル座標[0, 0.1,0 ]の位置を、J1の座標系で[0, 0, 1]の方向を軸として、ピストンタイプの拘束を入れるということをしています。動力学シミュレーションでは、この追加拘束の設定が読み込まれて、それが内部の物理エンジンの拘束として組み込まれて、結果的に動力学シミュレーションでも閉リンクが実現されるようになっています。

ただしこのあたりはまだ実験的な実装となっているところがありまして、シミュレータのタイプ(AISTSimulatorItemとかODESimulatorItemとか)にもよりますが、指定できるjointTypeの種類があまり多くなく、少し偏っています。

Bodyファイルで現在対応しているタイプは"src/Body/StdBodyLoader.cpp"の StdBodyLoader::Impl::readExtraJoint(Mapping* info)にて、以下のように実装されています。

    string jointType;
    if(!extract(info, { "joint_type", "jointType" }, jointType)){
        info->throwException(_("The joint type must be specified with the \"joint_type\" key"));
    }
    if(jointType == "fixed"){
        joint->setType(ExtraJoint::Fixed);
    } else if(jointType == "hinge"){
        joint->setType(ExtraJoint::Hinge);
    } else if(jointType == "ball"){
        joint->setType(ExtraJoint::Ball);
    } else if(jointType == "piston"){
        joint->setType(ExtraJoint::Piston);
    } else {
        info->throwException(format(_("Joint type \"{}\" is not available"), jointType));
    }

そして、実際に動力学シミュレーションで対応している拘束タイプは、例えばAISTSimulatorItemの場合は、"src/Body/ConstraintForceSolver.cpp"のConstraintForceSolver::Impl::initExtraJoint(ExtraJoint* extraJoint)にて、以下のように実装されています。

    if(extraJoint->type() == ExtraJoint::Piston){
        linkPair->constraintPoints.resize(2);
        auto R = extraJoint->localRotation(0);
        linkPair->jointConstraintAxes[0] = R.col(1); // Y
        linkPair->jointConstraintAxes[1] = R.col(2); // Z

    } else if(extraJoint->type() == ExtraJoint::Ball){
        linkPair->constraintPoints.resize(3);
        linkPair->jointConstraintAxes[0] = Vector3(1.0, 0.0, 0.0);
        linkPair->jointConstraintAxes[1] = Vector3(0.0, 1.0, 0.0);
        linkPair->jointConstraintAxes[2] = Vector3(0.0, 0.0, 1.0);
   }

同様にODESimulatorItemの場合は、"src/ODEPlugin/ODESimulatorItem.cpp"のODEBody::setExtraJoints(bool doFlipYZ)関数にて、以下のように実装されています。

        if(extraJoint->type() == ExtraJoint::Piston){
            jointID = dJointCreatePiston(worldID, 0);
            dJointAttach(jointID, odeLinkPair[0]->bodyID, odeLinkPair[1]->bodyID);
            dJointSetPistonAnchor(jointID, p.x(), p.y(), p.z());
            dJointSetPistonAxis(jointID, a.x(), a.y(), a.z());

        } else if(extraJoint->type() == ExtraJoint::Ball){
            jointID = dJointCreateBall(worldID, 0);
            dJointAttach(jointID, odeLinkPair[0]->bodyID, odeLinkPair[1]->bodyID);
            dJointSetBallAnchor(jointID, p.x(), p.y(), p.z());
        }

AGXSimulatorItemの場合は、もう少し多くの種類に対応していたかと思います。

現状ではそのようなかたちで、まだ作り込みが十分できていないところがあるのですが、とりあえずある一点で位置のみ拘束するボールジョイントタイプと、ある軸に沿った並進方向にしか動かなくするピストンタイプの拘束は使えるようになっているので、それで間に合う場合は、お使いいただけるかと思います。

上記の1については、産ロボ用途での需要があったため、1年ほど前に導入した仕組みなのですが、まだドキュメントでも説明できていなくて、申し訳ないです。こちらもうまく使えばChoreonoidのGUIで閉リンクモデルを扱えるようになるので、便利です。今後ドキュメントも含めて整備できればと思います。

ご返信が遅れてしまい申し訳ございません.
回答ありがとうございます.追加で,三点質問があります.

一つ目に確認なのですが,ドキュメントやBodyファイルにはextra jointで “hinge” が設定できるようになっていますが,AISTSimulatorItemとODESimulatorItemにおいては使用することができないということでよろしいでしょうか.

二つ目に,以下の動画のようにsample/general ディレクトリのClosedLinkSample.cnoidで動作を確認することができました.

この閉リンク機構をSimpleControllerを用いていて動かしたいのですが,J0を回転させるSimpleControllerを使用した際に以下の動画のようにextra jointで拘束しているはずの箇所が外れてしまいます.J3が回転していることからexra jointによる拘束が機能していないわけではないと思うのですが,SimpleControllerの実装方法などに問題があるのでしょうか.

実際にシミュレーションで使用したCnoidファイル,Bodyファイル,SimpleControllerを添付いたします.

最後に,バネとダンパを有する受動関節を研究で用いたいと考えており,ドキュメントに受動関節についての記述が見当たらなかったため,閉リンク機構を確認しておりました.現在実装されている機能の中で受動関節の代替となるものはございますでしょうか.また,今後受動関節を実装する予定はございますでしょうか.

一度に複数の質問となってしまい恐縮ですが,よろしくお願いいたします.