fix bug of extended-preview-controller

[Naoki-Hiraoka]

extended-preview-cotrollerクラスの計算過程に誤りがありました。
以下の例では、
Q,Rの値がデフォルトの1.0と1e-6の場合は問題が生じませんが、
両方を1e6倍してQ,Rの値を1e6と1.0とすると、理論上はデフォルトの値の場合と同じ結果になるはずにもかかわらず、過大な制御入力が生じてIKエラーになります。

(load "irteus/demo/sample-robot-model.l")

(defun test-ok ()
  (setq *robot* (instance sample-robot :init))
  (send *robot* :reset-pose)
  (send *robot* :fix-leg-to-coords (make-coords))
  (objects (list *robot*))
  (send *robot* :calc-walk-pattern-from-footstep-list
        (send *robot* :go-pos-params->footstep-list
              500 150 45) ;; x[mm] y[mm] th[deg]
        :debug-view t
        :Q 1.0
        :R 1e-6
        )
  )

(defun test-bag ()
  (setq *robot* (instance sample-robot :init))
  (send *robot* :reset-pose)
  (send *robot* :fix-leg-to-coords (make-coords))
  (objects (list *robot*))
  (send *robot* :calc-walk-pattern-from-footstep-list
        (send *robot* :go-pos-params->footstep-list
              500 150 45) ;; x[mm] y[mm] th[deg]
        :debug-view t
        :Q 1e6
        :R 1.0
        )
  )

(test-ok) ;;OK
(test-bag) ;;Error inverse-kinematics failed.

これを修正するPRです。
計算過程についての詳細は、
#550
で生成されたjmanual.pdfの「18.7.1歩行動作生成」の項をご覧ください(特に式(60)のあたり)。

恐らく、
https://github.com/fkanehiro/hrpsys-base/blob/5cbb5490f55c7017f7ff789c3d9de531ba92e1b6/rtc/AutoBalancer/PreviewController.cpp#L67
も同じ問題を抱えていると思います。

[Naoki-Hiraoka]

demo/walk-motion.lのtest-preview-control-0の出力を添付いたします。

現状
(test-preview-control-0 :preview-controller-class extended-preview-controller :q 1 :r 1e-6)
デフォルトの値。うまく機能する。

(test-preview-control-0 :preview-controller-class extended-preview-controller :q 1e6 :r 1)
Q,Rを1e6倍した値。機能しない。

本pull request適用後
(test-preview-control-0 :preview-controller-class extended-preview-controller :q 1 :r 1e-6)
デフォルトの値。うまく機能する。

(test-preview-control-0 :preview-controller-class extended-preview-controller :q 1e6 :r 1)
Q,Rを1e6倍した値。うまく機能する。

[YutaKojio]

これで合っていると思います．
hrpsysの方にも同様の修正お願いします．

[Naoki-Hiraoka]

fkanehiro/hrpsys-base#1280
でhrpsys-baseにもPull Requestを出しました。

[YutaKojio]

すみません，もう一つ念の為確認で，予見制御ゲインのプロットも載せてもらえるでしょうか？
PR前はpreview windowの最後で飛んだ値になっていて，PR後は0に収束するような値になるはず？

[Naoki-Hiraoka]

test-preview-control-0におけるゲインF(preview-controllerの変数f1-n)をプロットしました。0番目の要素が0になっているのは、preview-controllerの実装上の都合ですので無視してください。

PR前だと、最後のゲインが著しく大きな値になっていました。
PR後は、Q=1.0の時と変わりませんでした。最後のゲインが少し大きくなっているのは、最後の時刻の目標ZMPが無限時間先まで続くと仮定しているためだと考えられます。

Q=1.0 R=1e-6の場合

Q=1e6 R=1.0の場合(PR前)

Q=1e6 R=1.0の場合(PR後)

[YutaKojio]

ありがとう．
プロット的にも正しそうですね．

[k-okada]

@Naoki-Hiraoka テストコードも追加しましょう．

[Naoki-Hiraoka]

Q,Rをともに10倍にしても結果が変わらないことを確認するテストコードを追加しました。

[k-okada]

ありがとう．面倒だけど，一回コミットを整理して testのコミットをして，travisはしらせて，落ちるのを確認した後，次にfix bugのコミットをしてみてくれると助かります．