JAXON2サンプルでZMP seqが読み込めない

Japanese (日本語)
1

以下に従ってJAXON2のサンプルを実行しています．
https://choreonoid.org/ja/documents/latest/tips/jaxon2-sample.html

StandingStabilizer.cnoidは実行できましたが，WalkPatternControllerとWalkStabilizerだとZMP seqに関するエラーになりましたので，その旨をご報告させていただきます．

release-2.2，WSL2上のUbuntu20.04にて以下を実行．
choreonoid Jaxon2WalkPatternController.cnoid
シミュレーション開始ボタンを押す

メッセージビューの出力
Eigenライブラリバージョン3.3.7が使用されています．( 使用されているSIMD命令セット: SSE, SSE2 )
CustomizedSpringModel用のボディカスタマイザ “XXXX/devel/lib/choreonoid-2.2/customizer/SpringModelCustomizer.so” がロードされました．
Labo1用のボディカスタマイザ “XXXX/devel/lib/choreonoid-2.2/customizer/Labo1Customizer.so” がロードされました．
Bodyプラグインが読み込まれました．
Assimpプラグインが読み込まれました．
PoseSeqプラグインが読み込まれました．
Pythonプラグインが読み込まれました．
PythonSimScriptプラグインが読み込まれました．
URDFプラグインが読み込まれました．
Balancerプラグインが読み込まれました．
プロジェクトファイル “choreonoid/ext/jaxon2_sample/sample/Jaxon2WalkPatternController.cnoid” を読み込み中…
OpenGL 3.1 Mesa 21.2.6 is available for the “シーン” view.
Driver profile: Microsoft Corporation D3D12 (NVIDIA GeForce RTX 3090 Ti).
WorldItem “World” を読み出し中
BodyItem “JAXON2” を読み出し中
ボディ “XXXX/devel/share/choreonoid-2.2/JAXON2/model/JAXON2/JAXON2.body” を読み込み中
→ 完了!
SimpleControllerItem “WalkPatternController” を読み出し中
WalkPatternControllerのコントローラモジュール “XXXX/devel/lib/choreonoid-2.2/simplecontroller/Jaxon2WalkPatternController” をロード中･･･OK!
コントローラインスタンスを生成しました．
BodyItem “Floor” を読み出し中
ボディ “XXXX/devel/share/choreonoid-2.2/model/misc/floor.body” を読み込み中
→ 完了!
AISTSimulatorItem “AISTSimulator” を読み出し中
5 / 5 のアイテムが読みこまれました．
プロジェクト “choreonoid/ext/jaxon2_sample/sample/Jaxon2WalkPatternController.cnoid” を完全に読み込みました．
XXXX/devel/share/choreonoid-2.2/JAXON2/motion/JAXON2/SampleWalkPattern.seq
A valid ZMP seq is not available.
エラー： WalkPatternControllerのinitializeメソッドが失敗しました．
エラー： 稼働可能なコントローラが無いためシミュレーションを開始できません．
エラー： AISTSimulatorのシミュレーションの初期化ができませんでした．

2

不具合をご指摘いただきありがとうございます。
私の方で症状も確認しまして、修正版をアップしました。
jaxon2-draftリポジトリを最新版に更新してお試しください。

不具合がしばらく放置されたままになっており、申し訳ありませんでした。

以下に補足として今回の不具合の原因と修正内容について記します。

不具合については、ZMPデータの取得の仕方が本来使用すべき関数でなかったことにありました。具体的には、ZMPデータの取得を

zmpseq = motion.extraSeq<Vector3Seq>("ZMPSeq");

というコードで行っていましたが、よりZMPに特化した関数を用いて、

zmpSeq = getZMPSeq(motion);

とすることで、確実にデータを取得することができます。

Choreonoidのバージョンアップにともなって内部実装が変わったところがあったのですが、上記の古いコードではそれに対応できないようになっていました。今回の修正ではこのコードを置き換えることでZMP読み込みのエラーを解消しました。（なお、Jaxon2WalkPatternControllerについてはそもそもZMPデータを参照していませんでしたので、データの取得自体をやめるようにしました。Jaxon2WalkStabilizerについてはこの修正で動くようになりました。）

また、他に内部実装が変わった点として、BodyMotionクラスに含まれる関節変位軌道やリンク位置軌道のデータについても、データ構造が変わっています。これについて、以前は関節各変異軌道はBodyMotionクラスの

std::shared_ptr<MultiValueSeq> jointPosSeq();

によってMultiValueSeq型のデータとして取得することができました。同様にリンク位置軌道データは

std::shared_ptr<MultiSE3Seq> linkPosSeq();

によって取得することができました。

改良版では、各時刻フレームにおける関節変位とリンク位置のデータがBodyStateというひとつのオブジェクトに格納されるようになり、その時系列データであるBodyStateSeqを

std::shared_ptr<BodyStateSeq> stateSeq()

によって取得することができます。

この新しいデータ構造の特徴として以下があります。

  • 関節変位とリンク位置の両方を格納し、BodyStateごとに別オブジェクトになることで、キャッシュ効率が改善する

  • ひとつのBodyStateに複数物体の状態を格納することができ、さらに物体の数は時刻フレームごとに変化させることができる。これにより、同じ形状の物体がシミュレーション中に増えたり減ったり無くなったりするようなシミュレーションにも対応できる。

後者の機能がChoreonoidのある応用で必要となったので、このような改良をしました。

古いjointPosSeqやlinkPosSeqについても、互換性のため利用できるようにしてあります。ただし、BodyMotionをファイルから読み込むと、デフォルトではBodyStateSeqとして読み込まれており、jointPosSeqやlinkPosSeqにはアクセスできません。これを行なうためには、BodyMotionの

void updateJointPosSeqWithBodyStateSeq()
void updateLinkPosSeqWithBodyStateSeq()
void updateLinkPosSeqAndJointPosSeqWithBodyStateSeq()

といった関数を実行します。これらの関数で、bodyStateSeqの内容がjointPosSeqやlinkPosSeqに変換されて、利用できるようになります。

逆に

void updateBodyStateSeqWithLinkPosSeqAndJointPosSeq()

によって、jointPosSeqとlinkPosSeqの内容からbodyStateSeqの内容を更新することもできます。

この新しいBodyMotionでは通常はbodyStateSeqを使用することが望ましいので、Jaxon2サンプルのコントローラについても、そうするように修正しておきました。

あとはプロジェクトファイルの方で、AISTSimulatorItemの設定が古いものとなっていたので、新しいデフォルト設定に更新しました。これにより、シミュレーションの（内部計算の）速度が向上しています。

3

こちらでも動作することが確認できました．
ご対応ありがとうございます．
経緯と対応についても丁寧に解説してくださりありがとうございます．