link-listを用いてikを解いたときのエラー

[itohdak]

(load "package://euslisp/jskeus/irteus/irtmodel.l") ;; in case of installed from apt                  
(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")                         
(setq *robot* (instance hrp2jsknts-robot :init))                                                      
(send *robot* :inverse-kinematics                                                                     
      (make-coords)                                                                                   
      :link-list                                                                                      
      (send *robot* :link-list                                                                        
            (send (send *robot* :rarm :end-coords) :parent)                                           
            ;; (send (send *robot* :rarm :elbow-p) :child-link)                                       
            (send (send *robot* :rarm :wrist-y) :child-link)                                          
            )                                                                                         
      :move-target                                                                                    
      (send *robot* :rarm :end-coords)                                                                
      :debug-view :no-message                                                                         
      :translation-axis nil                                                                           
      :rotation-axis t                                                                                
      :revert-if-fail nil                                                                             
      ) 

として，肘から先の手首のみでikを解こうとすると，以下のようなエラーが出ます．

8.irteusgl$ (send *robot* :inverse-kinematics
      (make-coords)
      :link-list
      (send *robot* :link-list
            (send (send *robot* :rarm :end-coords) :parent)
            ;; (send (send *robot* :rarm :elbow-p) :child-link)
            (send (send *robot* :rarm :wrist-y) :child-link)
            )
      :move-target
      (send *robot* :rarm :end-coords)
      :debug-view :no-message
      :translation-axis nil
      :rotation-axis t
      :revert-if-fail nil
      )
Call Stack (max depth: 20):
  0: at (elt link-list i)
  1: at (send (elt link-list i) :worldpos)
  2: at (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos))
  3: at (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0)
  4: at (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i))
  5: at (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i)
  6: at (subseq link-list (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i))
  7: at (if (= (length link-list) 1) link-list (subseq link-list (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i)))
  8: at (setq valid-link-list (if (= (length link-list) 1) link-list (subseq link-list (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i))))
  9: at (while #:dolist9013 (setq link-list (pop #:dolist9013)) (setq valid-link-list (if (= (length link-list) 1) link-list (subseq link-list (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i)))) (setq index-links (mapcar #'(lambda (l) (position l valid-link-list)) collision-links)) (when (and debug (not (memq :no-message debug))) (warn "valid-link-list : ~A~%" (send-all valid-link-list :name)) (warn "index-links     : ~A~%" index-links)) (dotimes (i len) (do ((j (1+ i) (1+ j))) ((>= j len)) (if (and (or (setq i0 (elt index-links i)) (elt index-links j)) (not (eq (elt parent-links i) (elt collision-links j))) (not (eq (elt parent-links j) (elt collision-links i))) (not (eq (elt parent-links j) (elt parent-links i)))) (push (list (elt collision-links (if i0 i j)) (elt collision-links (if i0 j i))) ret)))) (when obstacles (if (atom obstacles) (setq obstacles (list obstacles))) (dotimes (i len) (if (elt index-links i) (dolist (o obstacles) (push (list (elt collision-links i) o) ret))))))
  10: at (let ((link-list nil) (#:dolist9013 link-lists)) nil (while #:dolist9013 (setq link-list (pop #:dolist9013)) (setq valid-link-list (if (= (length link-list) 1) link-list (subseq link-list (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i)))) (setq index-links (mapcar #'(lambda (l) (position l valid-link-list)) collision-links)) (when (and debug (not (memq :no-message debug))) (warn "valid-link-list : ~A~%" (send-all valid-link-list :name)) (warn "index-links     : ~A~%" index-links)) (dotimes (i len) (do ((j (1+ i) (1+ j))) ((>= j len)) (if (and (or (setq i0 (elt index-links i)) (elt index-links j)) (not (eq (elt parent-links i) (elt collision-links j))) (not (eq (elt parent-links j) (elt collision-links i))) (not (eq (elt parent-links j) (elt parent-links i)))) (push (list (elt collision-links (if i0 i j)) (elt collision-links (if i0 j i))) ret)))) (when obstacles (if (atom obstacles) (setq obstacles (list obstacles))) (dotimes (i len) (if (elt index-links i) (dolist (o obstacles) (push (list (elt collision-links i) o) ret)))))) nil)
  11: at (dolist (link-list link-lists) (setq valid-link-list (if (= (length link-list) 1) link-list (subseq link-list (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i)))) (setq index-links (mapcar #'(lambda (l) (position l valid-link-list)) collision-links)) (when (and debug (not (memq :no-message debug))) (warn "valid-link-list : ~A~%" (send-all valid-link-list :name)) (warn "index-links     : ~A~%" index-links)) (dotimes (i len) (do ((j (1+ i) (1+ j))) ((>= j len)) (if (and (or (setq i0 (elt index-links i)) (elt index-links j)) (not (eq (elt parent-links i) (elt collision-links j))) (not (eq (elt parent-links j) (elt collision-links i))) (not (eq (elt parent-links j) (elt parent-links i)))) (push (list (elt collision-links (if i0 i j)) (elt collision-links (if i0 j i))) ret)))) (when obstacles (if (atom obstacles) (setq obstacles (list obstacles))) (dotimes (i len) (if (elt index-links i) (dolist (o obstacles) (push (list (elt collision-links i) o) ret))))))
  12: at (let* ((parent-links (mapcar #'(lambda (ll) (let ((l ll)) (while (and l (send l :parent-link) (eps= (distance (send ll :worldpos) (send (send l :parent-link) :worldpos)) 0)) (setq l (send l :parent-link))) (send l :parent-link))) collision-links)) link-list valid-link-list index-links (len (length collision-links)) i0 ret) (if (atom (car link-lists)) (setq link-lists (list link-lists))) (when (and debug (not (memq :no-message debug))) (warn "collision-links : ~A~%" (send-all collision-links :name)) (warn "parnet-links    : ~A~%" (send-all parent-links :name))) (dolist (link-list link-lists) (setq valid-link-list (if (= (length link-list) 1) link-list (subseq link-list (let ((i 1)) (while (eps= (distance (send (elt link-list 0) :worldpos) (send (elt link-list i) :worldpos)) 0.0) (incf i)) i)))) (setq index-links (mapcar #'(lambda (l) (position l valid-link-list)) collision-links)) (when (and debug (not (memq :no-message debug))) (warn "valid-link-list : ~A~%" (send-all valid-link-list :name)) (warn "index-links     : ~A~%" index-links)) (dotimes (i len) (do ((j (1+ i) (1+ j))) ((>= j len)) (if (and (or (setq i0 (elt index-links i)) (elt index-links j)) (not (eq (elt parent-links i) (elt collision-links j))) (not (eq (elt parent-links j) (elt collision-links i))) (not (eq (elt parent-links j) (elt parent-links i)))) (push (list (elt collision-links (if i0 i j)) (elt collision-links (if i0 j i))) ret)))) (when obstacles (if (atom obstacles) (setq obstacles (list obstacles))) (dotimes (i len) (if (elt index-links i) (dolist (o obstacles) (push (list (elt collision-links i) o) ret)))))) ret)
  13: at (send self :collision-avoidance-link-pair-from-link-list link-list :obstacles (cadr (memq :obstacles args)) :debug (cadr (memq :debug-view args)))
  14: at (apply #'send-message self cascaded-link :move-joints-avoidance tmp-dim :union-link-list union-link-list :rotation-axis rotation-axis :translation-axis translation-axis :jacobi jacobi :debug-view debug-view method-args)
  15: at (apply #'send-message self cascaded-link :move-joints-avoidance tmp-dim :union-link-list union-link-list :rotation-axis rotation-axis :translation-axis translation-axis :jacobi jacobi :debug-view debug-view method-args)
  16: at (apply #'send-message self cascaded-link :move-joints-avoidance tmp-dim :union-link-list union-link-list :rotation-axis rotation-axis :translation-axis translation-axis :jacobi jacobi :debug-view debug-view method-args)
  17: at (send-message* self cascaded-link :move-joints-avoidance tmp-dim :union-link-list union-link-list :rotation-axis rotation-axis :translation-axis translation-axis :jacobi jacobi :debug-view debug-view method-args)
  18: at (progn (when (and debug-view (not (memq :no-clear debug-view)) *viewer*) (send *viewer* :viewsurface :clear)) (send self :put :collision-pair-list nil) (send-message* self cascaded-link :move-joints-avoidance tmp-dim :union-link-list union-link-list :rotation-axis rotation-axis :translation-axis translation-axis :jacobi jacobi :debug-view debug-view method-args) (when (and debug-view *viewer*) (send *viewer* :draw-objects :clear nil :flush nil) (send self :draw-collision-debug-view) (when (car target-coords) (dotimes (i (length target-coords)) (send-message (elt move-target i) coordinates :draw-on :flush nil :size 100) (send (elt target-coords i) :draw-on :flush nil :color #f(1.0 0.0 0.0)))) (dolist (p (send self :get :ik-draw-on-params)) (send* (car p) :draw-on :flush nil (cdr p))) (if (not (memq :no-flush debug-view)) (send *viewer* :viewsurface :flush))) (send self :put :ik-draw-on-params nil) :ik-continues)
  19: at (if success (progn (send self :put :ik-draw-on-params nil) :ik-succeed) (progn (when (and debug-view (not (memq :no-clear debug-view)) *viewer*) (send *viewer* :viewsurface :clear)) (send self :put :collision-pair-list nil) (send-message* self cascaded-link :move-joints-avoidance tmp-dim :union-link-list union-link-list :rotation-axis rotation-axis :translation-axis translation-axis :jacobi jacobi :debug-view debug-view method-args) (when (and debug-view *viewer*) (send *viewer* :draw-objects :clear nil :flush nil) (send self :draw-collision-debug-view) (when (car target-coords) (dotimes (i (length target-coords)) (send-message (elt move-target i) coordinates :draw-on :flush nil :size 100) (send (elt target-coords i) :draw-on :flush nil :color #f(1.0 0.0 0.0)))) (dolist (p (send self :get :ik-draw-on-params)) (send* (car p) :draw-on :flush nil (cdr p))) (if (not (memq :no-flush debug-view)) (send *viewer* :viewsurface :flush))) (send self :put :ik-draw-on-params nil) :ik-continues))
  And more...
/opt/ros/indigo/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: sequence index out of range in (elt link-list i)

jskeus/irtmodels.lを見てみると，
https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1273
のところで，worldposが根元のリンクと等しいリンクは弾かれており，
結果上記の例では定義されていない要素にアクセスしようとしてエラーが起こっているようです．

ちなみに，上のコードで

            ;; (send (send *robot* :rarm :elbow-p) :child-link)                                       
            (send (send *robot* :rarm :wrist-y) :child-link)

のコメントアウトをコメントイン，コメントインをコメントアウトすると正常にikが解けます．

[k-okada]

これは，hrp2jsknts-robot のモデルがわるいの？それともirtmodelが悪いの？後者の場合はsamplerobotで再現できるコードは作れるかな．

[itohdak]

後者だと思います．以下がsample-robotで同じことをしてみた例です．

(load "package://euslisp/jskeus/irteus/demo/sample-robot-model.l")
(setq *robot* (instance sample-robot :init))
(send *robot* :inverse-kinematics
      (make-coords)
      :link-list
      (send *robot* :link-list
            (send (send *robot* :rarm :end-coords) :parent)
            ;; (send (send *robot* :rarm :elbow-p) :child-link)
            (send (send *robot* :rarm :wrist-y) :child-link)
            )
      :move-target
      (send *robot* :rarm :end-coords)
      :debug-view :no-message
      :translation-axis nil
      :rotation-axis t
      :revert-if-fail nil
      )

前回と同じく，

            ;; (send (send *robot* :rarm :elbow-p) :child-link)
            (send (send *robot* :rarm :wrist-y) :child-link)

のコメントアウトとコメントインを入れ替えると正常に動きます．

[mmurooka]

こちら一週間くらい前の@itohdak の実験で手首のみのIKを解こうとして発覚し，取り急ぎ@iory の仮想関節を適当なところに追加する案でエラーを回避して進めてもらっていますが，せっかくなのでIssue立ててもらいました．

https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1273-L1281 でlink-listから根本のリンクと異なる位置のリンクを探している理由は何になりますでしょうか．
cc @snozawa

[snozawa]

#460
で対応してみています。
インデックスのチェックのみいれています。

このあたりのコードでworldposをみているのは、ヒューリスティックとして
３軸、２軸直交してたりする関節は隣接するところで干渉チェックしないようにしてたと思います。
隣接、をeps=でworldposが同じかどうかでみていたと思います。

ただ、肝心のvalid-link-listのエラーになった部分だけ思い出せないです。
おおもとは
b0e3b48
なのですが、何か思い出せますでしょうか@k-okada

[k-okada]

ただ、肝心のvalid-link-listのエラーになった部分だけ思い出せないです。

たしかに，なんでしょうね．思い出せない．根本だけ重なってるリンクを除去しているんでしょうか．でも肩は除去して手首は除去しないのも不思議ですよね．

プログラムの修正は，すこしなおして
#461
を作ってみました．ところで，これを直しても，もともともの @itohdak のコードは

/home/k-okada/catkin_ws/ws_euslisp/devel/share/euslisp/jskeus/eus/Linux64/bin/i\
rteusgl 0 error: vector expected in (m* |JACOBI#| jacobi tmp-mcc2)

となるけどいいのかな．

[snozawa]

ちなにみエラーがでたものはsamplerobotでしたか？
もとのコードの前に、(send *robot* :reset-pose)したらでなかったかもしれません。

たしかに，なんでしょうね．思い出せない．根本だけ重なってるリンクを除去しているんでしょうか．でも肩は除去して手首は除去しないのも不思議ですよね．

そうですね、このあたりの部分が私も思い出せないです。。。