loop-view-hand-pose.l

#!/usr/bin/env roseus
;; vim: set ft=lisp:

(require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l")

(ros::roseus "loop_view_hand_pose")

(setq arm :rarm)
(if (eq arm :larm)
  (setq bin-list (list :a :b :d :e :g :h :j :k))
  (setq bin-list (list :c :e :f :h :i :k :l :b))
  )

(jsk_2016_01_baxter_apc::baxter-init)
(send *ri* :gripper-servo-on arm)
(send *ri* :angle-vector
      (send *baxter* :fold-to-keep-object arm)
      5000)
(send *ri* :wait-interpolation)
(send *ri* :angle-vector
      (send *baxter* :fold-pose-back arm)
      5000)
(send *ri* :wait-interpolation)

(dolist (bin (list :c :k)) ;; bin-list)
  (ros::ros-info "[~a] Moving arm ~a to bin ~a" (ros::get-name) arm bin)
  (send *ri* :angle-vector
    (send *baxter* :avoid-shelf-pose arm bin)
    5000)
  (send *ri* :wait-interpolation)

  (send *ri* :angle-vector
    (send *baxter* :view-hand-pose arm bin)
    5000)
  (send *ri* :wait-interpolation)

  (send *ri* :angle-vector
    (send *baxter* :avoid-shelf-pose arm bin)
    5000)
  (send *ri* :wait-interpolation)

  (send *ri* :angle-vector
        (send *baxter* :fold-to-keep-object arm)
        5000)
  (send *ri* :wait-interpolation)
  )
(exit)