Fix SoftErrorLimiter limitation

[snozawa]

elのリミットの制限方法を修正しました。
もともといくつか挙動がおかしいところがあり、例えば

• prev_angle[i] = m_qRef.data[i];で１周期出力をセットしているのが最終出力になってなかった（このあとerrorのリミットで値が書き換えられる）
• vel=>pos=>errorの順で書き換えられたときに、前のリミットをみたさないことがある

などがあったので、修正しました。
具体的には、

• velのリミットは(m_qRef - prev_angle)/dt < uvlimit<=>m_qRef < uvlimit *dt + prev_angleとして角度のリミット形式にかきかえられる
• errorのリミットは(m_qRef - m_qCurrent) < limit<=>m_qRef < limit + m_qCurrentとして角度リミット形式にかきかえられる

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

また、速度・エラーなどもsamplerobot_softerror_imitter.pyでチェックして、多分travisのテストにもされると思います。

よろしくお願いいたします。

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/2313/

Build Log
last 10 lines

[...truncated 3217 lines...]
    at org.jenkinsci.plugins.ghprb.GhprbRepository.createCommitStatus(GhprbRepository.java:122)
    at org.jenkinsci.plugins.ghprb.GhprbBuilds.onCompleted(GhprbBuilds.java:118)
    at org.jenkinsci.plugins.ghprb.GhprbBuildListener.onCompleted(GhprbBuildListener.java:27)
    at org.jenkinsci.plugins.ghprb.GhprbBuildListener.onCompleted(GhprbBuildListener.java:12)
    at hudson.model.listeners.RunListener.fireCompleted(RunListener.java:201)
    at hudson.model.Run.execute(Run.java:1786)
    at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:43)
    at hudson.model.ResourceController.execute(ResourceController.java:98)
    at hudson.model.Executor.run(Executor.java:408)

Test FAILed.