HRP2W用のik_server

[hyaguchijsk]

HRP2W用のik_serverがなかったので，僕の方で追加しておきます．
start-jsk/rtmros_tutorials#87
start-jsk/rtmros_tutorials#88
この２つのPRがマージされていないとHRP2Wのモデルが生成されませんが，
待てないので手元で生成したものでテストします．

[hyaguchijsk]

ちょっと実装が適当すぎました．
以下のエラーで落ちるようです．

/home/leus/ros/hydro/src/jsk-ros-pkg/jsk_roseus/euslisp/jskeus/eus/Linux64/bin/irteusgl roseus-error: cannot find method :joint in (send self :calc-inverse-kinematics-weight-from-link-list leg-ul :union-link-list leg-ul), exitting...

どこのコードかがわかりませんが，
メッセージ的におそらく足がついていないことが根本的な原因ではないかと思われます．

何をどう書き換えたらよいでしょうか？

[s-noda]

何で動作確認をしていますか？
今モデルをつくっているところなので当てずっぽうですが、

可能性１． IKの目標に足のリンクが混ざっている。
可能性２． HRP2W のモデルが足を持っていないのに、 (find-method hrp2w :rleg) できる

pr2 では特に使い方を工夫しないでも動くようにしているはずで、

ik-server.l の :init の中で
もし(find-method hrp2w :rleg)だったら足を支持リンクに使う。
というコードが書いて有ります。
可能性２．ならば、その箇所を (and (find-method hrp2w :rleg) (send hrp2w :rleg)) に変えれば動くかもしれません。

モデルができたらこっちでも試してみます。

[s-noda]

hrp2w.l 44 行目

  (:fullbody-inverse-kinematics
   (target-coords &rest args &key (stop 250) (cog-gain 2.0) (additional-weight-list) &allow-ot\
her-keys)
   (let* ((leg-ul (send self :calc-union-link-list (send self :legs :links))))
     (send self :reset-joint-angle-limit-weight-old leg-ul)
     (send-message* self robot-model :fullbody-inverse-kinematics
                    target-coords
                    :stop stop :cog-gain cog-gain
                    :additional-weight-list
                    (append
                     additional-weight-list
                     (list
                      (list (car (send self :links))
                            #'(lambda ()
                                ;; set root-link 6dof-joint's weight based on legs' joint limi\
t                                                                                              
                                (let* ((min-weight
                                        (reduce #'(lambda (x y) (min x y))
                                                (coerce (send self :calc-inverse-kinematics-we\
ight-from-link-list
                                                              leg-ul :union-link-list leg-ul) \
cons))))
                                  (fill (instantiate float-vector 6) min-weight)))
                            )
                      ))
                    args)
     ))

のなかのエラーのようです。
hrp2w のモデルをつくってIKをとくだけのコードでも同じエラーがでます。

roseus hrp2w.l

(setq a (hrp2w))

(let* ((mt (list (send a :rarm :end-coords)
                 (send a :larm :end-coords)))
       (tc (send-all mt :copy-worldcoords))
       (ll (mapcar
            #'(lambda (mt) (send a :link-list (send mt :parent)))
            mt)))
  (send a :fullbody-inverse-kinematics
        tc
        :move-target mt
        :link-list ll
        :target-centroid-pos nil
        :debug-view :no-message
        ))

euscollada が足のないロボットに対応していない？

[s-noda]

hrp2w.l 44 行目

の

:fullbody-inverse-kinematics をコメントアウトすれば動くことを確認しました。

euscollada のほうは別途 issue を出します。

[hyaguchijsk]

@s-noda どうもありがとうございます．
fullbodyikを使わない方向で対処します．