pr2-moveit.l

(require :pr2-interface "package://pr2eus/pr2-interface.l")
(require :pr2eus-moveit "package://pr2eus_moveit/euslisp/pr2eus-moveit.l")
(load "package://pr2eus_moveit/tutorials/collision-object-sample.l")

;; pr2eus_moveit
(defun pr2-moveit-demo ()
  (let (cds ret)
    (pr2-init)

    (send *pr2* :angle-vector (send *ri* :state :reference-vector))
    (objects (list *pr2*))

    (setq *me* (instance pr2-moveit-environment :init))
    (send *me* :robot :angle-vector (send *ri* :state :reference-vector))

    (setq cds (send *me* :robot :larm :end-coords :copy-worldcoords))
    (send cds :translate #f(0 0 -100) :world)
    (send cds :draw-on :flush t :size 100)
    (send *me* :get-ik-for-pose cds :larm :end-coords (list :larm :end-coords))

    (setq ret (send *me* :motion-plan :larm))
    (send *me* :execute-trajectory (send ret :trajectory)) ;; FIX ME IT DOES NOT WORK... [ERROR] [1574826354.919305258, 274.079000000]: attempted to call service  /execute_kinematic_path, but failed 
    ))

;; moveit_demo
(defun pr2-moveit-collision-demo ()
  (let (ret)
    (pr2-init)
    (init-collision-demo *pr2* "base_link")
    (pub-objects)
    (objects (append (list *pr2*) (objects)))
    (send *pr2* :angle-vector (send *ri* :state :reference-vector))
    (send *irtviewer* :draw-objects)
    ;;
    (setq larm-target #s(coordinates plist nil rot #2f((0.968121 0.247905 0.035856) (-0.247379 0.968742 -0.018505) (-0.039323 0.009045 0.999186)) pos #f(-81.4698 172.586 808.425)))
    (setq *me* (instance pr2-moveit-environment :init))
    (send *me* :sync-robot-model *pr2*)
    (send *me* :get-ik-for-pose larm-target :larm :end-coords (list :larm :end-coords))

    (setq ret (send *me* :motion-plan :larm))
    (send *me* :execute-trajectory (send ret :trajectory))
    ;;
    ;; OR
    ;;
    ;; execute plan in rviz
    ;;
    ;;

    (send *pr2* :angle-vector (send *ri* :state :reference-vector))
    (send *irtviewer* :draw-objects)
    (sync-larm)

    (send *ri* :start-grasp :larm)
    (send *ri* :angle-vector (send *pr2* :angle-vector) 2000)

    (send *pr2* :angle-vector (send *ri* :state :reference-vector))
    (send *pr2* :larm :inverse-kinematics
          (send (send *pr2* :larm :end-coords :copy-worldcoords) :translate #f(0 0 100) :world))
    (send *irtviewer* :draw-objects)
    (send *ri* :angle-vector (send *pr2* :angle-vector) 3000)
    ))