関節角リミットがtableで定義されている関節ではvelocity limit overになることがある

[mmurooka]

SoftErrorLimitterの
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp#L271-L332
で
・ 関節速度がリミットを超えないように修正 (271行目から)
・ 関節角度がリミットを超えないように修正 (302行目から)
がされていますが，

関節可動域が，最小・最大角度のスカラ値ではなく，
隣接二関節が依存するテーブルとして与えられている場合（ https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp#L306 のif文に該当する場合 ），関節速度の修正が正しく行われない場合があります．

その結果，実機実験中に
robot::checkJointCommands関数
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/RobotHardware/robot.cpp#L546-L556
の速度チェックでリミットを超えていると判定され，
RobotHardwareプラグインの中の
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/RobotHardware/RobotHardware.cpp#L251
で，サーボOFFされてしまう問題に遭遇しています．

リミットがテーブルで速度が正しく修正されない理由は，
JSK内部Issueですが以下で https://github.com/start-jsk/rtmros_hrp2/issues/317 で議論されていて，
例としては以下の青い矢印の遷移の場合に
凹の形の可動域に収まるように関節角度指令値を上書くときに速度が早くなってしまうのだと思います．

現状のSoftErrorLimitterでは，
速度リミット -> 角度リミット
の順に確認していますが，逆にして
角度リミット -> 速度リミット
にすれば解決するかもしれないと思っておりますが，
どうすれば良さそうかアイディアありましたら教えて頂けますと幸いです．

[ban-masa]

HRP2の手首のmin-max-tableは以下になります。

これによると、(wrist-p, wrist-r)が(40, 75)の状態から(60, 55)の状態へ直線的に移動すれば、問題の症状が発生すると考えられます。

そこで、choreonoidでのシミュレーションにEuslispからこの指令を送ってみたのですが、問題は再現しませんでした。（目標角度列を実現しようとする途中でcollisionして引っかかったまま止まるがサーボは落ちない）
調べたところ、choreonoidはchoreonoid用のRobotHardwareRTCを使っているので、onExecute関数の処理が全く別でした。（hrpsysのRobotHardwareを継承してはいる）
結果として、現段階ではこの問題をシミュレーション上で検証することができない状態です。

解決策として、choreonoidのRobotHadwareにcheckJointCommands関数を実装し、リミットを超えているか判断するということを考えています。
ちなみに、hrpsys_choreonoid_tutorialsのmasterにはHRP2のシミュレーションは含まれていないため、以下のbranchを使用して行いました。
https://github.com/ishiguroJSK/rtmros_choreonoid/tree/koyama-hrp2-choreonoid

[mmurooka]

ありがとう．シミュレーション上でデバックできたらと思いましたが，RobotHardwareが違うんですね．

（目標角度列を実現しようとする途中でcollisionして引っかかったまま止まるがサーボは落ちない）

これは，CollisionDetectorで干渉判定されて止まるということかな．実機ではRARM_JOINT5,RARM_JOINT6のリンクは干渉チェックしていないですね．
https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/CMakeLists.txt#L190

[mmurooka]

調べたところ、choreonoidはchoreonoid用のRobotHardwareRTCを使っているので、onExecute関数の処理が全く別でした。（hrpsysのRobotHardwareを継承してはいる）

choreonoidのRobotHardwareRTCは
https://github.com/ishiguroJSK/rtmros_choreonoid/blob/koyama-hrp2-choreonoid/hrpsys_choreonoid/iob/RobotHardware_choreonoid.cpp#L66-L74
かな．onExecuteの中で，親クラスのonExecuteが呼ばれているので，

解決策として、choreonoidのRobotHadwareにcheckJointCommands関数を実装し、リミットを超えているか判断するということを考えています。

は必要なくて，既にcheckJointCommandsも呼ばれている気がするけれど，どうだろう．

[snozawa]

遅くなりました。
中身確認中ですが、非凸なlimitテーブルだと不連続にlimitの値がとんでしまう問題だったと思います。
確認、解決、テストはlimitテーブルの部分だけで閉じて行うとよさそうですね。

[ban-masa]

実際にはchoreonoidのRobotHardwareはhrpsysのRobotHardwareのonExecuteを呼んでいたので、checkJointCommands等も呼ばれており、hcfからサーボをオフすることも可能でした。

そこでst, abc, icの有無を切り替えながら、min-maxテーブルの縁から縁へ移動するような関節角度指令を送ってみて問題が再現しないか試しましたが、再現しませんでした。（checkEmergencyによってサーボが落ちることはあった）