PR2 inverse kinematics considering self collision

pr2-collision-check-ik.l

(load "package://pr2eus/pr2.l")
(setq *robot* (pr2))
(objects (list *robot*))

(send *robot* :reset-pose)
(send *robot* :inverse-kinematics
      (list (make-coords :pos (float-vector 370 -200 770))
            (make-coords :pos (float-vector 370 200 790)))
      :move-target (list (send *robot* :larm :end-coords) (send *robot* :rarm :end-coords))
      :list-list(list (send *robot* :link-list (send (send *robot* :larm :end-coords) :parent)) (send *robot* :link-list (send (send *robot* :rarm :end-coords) :parent)))
      :rotation-axis (list nil nil)
      :avoid-collision-distance 100
      :avoid-collision-null-gain 5.0
      :avoid-collision-joint-gain 0.8
      :collision-avoidance-link-pair
      (reduce #'append
              (mapcar #'(lambda (rarm-link)
                          (mapcar #'(lambda (larm-link) (list (send *robot* :link larm-link) (send *robot* :link rarm-link)))
                                  (list "l_shoulder_pan_link" "l_shoulder_lift_link" "l_upper_arm_roll_link" "l_elbow_flex_link" "l_forearm_roll_link" "l_wrist_flex_link" "l_wrist_roll_link")))
                      (list "r_shoulder_pan_link" "r_shoulder_lift_link" "r_upper_arm_roll_link" "r_elbow_flex_link" "r_forearm_roll_link" "r_wrist_flex_link" "r_wrist_roll_link"))
              )
      :debug-view :no-messge
      :stop 1000
      )

@furushchev

これで，干渉をよけようとしてIKを解くけれど，

1. collison-avoidance-link-pair で指定しているのは，(send *robot* :larm :links), (send *robot* :rarm :links)だけれど，:draw-on :flush tしてみると手首の太いリンクがこれに含まれていないのでぶつかってしまう．
2. stopを結構大きくしないと，解があるような場合も解まで到達できない場合がある．