isNoUnknownAccelMode: c1/M11 is inistialized as 0 size

[k-okada]

Thread 1 "hrpsys-simulato" received signal SIGSEGV, Segmentation fault.
0x00007ffff765cab9 in Eigen::ColPivHouseholderQR<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::computeInPlace() ()
   from /home/user/catkin_ws/devel/lib/libhrpModel-3.1.so.0
(gdb) where
#0  0x00007ffff765cab9 in Eigen::ColPivHouseholderQR<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::computeInPlace() ()
   from /home/user/catkin_ws/devel/lib/libhrpModel-3.1.so.0
#1  0x00007ffff765cc4b in Eigen::ColPivHouseholderQR<Eigen::Matrix<double, -1, -1, 0, -1, -1> >& Eigen::ColPivHouseholderQR<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::compute<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::EigenBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&) () from /home/user/catkin_ws/devel/lib/libhrpModel-3.1.so.0
#2  0x00007ffff765fd48 in hrp::ForwardDynamicsMM::solveUnknownAccels(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&) () from /home/user/catkin_ws/devel/lib/libhrpModel-3.1.so.0
#3  0x00007ffff76604f8 in hrp::ForwardDynamicsMM::solveUnknownAccels(hrp::Link*, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&) () from /home/user/catkin_ws/devel/lib/libhrpModel-3.1.so.0
#4  0x00007ffff766fcab in hrp::CFSImpl::setAccelerationMatrix() () from /home/user/catkin_ws/devel/lib/libhrpModel-3.1.so.0
#5  0x00007ffff7670204 in hrp::CFSImpl::solve(OpenHRP::CollisionSequence&) ()
   from /home/user/catkin_ws/devel/lib/libhrpModel-3.1.so.0
#6  0x0000000000419c5f in Simulator::oneStep() ()
#7  0x000000000040bf45 in main ()

[k-okada]

16.04 だけなのですが以下のようになります．ロボットはPA10でdynamics有りのシミュレーションをしています．
よくわかっていませんが、
https://github.com/fkanehiro/openhrp3/blob/master/hrplib/hrpModel/ForwardDynamicsCBM.cpp#L93

	isNoUnknownAccelMode = (n == 0);

	M11.resize(n, n);
	M12.resize(n, given_rootDof+m);
	b1. resize(n, 1);
	c1. resize(n);

みたいにnが０の場合があって、

        dvector a(M11.colPivHouseholderQr().solve(c1));

で落ちているように見えます．
https://github.com/fkanehiro/openhrp3/blob/master/hrplib/hrpModel/ForwardDynamicsCBM.cpp#L710

[pazeshun]

@k-okada Ubuntu 18.04でも同じ問題が発生し、このパッチを当てたら治りました。
しかし、ros-melodic-openhrp3にはこのパッチが含まれていないようなので、入れていただいてもよろしいでしょうか。
https://github.com/tork-a/openhrp3-release/blob/release/melodic/openhrp3/hrplib/hrpModel/ForwardDynamicsCBM.cpp#L696-L701

[k-okada]

入れてリリースしました ros/rosdistro#24466 が，hrpsysユーザからは何のコメントもないので，本当に必要かどうか，，，，

…

-- ◉ Kei Okada 2020年4月10日(金) 23:17 Shun Hasegawa <[email protected]>:

@k-okada <https://github.com/k-okada> Ubuntu 18.04でも同じ問題が発生し、このパッチを当てたら治りました。 しかし、ros-melodic-openhrp3にはこのパッチが含まれていないようなので、入れていただいてもよろしいでしょうか。 https://github.com/tork-a/openhrp3-release/blob/release/melodic/openhrp3/hrplib/hrpModel/ForwardDynamicsCBM.cpp#L696-L701 — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#126 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXEE6VQ43C66JOUBIGTRL4S6RANCNFSM4DV35W4Q> .

[pazeshun]

PA10のシミュレーションを試してる人がほとんどいないからじゃないかと思っています。SampleRobotとかでは起きないので。
僕も、恥ずかしながらtravisで落ちるまで気づきませんでした。

リリースありがとうございます。

[k-okada]

SampleRobot以外にもローカルなモデルはいっぱいあるのになんでPA10だけなんだろう？ どこかで，同じ問題が起こっています．で，手元直しています．とかルートのモデルがなんちゃらの場合はこれが必要です，なんて言ってくれたら自信を持ってマージできるのに．．． -- ◉ Kei Okada 2020年4月14日(火) 16:51 Shun Hasegawa <[email protected]>:

…

PA10のシミュレーションを試してる人がほとんどいないからじゃないかと思っています。SampleRobotとかでは起きないので。 僕も、恥ずかしながらtravisで落ちるまで気づきませんでした。 — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#126 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXBRBDHQJY5WI33UFLLRMQIWVANCNFSM4DV35W4Q> .

[YoheiKakiuchi]

識者の方に教えてもらいたいところですが、

choreonoidで同名のファイルがあって、パッチに相当するものはなさそうですね。
パット見で違うところは、以下の２つくらいに見えます。

c1の定義が違う(choreonoidはvector, hrpsysはmatrix)
https://github.com/s-nakaoka/choreonoid/blob/master/src/Body/ForwardDynamicsCBM.cpp#L105

preserveHighGainModeJointState()の場所が違う
https://github.com/s-nakaoka/choreonoid/blob/master/src/Body/ForwardDynamicsCBM.cpp#L135
https://github.com/s-nakaoka/choreonoid/blob/master/src/Body/ForwardDynamicsCBM.cpp#L195

choreonoidでは、PA10のhigh_gainシミュレーションはできていそうです。

あと、他のロボットと比べて気になったところは、methodがEULERになっている.
(PA10はルンゲクッタ)
https://github.com/tork-a/rtmros_nextage/blob/indigo-devel/nextage_ros_bridge/conf/nextage.xml.in#L7

[fkanehiro]

@s-nakaoka @hattorishizuko
いかがでしょうか。

[s-nakaoka]

@s-nakaoka @hattorishizuko
いかがでしょうか。

議論のポイントがよく分かっていないのですが、PA10とSampleRobotの違いはルートが固定かフリーかになるかと思います。ハイゲインモードは（関節角度等の）位置指令を与えた部分は必ずその位置になるという前提で加速度が未知の部分を計算するという手法であり、ルートが固定の場合は（全関節に位置指令を与えるのであれば）加速度が未知の変数はなくなるので、そもそもこの計算方法を適用するような状況ではないと言えるかもしれません。

ハイゲインモードは元々産総研のヒューマノイドグループで２足歩行ロボットの歩行パターンの研究で必要にかられて構築された経緯があります。二足歩行ロボットの歩行において全関節の軌道を与えてそれが完璧にトラッキングされたという前提において、フリーであるルートリンクの加速度（＝軌道）を計算し、その歩行パターンでロボット転倒するか否かを検証するために開発されたものです。もちろん実際のロボットでは与えた目標関節角軌道が完璧に再現されることなど有り得ませんが、歩行パターン生成アルゴリズム（とその実装）の検証には有効です。

ただしこの計算方法は与えた位置指令（と環境とのインタラクションも含む部分）に矛盾があると破綻します。

なのでルートが固定されたPA10のようなロボットに適用するのはあまり筋がよくありません。少し特殊な計算方法ですので、それ以外の状況でも使用には注意を要します。

OpenHRP3ではPA10をハイゲンモードで動かすようなサンプルも入っていたのかもしれませんが、明確な必要性が無くとりあえずという感じで入った可能性もあります。ChoreonoidではPA10でもハイゲインモードで動作するとのことですが（サンプルではそのようなものは用意していないのでこちらでは検証できていませんが）、そのあたりはChoreonoidを改良する中で計算が破綻しないような工夫をしたことはあるかもしれません。（そうだとしてもあまりよく覚えていはいないのですが…。）

いずれにしてもOpenHRP3は10年ほど前に開発が終了したソフトウェアであり、その後継で現在も開発が継続しているChoreonoidではこの件問題なく動いているとのことで、Choreonoidへの移行もご検討いただければ幸いです。

[Naoki-Hiraoka]

ハイゲインモードでルートが固定されたロボットをシミュレーションする場合に、ロボットと環境が接触しているときに行われる処理でエラーが発生します。PA10はBASE_LINKが床と接触しています.

SampleRobot以外にもローカルなモデルはいっぱいあるのになんでPA10だけなんだろう？

hrpsys-simulatorを使う多くのモデルはルートが固定されていないためです。
hironxはルートが固定されているロボットですが、https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/launch/hironx_startup.launch は「ルートが固定されていないモデルを使い」かつ「シミュレーションをしていない」ため、たまたまエラーが発生していませんでした。openhrp3をソースから入れてhironx_ros_bridgeをNaoki-Hiraoka/rtmros_hironx@e523834 のように変更してrtmlaunch hironx_ros_bridge hironx_startup.launchすると同様のエラーが発生します。

今回のエラー自体は、ハイゲインモードでルートが固定されたロボットは加速度が未知の要素の数が0になるので、
dvector a(M11.colPivHouseholderQr().solve(c1));
で線形方程式の要素数が0になってEigenが落ちるというものです。ハイゲインモードの問題というよりも、Eigenの使い方の問題だと思います。このPull Requestの直し方でも問題はないですが、関数全部を無効化するのではなくEigenの使い方が良くない箇所だけを直す方が良いのではないかと考え、代わりのPull Requestを作成しました。#155

この問題が修正されないと、rtmros_commonのテストでfkanehiro/openhrp3をソースから入れるケースが通りません。現在はやむをえず代わりにtork-a/openhrp3-releaseを使用しています。
https://github.com/start-jsk/rtmros_common/blob/fe1c671db7639fd2e990adf281a563dafec3d817/.travis.rosinstall.melodic#L2