お世話になります．
オムロンの上岡です．
SimpleControllerを使って制御器の実装とテストを行っているのですが，
制御器の中間情報の時系列データ（e.g. COM, ZMP）を保存したいと考えています．
BodyMotionに入れて関節角度データなどと同じ周期で保存できると解析がしやすくなるので望ましいのですが，
ModyMotionへのデータ追加方法が分からないので，可能でしたら方法をご教授ください．
コードを追ってみると現状のデータはSimulationBodyの中でセットされるようなので，
ここを修正すれば所望のことができそうなのは分かるのですが，本体を汚さずにできる方法があると助かります．
よろしくお願い致します．
なるほど、BodyMotionに追加で任意のデータを入れられるようになると、確かに便利になりそうですね。YAMLなのでどこからでも簡単に読み込むことができますしね。
申し訳無いのですが、現状ではこれを行うAPIは用意されていません。
今回の件で使えるかは分かりませんが、BodyMotionとは別に、コントローラから任意のデータをログとして出力して記録するための機能はあります。
まずシンプルコントローラのinitialize関数に与えられるSimpleControllerIOクラスについて、これのベースとなるControllerIOクラスがあります。そちらはChoreonoidの一番大元のコントローラのクラスとなるControllerItemでも使えるクラスとなっています。
そのControllerIOクラス（src/Util/ControllerIO.h)に、以下の関数が定義されています。
これはまずコントローラ（シンプルコントローラ含む）のinitialize関数でio->enableLog()とすることでこの機能を有効化します。そしてcontrol関数内でoutputLogを実行することで、その制御ステップにおける任意のログデータを出力できます。データの型はReferenced*となっていて、これはChoreonoidのスマートポインタの対象となる基底のクラスで、これを継承したものであれば何でもOKです。既存のクラスでも、自前でこのクラスを継承したものでもOKです。例えばsrc/Util/ValueTree.hで定義されている、MappingやListingといったYAMLのノードに対応するクラスについても、このReferencedを継承したものですので、MappingやListingを用いてデータを出力してもよいです。それ以外でも、Referencedを継承してあれば、何でも出力できます。
そのようにして出力したデータは、src/BodyPlugin/ControllerLogItem.hで定義されているControllerLogItemというアイテムとして、コントローラアイテムの子アイテムとして出力されます。これの
std::shared_ptr<ReferencedObjectSeq> log()
という関数で、ログデータを参照できます。ReferencedObjectSeqはReferenced型のオブジェクトが時系列で可能されている配列のクラスです。
シンプルコントローラの場合は常にこのControllerLogItemでの出力となりますが、コントローラをControllerItemを直接継承して実装している場合は、そのvirtual関数である createLogItem() をオーバーライドすることで、ControllerLogItemを継承した任意のアイテム型で出力できます。
そのようにしてどのようなデータでも出力できるのですが、今回の件ですとそれをYAML等のファイルに出力したいかもしれませんね。そのような機能はControllerLogItemには備わっていませんので、どこかに実装する必要があります。Pythonスクリプトで実装すると手軽かもしれません。まあChoreonoid本体でControllerYamlLogItemみたいなのを予め用意して使えるようにしてあればよかったかもしれませんね。
ただしファイルに出力さえできればよいのであれば、シンプルコントローラの実装でファイルを開いてcontrol関数から直接ファイルに出力した方が手っ取り早いかもしれません。それに対して、上記の手法は出力したログデータをChoreonoidの中で追加の機能として活用したい場合に、特に有効かと思います。
この例として、BodyPluginにBodyContactPointLoggerItemというものがあります。シミュレーションにおいてこのアイテムを対象のBodyItemの子アイテムとして配置しておくと、そのBodyの接触点情報をControllerLogItem(を継承したBodyContactPointLogItem)として出力してくれます。さらにそのアイテムのチェックを入れることで、シーンビュー上で接触点情報を確認することも可能です。そのようにChoreonoid上の機能として連携できます。これを用いたサンプルとして、sample/generalにFallingBoxesContactPoints.cnoidというプロジェクトファイルを用意しています。箱を落下させるシミュレーションでの接触点の状況をこのアイテムを使って可視化するものです。
これを見ていただければ、今回紹介した機能の利用方法のイメージをつかんでいただけるかもしれません。
nakaokaさん，
ご教授くださりありがとうございます．
ControllerLogItemというものがあるのですね．
確認してみます．ありがとうございました．