Poseへの接触点情報の追加

[kindsenior]

Pose::LinkInfoとPoseSeqファイルに接触リンクの接触点に関する情報を追加しました

コミットの概要は以下のようになっています

• LinkInfoのcontacttPoints_メンバ変数に接触点群が保持されます
  • 各接触点は接触リンク固定座標系で表現されています
• PoseSeqファイルにはikLinkInfo内にcontactPointsというキーで点のリストが保存されます
• PoseSeqViewBase::setCurrentLinkStateToIkLink関数でcollidedというbool変数を削除し，代わりにcontactPoints(std::vector)のサイズでリンクが接触しているどうかを判断しています．その際に一つのリンクが複数の物体と干渉する際にbreak文は不都合なので削除しました

[s-nakaoka]

確認が遅くなってしまい申し訳ありません。 変更内容については概ね理解できたと思うのですが、念の為以下について確認させてください。

• 接触点が必要になった理由は何になりますでしょうか？（どのような用途で使用していますか？）
• この改良では接触点情報（std::vector）のコピーを行っていますが、これによってキーポーズからの軌道生成（補間）処理や自動バランス補正の処理が重くなることはないでしょうか？

[kindsenior]

ご確認ありがとうございます

• 接触点が必要になった理由は，多点接触を伴う動作や爪先・踵のエッジで接触する動作の軌道計画を行う際に各リンクの接触面の領域として利用するからです また，今は足先・手先の適当な座標系の原点を中心にキーポーズ間の軌道が補間されると思います．しかし，将来的に足平面接触→爪先エッジ接触のように遷移する際には接触している爪先エッジ周りに足平を回転するといったようにリンク内での接触点の情報を利用した補間を行わなければならなくなると思います．その際にも接触点をキーポーズに保管しておいた方がよいと考えています．

• 「処理が重く」というのがどの程度かによりますが，少なくとも軌道生成補間処理に関しては指摘されるまで気づかない程度の処理時間ではありました(定性的で申し訳ございません) しかし，ご指摘の通りあまり早い処理にはなっていないので， 現在は， bool PoseSeqViewBase::setCurrentLinkStateToIkLink() で，std::vector contactPointsを宣言してそれをlinkInfo->setTouching()で登録しておりますが， contactPoints.push_back()をしているタイミングで随時linkInfoに接触点を追加する べきというのはあるかもしれませんので，ご意見いただけると幸いです

[s-nakaoka]

対応が遅くなってしまい申し訳ありません。ご回答いただきありがとうございます。 マージしておきました。

修正の意図については了解です。

また、処理速度の面で、先の書き込みでは私自身まだ十分把握できていなかったのですが、再度コードを確認したところ、この修正では軌道生成やバランス補正において処理が重くなる要素は無いようですね。LinkInfo作成時に情報を入れているだけで、その後の参照においては特に追加の処理はないようですので。面倒な指摘をしてしまいましてすみません。

[s-nakaoka]

以下参考までにお知らせします。上記のようにコード自体に重くなる要素はほぼないのですが、念の為コード修正による軌道生成やバランス補正の処理時間の差を計測してみました。データとしては某二足歩行人型ロボットのおよそ３分のダンスデータをバランス補正計算回数３回として試してみました。環境はUbuntu 18.04でRyzen 3900Xです。

この条件で、軌道生成におよそ３秒ほどかかります。それで、修正前はだいたい３秒なのですが、プルリク適用後は3.1秒〜3.2秒程度に処理が遅くなりました。割合で言うとざっくり5%くらい遅くなる感じです。

vector<Vector3> contactPoints_ が追加されていることでキャッシュヒットに影響があるのかと思い、そこを除去して適当にビルドできるようにしたコードで試してみたのですが、結果は同じでした。

そこで予測するに、どうもこの修正によるコード追加でコード量が増えた分、プログラムコードのメモリ内での配置が少し変わって、それがプログラムコードのキャッシュのヒットに影響を与えた可能性があるかなと思っています。この判断の材料として、CMakeLists.txt内で.cppの順番を適当に変えて試したところ、処理速度の差が2%程度に縮まったりもしました。これはDLL内での各コードの配置が変わったことによるものだと思います。

というわけで、Kojimaさんのコードが悪いわけでは全く無いのですが、ちょっとしたコード量の変化で処理速度が数パーセント変わることもあるという事実が分かりました。以前も同様のことがあったので、そういうものなのだと思います。コードを修正して遅くなったからと言って、そのコード自体が悪いとは限らないということを、覚えておく必要がありますね。

あとはオブジェクトファイルのリンクの順番でも実行速度が変わるようなので、その部分を最適化してくれるツールがあったりするとよいですよね。実際順序を変えるだけで数パーセントのレベルではけっこう変わるので、どう並べたらよいものか気になるところではあります。

以上本件とは直接関係ありませんが、本件のマージ中に気づいたことなので、参考までにコメントしました。

[s-nakaoka]

あと今回の修正と直接関係する事柄として、以下の点が若干気になりましたので、念の為書いておきます。

• isTouching_ メンバは廃止して、isTouching()関数は !contactPoints.empty() を返すようにしてもよいような気がします
• contactPointsは干渉チェッカが返すものをそのまま入れているようなので確か座標系はグローバル座標かと思うのですが、このようにリンクの情報として付随させる場合、リンクローカル座標にしておいた方が都合がよかったりしないでしょうか

[s-nakaoka]

@kindsenior

このプルリク（マージ済み）のコードに問題があり、修正したのでお知らせします。 1a1e10d1c9afade463dd826e2255201c3a12a2fd

Poseオブジェクトのrestoreにおいて、ikLinkに関してこのプルリクで新たに追加された "contactPoints" が記述されていないと、ikLinkの接触情報が（全て）取り込まれないようになっていました。このため、この改良がなされる以前に作成された動作データ（キーポーズ列ファイル）を読み込む場合に、作成時の動作が正常に再現しないという不具合が発生していました。

具体的には、ZMP軌道の自動生成に影響があり、自動バランス補正が正しく機能しない状態となっていました。例えば、サンプルとしてバンドルされているsample/PoseSeq/GR001.cnoid のプロジェクトでも、サンプルの動作データでZMP軌道が正しく生成されず、生成された動作をシミュレーションすると転倒してしまっていました。

そこでPoseオブジェクトのrestore関数について、contactPointsノードがない場合でも、接触情報（isTouching等の情報）は読み込んで反映されるように修正しました。この場合空のcontactPointsが設定されます。

後は関連する部分のコードを少し整理しておきました。

これでGR001のサンプルも含めて、私の手持ちの既存の動作データも正常に動かせるようになりました。

ただしこの修正でcontactPointsの処理にまずい部分がひょっとすると入っているかもしれませんので、そちらについて問題がないかご確認いただければと思います。

というわけなのですが、修正においてはなるべく既存データとの互換性も考慮していただけるとうれしいです。（と言いつつ私も互換性をなくす修正をちょくちょくしてしまっているとは思いますが・・・。）

以上よろしくお願い致します。