jsk-ros-pkg / jsk_control

jsk control ros packages
http://github.com/jsk-ros-pkg/jsk_control
14 stars 51 forks source link

solve-qpoases-qpのint-status #684

Closed mmurooka closed 7 years ago

mmurooka commented 7 years ago

@snozawaさん qpoasesがときどきinternal errorになって解けないときに, 変数を変換する(適当な正則行列Aによってy=Axでxからyに変換する)と解けることがあるというのをオフラインでお話させていただきましたが,

これを効率よくやるために,qpが解けなかった時にそれがinternal errorなのかinfeasibleなのかを区別したくて, https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qpoases/euslisp/eus-qpoases.l#L191int-statussolve-qpoases-qp関数の外から見れるようにしたいです. 返り値を変えるとこれまでのプログラムが動かなくなってしまうのが問題ですが,何か良い案はありますでしょうか. あまり綺麗ではないですがグローバル変数にセットする,くらいしかないものでしょうか.

snozawa commented 7 years ago

これはirteusのIKでも同様のことを思ったことがあり、決め手がなかったです。 (ikのloop回数などを外から知りたい、など)

上記のときにでてた同様の解決法で、思い出すと以下があった気がします。

  1. (クラスにして)クラスのslotsかなにかにdebug関係のものを追加し、そこに値を保存しておく
  2. グローバル変数にする
  3. 引数でdebug関係のものを与えるようにし、関数のなかでそれを破壊的にかきかえる。ログをとりたいユーザは明示的にそれを引数で与えて、中身をみる
(defun test (&key (debug-log (list :log1 nil :log2 nil)))
  (setf (cadr (memq :log1 debug-log)) 1.0)
  (setf (cadr (memq :log2 debug-log)) 5.0)
  t)

(let ((debug-log (list :log1 nil :log2 nil)))
  (test :debug-log debug-log)
  (print debug-log)
  t)

おそらく、1が罪悪感が少ない素直な感じになると思います。 3も当時の誰かの案で、実装は簡単で、かつおもしろいなと思った気がします。

3の亜種で、実際にこの関数限定で適用できる方法は、実はやってる例はあります。 https://github.com/jsk-ros-pkg/euslib/blob/master/demo/nozawa/multicontact_motion_planner/qp-inverse-kinematics2.l#L106-L114 のように、@mmurookaさんのいってる関数にはqp-solverをlamdaで与えられますが、 それでなんとか引数をもらってきてると思います。

mmurooka commented 7 years ago

見逃していて上のコメントを見て気づいたのですが, そもそも引数の:statusでqpがどのように成功/失敗したかは取ってくることができたんですね(=3の方法がすでにやられていたんですね). なのでやりたかったことはできそうでした.