send *robot* :head :look-at (float-vector) すると、首が目標位置を向く

[kochigami]

send *robot* :head :look-at (float-vector) で、
ロボットがどのリンクから目標物を見ようとしているか　はどのようにしたら確認できますか。
曖昧な質問ですみません。

現象：いつも目標より顔が上に上がってしまう。
予想：多分首を目標に向けている

関連しそうなpull-request: #408
jskeusのバージョン: ros-indigo-jskeus:amd64/trusty 1.1.0-0trusty-20170313-071400-0700 uptodate
(上の変更も取り入れられている)

ロボット：pepper

やってみたこと：

• 目標物の base_footprint （ペッパーの足元）相対の位置の確認（間違ってはなさそう）
• head-end-coords の位置を目のところにずらした
  https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_naoqi_robot/peppereus/pepper.yaml#L33
  translate : [0, 0, -0.05]
  
• 何も変わらなかった

2.irteusgl$ send *pepper* :links
(#<bodyset-link #X75ffef0 base_link  0.0 0.0 0.0 / 0.0 0.0 0.0> #<bodyset-link #X78f9f60 Tibia  29.968 -4.238 -484.295 / 0.001 -0.098 -0.012> #<bodyset-link #X7e21d50 Hip  0.02 0.0 -139.0 / 0.0 0.0 -0.012> #<bodyset-link #X7dc4918 Pelvis  0.02 -0.969 -217.994 / 0.001 -0.112 -0.012> #<bodyset-link #X6d67a70 LShoulder  -57.0 149.74 86.82 / 3.142 1.532 3.142> #<bodyset-link #X6d47880 LBicep  -57.0 149.74 86.82 / 1.959 1.47 1.96> #<bodyset-link #X6d3f218 LElbow  -63.733 181.605 -92.059 / 0.661 1.42 -0.73> #<bodyset-link #X6d300e8 LForeArm  -63.733 181.605 -92.059 / 0.203 1.273 -1.179> #<bodyset-link #X6d302e0 l_wrist  -20.69 190.458 -235.478 / 0.203 1.273 -1.467> #<bodyset-link #X70c8b20 RShoulder  -57.0 -149.74 86.82 / 3.142 1.545 3.142> #<bodyset-link #X70ccab0 RBicep  -57.0 -149.74 86.82 / -1.881 1.485 -1.882> #<bodyset-link #X70b6488 RElbow  -61.547 -179.406 -92.506 / -0.55 1.417 0.919> #<bodyset-link #X6f7cf00 RForeArm  -61.547 -179.406 -92.506 / -0.176 1.236 1.284> #<bodyset-link #X6f6dc98 r_wrist  -13.004 -188.056 -234.17 / -0.176 1.236 1.609> #<bodyset-link #X6cb85e0 Neck  -38.0 0.0 169.9 / -0.218 0.0 0.0> #<bodyset-link #X6c26aa8 Head  -38.0 0.0 169.9 / -0.218 -0.707 0.0>)
3.irteusgl$ send *pepper* :head :links
(#<bodyset-link #X6cb85e0 Neck  -38.0 0.0 169.9 / -0.218 0.0 0.0> #<bodyset-link #X6c26aa8 Head  -38.0 0.0 169.9 / -0.218 -0.707 0.0>)

[k-okada]

$ (progn (send *pepper* :reset-pose)(objects (list *pepper* (setq *box* (make-cube 100 100 100 :pos #f(500 300 00))))))
(send *pepper* :head :look-at  (send *box* :worldpos) :draw-on :debug-view t)

で添付のような画像が見えるはずです．

判定は難しいけど、

(do-until-key (send (send *pepper* :head :end-coords) :draw-on :flush t :size 1000)(x::window-main-one))

として、対象にｚ軸が突き刺さっていれば問題ないはず．

[k-okada]

これみると、頭のend-coordsはおでこカメラだね．
両眼の中心がよかったらpepper.yamlを直してみましょう．
https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_naoqi_robot/peppereus/pepper.yaml#L32-L34

[snozawa]

関連情報ですが、デフォルトのhead-end-coordsを直したあとに、
おでこも使いたくなった場合は、以下のようにしてlook-atの引数にmove-targetを与えるとできます。

(load "irteus/demo/demo.l")
(unless (boundp '*robot*)
  (setq *robot* (instance sample-robot :init)))
(send *robot* :reset-pose)
(send *robot* :fix-leg-to-coords (make-coords))
(objects (list *robot*))
(let ((mt (make-cascoords :coords (send (send *robot* :head :end-coords :copy-worldcoords) :translate (float-vector 70 0 0)) :parent (send *robot* :head :end-coords :parent))))
  (send *robot* :head :look-at (float-vector 800 0 900) :debug-view :no-message :move-target mt)
  (send (send mt :parent) :dissoc mt))

[kochigami]

@k-okada 先生　@snozawa 先生

ご教示ありがとうございました。
認識結果（か計算結果）が間違っていて、正しい挙動であることが分かりました・・・

おでこカメラは直しておきました。　jsk-ros-pkg/jsk_robot#810

同じ目線で見ているはずの人の顔で、
このようになっていることは分かりました。

[kochigami]

追記：
首が目標を向く理由：
base_link (お腹)が座標系の原点なので、

• base_footprint相対で座標を出すなら base_link に対してその分座標変換する
• 初めから　base_link相対で座標を出す

をしないといけなかった