[SoftErrorLimiter] fix priority among vel, err, pos

[Naoki-Hiraoka]

#785 以降、SoftErrorLimiterは、

vel=>pos=>errorの順で書き換えられたときに、前のリミットをみたさないことがある

ことを防ぐために、

速度、角度、エラーの３点につきそれぞれ角度形式のリミットを計算して、
最後にm_qRef=min(速度リミット、角度リミット、エラリミット)のようにして３点のなかの
一番厳しいリミット値で制限するようにしました。

という仕様になっています。この仕様の問題を直すPull Requestです。

vel=>pos=>errorの順で書き換えられたときに前のリミットをみたさないのは、速度、角度、エラーの3つのリミットを同時に満たす関節角度が存在しない場合なので、3つのリミットを調停する必要があります。しかし、#785 の「一番厳しいリミット値で制限する」というのは定義が曖昧なので、現在の実装ではどのような挙動になるのか整理されていない状態になっています。その結果、複数リミットの境界付近の動作を行わせることができませんでした。

このPull Requestでは、

3.　vel (highest priority)
2.　error
1.　pos (lowest priority)

の順の優先順位で調停するようにしました。すなわち、速度リミットを満たす範囲内でエラーリミットを満たし、それらのリミットを満たす範囲内で位置リミットを満たすように指令関節角度を修正します。
posが低優先度である理由は、velとerrorは一瞬でもリミットを侵すとRobotHardwareの機能によって全身が脱力してしまい、動作続行困難になるためです。

hrpsys-base/rtc/RobotHardware/robot.cpp

Line 622 in e98cf26

bool robot::checkJointCommands(const double *i_commands)

hrpsys-base/rtc/RobotHardware/robot.cpp

Line 657 in e98cf26

bool robot::checkEmergency(emg_reason &o_reason, int &o_id)

velがerrよりも高優先度である理由は、SoftErrorLimiterのsetServoErrorLimitサービスによってerrorのリミットが不連続に変化した場合に、velのリミットを侵しRobotHardwareの機能によって全身が脱力してしまい動作続行困難になることを防ぐためです。

hrpsys-base/rtc/RobotHardware/robot.cpp

Line 622 in e98cf26

bool robot::checkJointCommands(const double *i_commands)

実装上は、
速度、角度、エラーの３点につきそれぞれ角度形式のリミットを計算したあと、単純にpos=>error=>velの順でチェックして指令関節角度を修正することで、上記優先度に基づく調停と等価な処理が行われます。

また、これによって

hrpsys-base/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp

Line 342 in e98cf26

if ( llimit > m_qRef.data[i] && prev_angle[i] > m_qRef.data[i] ) { // ref < llimit and prev < ref -> OK

の、指令関節角度が位置リミットを満たす方向に動いている場合は位置リミットを考慮しないでOK、というif文が不要になったため、削除しました。

[Naoki-Hiraoka]

• サーボオン直後に急激に動くことを防ぐために、サーボオンしてから一度も位置リミットを満たしていない関節は位置リミットチェックをしないようにする必要がある
• 基板との通信エラーのときにservo_stateがfalseになる仕様のiobのロボットの場合、一瞬の通信エラーが発生してすぐに復帰した場合に、servo_stateがon->off->onに一瞬で切り替わることによってsoftErrorLimiterのenable->disable->enableが一瞬で切り替わるので危険.