Open furushchev opened 6 years ago
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)
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)
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:
↓
but IK fails with weird pose:
Is there any idea?