pr2-test-ik.l

(load "package://pr2eus/pr2-interface.l")
(setq *robot* (pr2))
(send *robot* :reset-pose)
(setq *cube* (make-cube 300 300 300))
(send *cube* :translate #f(500 0 800))
(send *robot* :larm :inverse-kinematics
      (make-coords :pos #f(500 0 950)) :rotation-axis :z)
(send *robot* :rarm :inverse-kinematics
      (make-coords :pos #f(500 0 650)) :rotation-axis :z)
(send *robot* :larm :end-coords :assoc *cube*)
(setq *target-coords* (make-coords :pos #f(500 0 900) :rpy #f(45 0 0)))
(objects (list *robot* *cube*))

(send *robot* :inverse-kinematics *target-coords*
      :move-target *cube*
      :link-list (send *robot* :link-list (send (send *robot* :larm :end-coords) :parent))
      :rotation-axis t
      :debug-view t)