Weird solution of inverse kinematics for directing head with torso

[furushchev]

I want to let robots head a certain point using both head pan-tilt link and torso link so to make the movement of head is as least as possible (= want to make use of torso at first)

For example, I want to solve ik like below:

↓

(load "package://pr2eus/pr2.l")
(load "models/arrow-object.l")
(setq *robot* (pr2) *target* (arrow))
(send *target* :locate #f(1000 0 1500))
(objects (list *robot* *target*))
(send *robot* :reset-manip-pose)

(send *robot* :inverse-kinematics
      (make-coords :pos (send a :worldpos))
      :move-target (send *robot* :head :end-coords)
      :link-list (send *robot* :link-list (send (send *robot* :head :end-coords) :parent))
      :additional-weight-list (list
                               (list (send *robot* :head :neck-y :child-link) 0)
                               (list (send *robot* :head :neck-p :child-link) 0)
                               (list (send *robot* :torso :waist-z :child-link) 1000))
      :rotation-axis nil
      :translation-axis :z
      :debug-view :no-message)

but IK fails with weird pose:

;; inverse-kinematics failed.
;; dif-pos : #f(-637.18 0.0 0.0)/(637.18/1)
;; dif-rot : #f(0.0 0.0 0.0)/(0.0/0.017453)
;;  coords : #<coordinates #X6acf9c8  0.0 0.0 0.0 / 0.0 0.0 0.0>
;;  angles : (300.0 75.0 50.0 110.0 -110.0 -20.0 -10.0 -10.0 -75.0 50.0 -110.0 -110.0 20.0 -10.0 -10.\
0 0.0 50.0)
;;    args : ((#<coordinates #X7970440  1000.0 0.0 1500.0 / 0.0 0.0 0.0>) :move-target #<cascaded-coo\
rds #X7386608 :head-end-coords  151.939 0.0 1519.404 / 3.142 0.698 3.142> :link-list (#<bodyset-link \
#X721c008 torso_lift_link  -50.0 0.0 1115.675 / 0.0 0.0 0.0> #<bodyset-link #X618dd78 head_pan_link  \
-67.07 0.0 1497.125 / 0.0 0.0 0.0> #<bodyset-link #X618e498 head_tilt_link  0.93 0.0 1497.125 / 0.0 0\
.873 0.0>) :move-target #<cascaded-coords #X7386608 :head-end-coords  151.939 0.0 1519.404 / 3.142 0.\
698 3.142> :link-list (#<bodyset-link #X721c008 torso_lift_link  -50.0 0.0 1115.675 / 0.0 0.0 0.0> #<\
bodyset-link #X618dd78 head_pan_link  -67.07 0.0 1497.125 / 0.0 0.0 0.0> #<bodyset-link #X618e498 hea\
d_tilt_link  0.93 0.0 1497.125 / 0.0 0.873 0.0>) :additional-weight-list ((#<bodyset-link #X618dd78 h\
ead_pan_link  -67.07 0.0 1497.125 / 0.0 0.0 0.0> 0) (#<bodyset-link #X618e498 head_tilt_link  0.93 0.\
0 1497.125 / 0.0 0.873 0.0> 0) (#<bodyset-link #X721c008 torso_lift_link  -50.0 0.0 1115.675 / 0.0 0.\
0 0.0> 1000)) :rotation-axis nil :translation-axis :z :debug-view :no-message)
;; command : (let ((r (instance pr2-sensor-robot :init))) (progn (send r :newcoords (make-coords :4x4\
 #2f((1.0 0.0 0.0 0.0) (0.0 1.0 0.0 0.0) (0.0 0.0 1.0 0.0) (0.0 0.0 0.0 1.0)))) (mapc #'(lambda (j a)\
 (send* j :joint-angle a nil)) (list (send r :torso_lift_joint) (send r :l_shoulder_pan_joint) (send \
r :l_shoulder_lift_joint) (send r :l_upper_arm_roll_joint) (send r :l_elbow_flex_joint) (send r :l_fo\
rearm_roll_joint) (send r :l_wrist_flex_joint) (send r :l_wrist_roll_joint) (send r :r_shoulder_pan_j\
oint) (send r :r_shoulder_lift_joint) (send r :r_upper_arm_roll_joint) (send r :r_elbow_flex_joint) (\
send r :r_forearm_roll_joint) (send r :r_wrist_flex_joint) (send r :r_wrist_roll_joint) (send r :head\
_pan_joint) (send r :head_tilt_joint)) '(300.0 75.0 50.0 110.0 -110.0 -20.0 -10.0 -10.0 -75.0 50.0 -1\
10.0 -110.0 20.0 -10.0 -10.0 0.0 50.0)) (objects (list r))) (send* r :inverse-kinematics (list (list \
(make-coords :pos #f(1000.0 0.0 1500.0) :rot #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) :dump-c\
ommand nil :debug-view t :move-target (let* ((p (send r :link "head_tilt_link")) (mt (make-cascoords \
:coords (send (send p :copy-worldcoords) :transform (make-coords :4x4 #2f((3.330669e-16 0.0 1.0 80.0)\
 (0.0 1.0 0.0 0.0) (-1.0 0.0 3.330669e-16 130.0) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :link\
-list (list (send r :link "torso_lift_link") (send r :link "head_pan_link") (send r :link "head_tilt_\
link")) :move-target (let* ((p (send r :link "head_tilt_link")) (mt (make-cascoords :coords (send (se\
nd p :copy-worldcoords) :transform (make-coords :4x4 #2f((3.330669e-16 0.0 1.0 80.0) (0.0 1.0 0.0 0.0\
) (-1.0 0.0 3.330669e-16 130.0) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :link-list (list (send\
 r :link "torso_lift_link") (send r :link "head_pan_link") (send r :link "head_tilt_link")) :addition\
al-weight-list (list (list (send r :link "head_pan_link") 0) (list (send r :link "head_tilt_link") 0)\
 (list (send r :link "torso_lift_link") 1000)) :rotation-axis nil :translation-axis :z :debug-view :n\
o-message)))

Is there any idea?

[furushchev]

I found that the weights of pan-tilt link should not be zero but relatively small values to torso and starts to solve with desired head direction.
I got almost desired pose but still ik fails...

(send *robot* :head :angle-vector #f(0 0))
(send *robot* :inverse-kinematics
      (make-coords :pos (send *target* :worldpos))
      :move-target (send *robot* :head :end-coords)
      :link-list (send *robot* :link-list (send (send *robot* :head :end-coords) :parent))
      :additional-weight-list (list
                               (list (send *robot* :head :neck-y :child-link) 100)
                               (list (send *robot* :head :neck-p :child-link) 100)
                               (list (send *robot* :torso :waist-z :child-link) 1000))
      :rotation-axis nil
      :translation-axis :z
      :debug-view :no-message)

[Naoki-Hiraoka]

This code solved the problem.

(load "package://pr2eus/pr2.l")
(load "models/arrow-object.l")
(setq *robot* (pr2) *target* (arrow))
(send *target* :locate #f(1000 0 1500))
(objects (list *robot* *target*))
(send *robot* :reset-manip-pose)
(send *robot* :head :angle-vector #f(0 0))

(send *robot* :put :tmp-head-end-coords
      (make-cascoords
       :name :tmp-head-end-coords
       :coords (send (send *robot* :head :end-coords :copy-worldcoords) :translate (float-vector 0 0 (distance (send *robot* :head :end-coords :worldpos) (send *target* :worldpos))))
       :parent (send (send *robot* :head :end-coords) :parent)))

(send *robot* :inverse-kinematics
      (make-coords :pos (send *target* :worldpos))
      :move-target (send *robot* :get :tmp-head-end-coords)
      :link-list (send *robot* :link-list (send (send *robot* :get :tmp-head-end-coords) :parent))
      :additional-weight-list (list
                               (list (send *robot* :head :neck-y :child-link) 100)
                               (list (send *robot* :head :neck-p :child-link) 100)
                               (list (send *robot* :torso :waist-z :child-link) 1000))
      :rotation-axis nil
      :translation-axis :z
      :move-joints-hook #'(lambda () (send (send *robot* :get :tmp-head-end-coords) :transform (send (send *robot* :get :tmp-head-end-coords) :transformation (send (send *robot* :head :end-coords :copy-worldcoords) :translate (float-vector 0 0 (distance (send *robot* :head :end-coords :worldpos) (send *target* :worldpos)))))))
      :debug-view :no-message)