PR2 manipulation

pr2-tray-inifinity-rotate.l

(load "models/tray-object.l")
(load "package://pr2eus/pr2.l")

(defun pr2-rotate-tray
    ()
  (setq *obj* (tray))
  (send *obj* :put :long-left-coords
        (make-cascoords :parent *obj* :pos (float-vector 0 200 0) :rpy (list 0 pi/2 0) :name :long-left-coords))
  (send *obj* :put :long-right-coords
        (make-cascoords :parent *obj* :pos (float-vector 0 -200 0) :rpy (list 0 pi/2 0) :name :long-right-coords))

  ;; (send (send *obj* :get :long-left-coords) :draw-on :flush t :size 50 :color #f(0 1 0) :width 5)                                                                                                                              
  ;; (send (send *obj* :get :long-right-coords) :draw-on :flush t :size 50 :color #f(0 1 0) :width 5)                                                                                                                             

  (setq *robot* (pr2))

  (objects (list *robot* *obj*))
  (send *obj* :newcoords (make-coords :pos (float-vector 400 0 500)))

  (do-until-key
   (send *obj* :rotate (deg2rad 1) :z :world)
   (send *robot* :inverse-kinematics
         (list (send *obj* :get :long-left-coords) (send *obj* :get :long-right-coords))
         :move-target (list (send *robot* :larm :end-coords) (send *robot* :rarm :end-coords))
         )
   (send *irtviewer* :draw-objects)
   (x::window-main-one)
   ))