#42 ikのエラー情報を返す

[Kei Okada]

fullbody-ik-with-error
はIKがなぜとかなかったか（重心が thre を満たさなかった等）の情報をリストで返すものでもっと賢くかけるきがするのでマージ予定はないです。

(defmethod euscollada-robot
  (:fullbody-inverse-kinematics-with-error
   (target-coords
    &rest
    args
    &key
    (revert-if-fail t)
    link-list move-target
    rotation-axis translation-axis thre rthre
    target-centroid-pos centroid-thre centroid-offset-func
    &allow-other-keys)
   (let* ((initial-av (copy-object (send self :angle-vector)))
      (inital-coords (copy-object (send self :worldcoords)))
      (target-coords
       (mapcar #'(lambda (x)
               (if (functionp x) (funcall x) x))
           target-coords))
      dif-pos dif-rot success cdiff
      )
     (if (atom (car link-list)) (setq link-list (list link-list)))
     (if (atom move-target) (setq move-target (list move-target)))
     (if (atom target-coords) (setq target-coords (list target-coords)))
     (if (atom rotation-axis) (setq rotation-axis (list rotation-axis)))
     (if (atom translation-axis) (setq translation-axis (list translation-axis)))
     (if (atom thre) (setq thre (list thre)))
     (if (atom rthre) (setq rthre (list rthre)))
     (send* self :fullbody-inverse-kinematics target-coords
        :revert-if-fail nil args)
     (setq dif-pos
       (mapcar #'(lambda (mv tc trans-axis)
               (send mv :difference-position tc
                 :translation-axis trans-axis))
           move-target target-coords translation-axis))
     (setq dif-rot
       (mapcar #'(lambda (mv tc rot-axis)
               (send mv :difference-rotation tc
                 :rotation-axis rot-axis))
           move-target target-coords rotation-axis))
     (setq cdiff
       (send self :difference-cog-position
         target-centroid-pos centroid-offset-func))
     (setq success
       (send self :ik-convergence-check
         t dif-pos dif-rot
         rotation-axis translation-axis thre rthre
         centroid-thre target-centroid-pos centroid-offset-func))
     (cond
      ((and (not success) revert-if-fail)
       (send self :angle-vector initial-av)
       (send self :newcoords inital-coords)
       (list
    (cons :dif-pos dif-pos)
    (cons :dif-rot dif-rot)
    (cons :thre thre)
    (cons :rthre rthre)
    (cons :cog-dif cdiff)
    (cons :cthre centroid-thre))
       )
      (t (send self :angle-vector))))))

[Kei Okada]

これ，重要ですね．
irtmode.lの1819行ぐらいからIKがとけなかったエラーがあった時にその内容をファイルに書き出していますが，にたような目的でしょうか？full-bodyなどikを呼び出しているプログラムおこれが使えるようにするのがいいでしょうか？

例えばこの部分をメソッド化してfullbodyはそれをoverrideするとかでしょうか．

inverse-kinematicsメソッド内ではファイルに書き出していますが，帰り値で返すほうがいいでしょうか？あるいは，ik-error という大域変数をつくって入れるとか，:ik-fail-info というメンバ変数をつくって格納していく，などはありますでしょうか？

[s-noda]

僕もどうするのが正解か決めかねていますが

ファイル書き出しはすでに動いている気がします。
必要最低限の情報は書いてあるので（関節角度列とベースの位置姿勢）、書き出されたファイル名が分かるようにすれば目的は達成できそうです。

ただ、いちいちファイルの読み書きをするのは面倒なので、僕の希望では、ik-fial-callback のようなコールバック関数をセットできるようにして出力をいじれるようにしたいです。もちろんデフォルトは nil にするので古いプログラムも動くはず。

[Shunichi Nozawa]

今までのIKのコードが概ね
解けたらnil以外、解けてないとnil
というルールできていたので、

ただ、いちいちファイルの読み書きをするのは面倒なので、僕の希望では、ik-fial-callback のようなコールバック関数をセットできるようにして出力をいじれるようにしたいです。もちろ\
んデフォルトは nil にするので古いプログラムも動くはず。

のようにikの返り値を変えるよりは

inverse-kinematicsメソッド内ではファイルに書き出していますが，帰り値で返すほうがいいでしょうか？
あるいは，ik-error という大域変数をつくって入れるとか，:ik-fail-info というメンバ変数をつくって格納していく，などはありますでしょうか？

のように:ik-fail-infoといったslotsを作るのに賛成です。

s-nodaさん

ちなみに、

例えばこの部分をメソッド化してfullbodyはそれをoverrideするとかでしょうか．

cascaded-linkの:inverse-kinematics (irtmodel.l)に
この機能を追加したとして、:fullbody-inverse-kinematicsは
何かうわがく必要があるでしょうか。

[s-noda]

ik-fail-info の値としては、IKのパラメタにくわえて、dif-pos, dif-rot, cog-dif あたりが欲しいと思っています。

:inverse-kinematics
を見た感じ全部取得できるように思うので :fullbody... に別途変更を加える必要はないのではないかと考えています。（僕の気づいていない問題がなければですが）

[Kei Okada]

以下でどうでしょうか？他に必要なものを教えて下さい．

~~~~~
Index: irtmodel.l
===================================================================
--- irtmodel.l (リビジョン 969)
+++ irtmodel.l (作業コピー)
@@ -1968,6 +1968,10 @@
(format f "(defun ~A-check () (let ((r robot)) (send* r :inverse-kinematics ~A)))~%" command-id command-args)
(format f "(defun ik-setup () (~A-setup))~%" command-id)
(format f "(defun ik-check () (~A-check))~%" command-id)

• (format f "(setq ik-failed '(")
• (format f "(:dif-pos . ~A) (:dif-rot . ~A~%)" dif-pos dif-rot)
• (if target-centroid-pos (format f "(:dif-cog . ~A)" (send self :difference-cog-position target-centroid-pos centroid-offset-func)))
• (format f "))~%")
  )
  ) ;;let
  ) ;; dump
  ~~~~~~

[Kei Okada]

上のコードをr987でコミットしました．他にも必要なものがあったら気軽にreopenしてください．

[s-noda]

ありがとうございます。