初めて質問させていただきます．
大学の研究にてChoreonid-2.0.0を使わせていただいております．
SimpleControllerとBodyHandlerの併用したいと考えており，確認のためClosedLinkSampleにjoint1のみを回転させるSimpleControllerを追加してシミュレーションを実行したところ，BodyHandlerが動作せず閉リンク機構を保てなくなります．
BodyHandlerで使用しているupdateLinkedJointDisplacementsがシミュレーション中に動作していないことが原因だと思うのですが，シミュレーションを行いながらBodyHandlerを同時に用いることは可能でしょうか．
ご回答いただけると幸いです．よろしくお願いいたします．
閉リンクモデルへの対応は、現状では以下の２つに分けて考える必要があります。
- GUI上でモデルを操作する際の運動学計算
- 動力学シミュレーションを行う際の拘束
ご使用いただいている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については、産ロボ用途での需要があったため、１年ほど前に導入した仕組みなのですが、まだドキュメントでも説明できていなくて、申し訳ないです。こちらもうまく使えばChoreonoidのGUIで閉リンクモデルを扱えるようになるので、便利です。今後ドキュメントも含めて整備できればと思います。
ご返信が遅れてしまい申し訳ございません．
回答ありがとうございます．追加で，三点質問があります．
一つ目に確認なのですが，ドキュメントやBodyファイルにはextra jointで “hinge” が設定できるようになっていますが，AISTSimulatorItemとODESimulatorItemにおいては使用することができないということでよろしいでしょうか．
二つ目に，以下の動画のようにsample/general ディレクトリのClosedLinkSample.cnoidで動作を確認することができました．
この閉リンク機構をSimpleControllerを用いていて動かしたいのですが，J0を回転させるSimpleControllerを使用した際に以下の動画のようにextra jointで拘束しているはずの箇所が外れてしまいます．J3が回転していることからexra jointによる拘束が機能していないわけではないと思うのですが，SimpleControllerの実装方法などに問題があるのでしょうか．
実際にシミュレーションで使用したCnoidファイル，Bodyファイル，SimpleControllerを添付いたします．
- ClosedLinkSample.cnoid (9.6 KB)
- ClosedLinkSample.body (5.0 KB)
- closed_link_controller.zip (756 Bytes)
最後に，バネとダンパを有する受動関節を研究で用いたいと考えており，ドキュメントに受動関節についての記述が見当たらなかったため，閉リンク機構を確認しておりました．現在実装されている機能の中で受動関節の代替となるものはございますでしょうか．また，今後受動関節を実装する予定はございますでしょうか．
一度に複数の質問となってしまい恐縮ですが，よろしくお願いいたします．
回答が遅くなりましてすみません。
一つ目に確認なのですが，ドキュメントやBodyファイルにはextra jointで “hinge” が設定できるようになっていますが，AISTSimulatorItemとODESimulatorItemにおいては使用することができないということでよろしいでしょうか．
はい、大変申し訳ないのですが、現状ではそのようになります。
この閉リンク機構をSimpleControllerを用いていて動かしたいのですが，J0を回転させるSimpleControllerを使用した際に以下の動画のようにextra jointで拘束しているはずの箇所が外れてしまいます．J3が回転していることからexra jointによる拘束が機能していないわけではないと思うのですが，SimpleControllerの実装方法などに問題があるのでしょうか．
コントローラのソースコードについて、zipファイルとなっていましたが、分かりやすくなるようここにはっておきますね。
#include <cnoid/SimpleController>
#include <vector>
#include <iostream>
#include <Eigen/Dense>
using namespace cnoid;
class closed_link_controller : public SimpleController {
Link* joint0;
double dt;
double t = 0;
double tf = 10;
private:
double q0;
Eigen::Vector3d des, par;
Eigen::Vector4d a;
public:
virtual bool initialize(SimpleControllerIO *io) override;
virtual bool control() override;
Eigen::Vector3d makeSpline3(double, double, double, double, double, double);
};
bool closed_link_controller::initialize(SimpleControllerIO* io){
dt = io->timeStep();
joint0 = io->body()->joint(0);
joint0->setActuationMode(Link::JointAngle);
io->enableIO(joint0);
q0 = joint0->q();
return true;
}
bool closed_link_controller::control(){
double q = joint0->q();
if(t < tf)
des = makeSpline3(t, tf, q0, 0, 10 * M_PI, 0);
joint0->q_target() = des(0);
t = t + dt;
return true;
}
Eigen::Vector3d closed_link_controller::makeSpline3(double t, double tf, double xi0, double dxi0, double xif, double dxif){
a <<
xi0,
dxi0,
(-3.*xi0 - 2.*dxi0*tf + 3.*xif - dxif*tf) / (tf*tf),
(2.*xi0 + dxi0*tf - 2.*xif + dxif*tf) / (tf*tf*tf);
par(0) = a(0) + a(1)*t + a(2)*t*t + a(3)*t*t*t;
par(1) = a(1) + 2.*a(2)*t + 3.*a(3)*t*t;
par(2) = 2.*a(2) + 6.*a(3)*t;
return par;
}
CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(closed_link_controller)
動力学シミュレーションでは、基本的にはトルクで制御する必要があります。（使用する物理エンジンやその設定によっては速度や位置で制御できるものもあります。）このコードでは、
joint0->setActuationMode(Link::JointAngle);
として、joint0を関節角度（つまり位置）で制御するモードにしていて、
joint0->q_target() = des(0);
で関節角度の指令値であるq_target()に値を設定して制御しようとしています。
しかし閉ループがある場合はその部分の拘束力を計算しながらシミュレーションをしていることもあり、位置制御ではうまくいきません。この場合はトルク制御を使用するようにしてください。
正確に言うと、この場合は少し話がややこしくなっておりまして、AISTSimulatorItemには位置制御をしようとするとその部分だけ「ハイゲインモード」と呼ばれる計算方法に切り替わる機能があります。するとその部分は与えた位置指令を厳密に再現しようとするのですが、その際にそれと整合のとれる加速度をフリーとなっているリンク（例えばルートリンクが固定されていないモデルの場合のルートリンクなど）に設定するようになっていて、今回のケースのようにフリーとなる部分がない場合は計算が破綻します。これについては、特殊なシミュレーション方法になりますので、あまり気にせず、AISTSimulatorItemでは基本的にトルク指令を使う、と認識いただければよいかと思います。（このようなシミュレーション方法は二足歩行ロボットの動作が転倒しないかどうか簡易的に確認する場合などに有効で、元々このシミュレータは産総研で二足歩行ロボットを研究するために開発されたので、そのような機能が入っています。）
最後に，バネとダンパを有する受動関節を研究で用いたいと考えており，ドキュメントに受動関節についての記述が見当たらなかったため，閉リンク機構を確認しておりました．現在実装されている機能の中で受動関節の代替となるものはございますでしょうか．また，今後受動関節を実装する予定はございますでしょうか．
受動関節というのは、アクチュエータをもたないフリーの関節と考えてよろしいでしょうか？そうでしたら、コントローラからトルク指令を与えなければ、そのような動作となります。
お忙しい中、ご回答いただきありがとうございます。
extra jointと受動関節について承知いたしました。
SimpleControllerについては、ご教授いただいた内容を確認してみます。
ご教授いただいた通り位置制御からトルク制御へ変更した結果、以下の動画のようにSimpleControllerでClosedLinkSample.cnoidを動作させることができました。
お忙しい中、詳細な説明をありがとうございます。
今後ともよろしくお願いいしたします。
うまく動いたようで良かったです！
ご報告ありがとうございました。