動かす joint を指定して IK を解きたい

[hiroya1224]

見えているARマーカーの位置・姿勢からロボット自身がどれだけ傾いているかを推定するために，
画像の青色の板(実際に観測したARマーカー)を赤色の板(実際に距離を測って与えたARマーカー)に合うようにIKを解こうとしています．

初期状態での位置関係は下の画像のようになっていて，

これを，head と青色マーカーを assoc して赤色マーカーへ合わせようとすると，

のように，首を動かしてしまって「同じ姿勢のまま前に傾く」ことが表現できません．

動かす joint を指定してIKを解くような方法はありますか？

[hiroya1224]

:additional-weight-list を使って，動かしたくない joint がある link には小さな値を，動かしたいものには大きな値を設定するとうまく行きました．

長いですが，サンプルコードを貼っておきます．
*robot* は jsk-ros-pkg/kxr-ros で定義されているkxrl2makaberaspyを使っています．

;; define ar markers
(defun init-markers ()
    (setq *ar-obsv* (make-cube 50 50 5)
          *ar-targ* (make-cube 50 50 5))
    (send *ar-obsv* :newcoords 
        (make-coords :pos #f(283.385 50.0 239.164)
                     :rot #2f((4.891898e-09 -0.104528 -0.994522) 
                              (-1.0 7.915258e-09 -5.750772e-09) 
                              (8.473017e-09 0.994522 -0.104528))))
    (send *ar-obsv* :set-color :blue)
    (send *ar-targ* :newcoords 
        (make-coords :pos #f(325.0 60.0 76.1474)
                     :rot #2f((2.000000e-09 0.5 -0.866025)
                              (-1.0 2.000000e-09 -1.154700e-09)
                              (1.154700e-09 0.866025 0.5))))
    (send *ar-targ* :set-color :red)
    ;; show the markers and a robot
    (objects (list *ar-obsv* *ar-targ* *robot*)))

;; (trace-to-root end-coords root-name) |--> (list of all links from end-coords to root)
(defun trace-to-root (end-coords root-name)
    (let ((el (list (send end-coords :parent))))
    (loop
        (push (send (car el) :parent) el)
        (when (equal (send (car el) :name) root-name) (return nil)))
    (cdr el)))

;; e.g., (range 10 :init-elem 1 :step 2) |--> (1 3 5 7 9 11 13 15 17 19)
(defun range (N &key (init-elem 0) (step 1))
    (let (res)
    (dotimes (k N)
      (push (+ init-elem (* k step)) res))
      (reverse res)))

;; e.g., ((a b c) (0 1 2)) |--> ((a 0) (b 1) (c 2))
(defun list-transpose (l)
    (let (res)
    (dotimes (i (length (car l)))
        (push (mapcar #'(lambda (x) (elt x i)) l) res))
    (reverse res)))

(defun get-weighted-ll (rb limb &key (init-elem 100) (step 100))
    (list-transpose (list (trace-to-root (send rb limb :end-coords) "torso") 
                    (range (length (trace-to-root (send rb limb :end-coords) "torso"))
                           :init-elem init-elem :step step))))

;; main IK
(defun ar-ik-common (obsv-coords targ-coords head-and-limbs &optional (rb (copy-object *robot*))
                                                            &key (debug-view :no-message) (revert-if-fail nil) (thre 1) (rthre (deg2rad 1)))
  ;; (car head-and-limbs) must be :head
  (send (send rb :head :end-coords :parent) :assoc obsv-coords)
  (send rb :fullbody-inverse-kinematics
      (append (list targ-coords) (mapcar #'(lambda (x) (send rb x :end-coords :copy-worldcoords)) (cdr head-and-limbs)))
      :move-target (append (list obsv-coords) (mapcar #'(lambda (x) (send rb  x :end-coords)) (cdr head-and-limbs)))
      :link-list (mapcar #'(lambda (x) (send rb :link-list (send rb x :end-coords :parent))) head-and-limbs)
      ;; :rotation-axis (make-sequence 'list (length head-and-limbs) :initial-element t)
      ;; :translation-axis (make-sequence 'list (length head-and-limbs) :initial-element t)
      :revert-if-fail revert-if-fail
      :rthre (make-sequence 'list (length head-and-limbs) :initial-element rthre)
      :thre (make-sequence 'list (length head-and-limbs) :initial-element thre)
      :additional-weight-list
        (concatenate cons
             (list-transpose (list (send rb :head) (range (length (send rb :head)) :step 0))) ;; fix head
             (get-weighted-ll rb :rleg :init-elem 250 :step 50)
             (get-weighted-ll rb :lleg :init-elem 250 :step 50)
             (get-weighted-ll rb :torso :init-elem 1000 :step 500))
      :stop 100
      :debug-view debug-view
  )
  (send (send rb :head :end-coords :parent) :dissoc obsv-coords)
)

ar-ik-common 内の:fullbody-inverse-kinematicsで

      :additional-weight-list
        (concatenate cons
             (list-transpose (list (send rb :head) (range (length (send rb :head)) :step 0))) ;; fix head
             (get-weighted-ll rb :rleg :init-elem 250 :step 50)
             (get-weighted-ll rb :lleg :init-elem 250 :step 50)
             (get-weighted-ll rb :torso :init-elem 1000 :step 500))

となっているのが追加点で，頭以外にも足裏から torso までのlinkと胴体部分の weight を大きくしています．

この結果，前回の頭も動かしていた状態:

から，頭や腕を動かさずに傾いた状態:

を表現することができ，実際のロボットの姿勢に近い状態が得られました．

[hiroya1224]

(cc 忘れていました)
cc. @k-okada @pazeshun @haraduka

[Naoki-Hiraoka]

今更ですが，
引数:link-listとして，通常のリンクリストから動かしたくない関節のリンクだけを除いたものを与えると，その関節を全く動かさずにIKが解ける，
という方法もあったなと思いました．

[pazeshun]

@hiroya1224 ありがとうございます。
:additional-weight-listが良く使われている（けど気付かない人も多い）例として、HRP2の爪先を動かなくしてIKを解くというのがあり、これが一番シンプルなコードかと思います。
https://github.com/start-jsk/rtmros_tutorials/blob/44917f78c458edc6552321c009d93d8a14402f85/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l#L26-L37

Baxterでも、同様にして、グリッパの手先関節を動かなくしていた時期がありました。
https://github.com/start-jsk/jsk_apc/pull/1362/files#diff-362f4f01bf9e24300d70132e8f4740afR16-R25
ところがこれだと、明らかに届きそうな場所なのにIKが失敗するということがあり（start-jsk/jsk_apc#1470 ）、現在は @Naoki-Hiraoka のコメントと同様に、:link-listに与えるリストを編集する、という方法を取っています。
https://github.com/start-jsk/jsk_apc/pull/1529/files#diff-362f4f01bf9e24300d70132e8f4740afR40-R50

IKが失敗する問題は、#380 で治るはずなのですが、逆に既存のコードが動かなくなってしまう部分があってストップしているので、:additional-weight-listを使う場合は留意しておくとよいかと思います。