Using Euslisp(roseus) to control robot behavior through perception

Description: This tutorial shows how to write perception based behavir program

Tutorial Level: BEGINNER

Install related software

Euslisp Setup

Install from deb

• sudo apt-get install ros-hydro-pr2eus

Gazebo Setup (if use gazebo simulator)

Install from deb

• sudo apt-get install ros-hydro-pr2-gazebo

writing the robot node

source code

create the pr2main.l file in your folder https://github.com/jsk-ros-pkg/jsk_pr2eus/tree/master/pr2eus_tutorials

• (load "package://pr2eus/pr2-interface.l")
  
  (ros::roseus "pr2_main")
  
  (defmethod pr2-robot
    (:initial-pose ()
     (send self :angle-vector
           (float-vector 12.0 45.0 60.0 0.0 -120.0 0.0 -45.0 0.0 -45.0 60.0 0.0 -120.0 0.0 -45.0 0.0 0.0 35.0))
     ))
  
  #|
  ;; make limb's index vector
  (map integer-vector
       #'(lambda (j)
           (position j (send *pr2* :joint-list)))
       (send *pr2* limb :joint-list))
  |#
  (defmethod robot-interface
    (:error-coords (limb)
     (let ((pt (send self :state :potentio-vector))
           (ds (send self :state-vector :desired))
           (limb-pt (send robot limb :angle-vector))
           (pt-cds (send robot limb :end-coords))
           limb-ds ds-cds diff-cds)
       (send robot :angle-vector ds)
       (setq ds-cds (send robot limb :end-coords)
             limb-ds (send robot limb :angle-vector))
       (setq diff-cds (send pt-cds :transformation ds-cds))
       (setf (get diff-cds :diff-angle-vector) (v- limb-pt limb-ds))
       (setf (get diff-cds :limb) limb)
       (send robot :angle-vector pt)
       diff-cds
       ))
    )
  
  ;; initial pose
  (defun goto-init-pose (&key (wait t) (tm 4000))
    (send *pr2* :initial-pose)
    (send *ri* :angle-vector (send *pr2* :angle-vector) tm)
    (if wait (send *ri* :wait-interpolation)))
  
  (warn "
  (pr2-init)
  (goto-init-pose)
  ")

then create the tabletop-sample.l in the same folder

• (ros::roseus-add-msgs "posedetection_msgs")
  
  (load "pr2main.l")
  ;; (load "package://pr2eus_tutorials/arm-navigation-sample.l")
  
  (defun detect-callback ( msg )
    (setq *msg* msg))
  
  (setq *msg* nil)
  (ros::roseus "tabletop_object_sample")
  ;; /ObjectDetection is the topic that tells object_pose
  (ros::subscribe "/ObjectDetection" posedetection_msgs::ObjectDetection
                  #'detect-callback)
  
  ;; initialize PR2 ROS-euslisp interface
  (pr2-init t)
  ;; goto initial pose
  (goto-init-pose)
  (send *irtviewer* :draw-objects)
  
  ;;
  (unless (boundp '*tfl*)
    (setq *tfl* (instance ros::transform-listener :init)))
  
  ;; get pose in /ObjectDetection
  (defun detect-one (&optional (timeout 500))
    "subscribe just one detection result and return list of worldcoords of object's centroid"
    (let (lst
          (cntr 0)
          trans-base->table)
      (setq *msg* nil)
  
      (ros::rate 10)
      (while (null *msg*)
        (ros::spin-once)
        (ros::sleep)
        (if (> (incf cntr) timeout)
            (return-from detect-one nil)))
  
      (setq trans-base->table
            (send *tfl* :lookup-transform
                  "/base_footprint"
                  (send *msg* :header :frame_id)
                  (send *msg* :header :stamp)))
  
      (dolist (obj (send *msg* :objects))
        (let ((trs
               (send (send trans-base->table :copy-worldcoords)
                     :transform (ros::tf-pose->coords (send obj :pose)))))
          (setf (get trs :type) (send obj :type))
          (push trs lst)
          ))
      lst))
  
  (unix::sleep 5) ;; waiting detection
  
  (defun pickup-pick (&key (arm :rarm))
    (let ((av-st (send *pr2* :angle-vector))
          av-ed av-mid)
    (setq *res* (detect-one))
  
    ;; sort nearest first// 
    (sort *res*
          #'(lambda (x y)
              (<= (norm (send (send (send *pr2* arm :end-coords :worldcoords) :transformation x) :pos))
                  (norm (send (send (send *pr2* arm :end-coords :worldcoords) :transformation y) :pos)))))
    (cond
     ((send *pr2* arm :inverse-kinematics (car *res*) :rotation-axis :z)
      ;;(setq av-ed (send *pr2* :angle-vector))
      ;; move hands to object IK(inverse kinematics)
      (let ((cds (send *pr2* arm :end-coords :copy-worldcoords)))
        (send cds :translate (float-vector 25 0 10 )) ;; 25mm offset of centroid
        (if (send *pr2* arm :inverse-kinematics cds :rotation-axis :z)
            (setq av-ed (send *pr2* :angle-vector))))
  
      (let ((cds (send *pr2* arm :end-coords :copy-worldcoords)))
        (send cds :translate (float-vector -75 0 0)) ;; linear reach to object
        (if (send *pr2* arm :inverse-kinematics cds)
            (setq av-mid (send *pr2* :angle-vector))))
      (send *irtviewer* :draw-objects)
      (when av-mid
        ;; open hand
        (send *ri* :stop-grasp arm)
        (send *ri* :wait-interpolation) ;; wait for finising action
        ;; move arm
        (send *ri* :angle-vector-sequence
              (list av-mid av-ed) (list 3800 1200))
        (send *ri* :wait-interpolation) ;; wait for finising action
  
        ;; close hand
        ;;(> (send *ri* :start-grasp arm) 8) ;; hand width is more than 8mm
        ;; gripper action not returned while have not reached goal position ???
        (send *ri* :move-gripper arm 0.03 :wait nil)
        (unix::usleep (* 2 1000 1000))
        )
      )
     (t nil))
    (send *irtviewer* :draw-objects)
    ))
  
  (defun pickup-up (&key (arm :rarm))
    ;; move hand up a little
    (send *pr2* arm :move-end-pos (float-vector 0 0 55) :world)
    (send *ri* :angle-vector (send *pr2* :angle-vector) 2500)
    (send *ri* :wait-interpolation)
    (send *irtviewer* :draw-objects)
    
    (goto-init-pose)
    (send *irtviewer* :draw-objects)
    )
  
  (warn ";;
  ;; (pickup-pick :arm :rarm)
  ;; (pickup-up :arm :rarm)
  ;; (pickup-pick :arm :larm)
  ;; (pickup-up :arm :larm)
  ;;
  ")

Examining the codes

launch gazebo world (if use gazebo)

• roslaunch pr2_gazebo pr2_empty_world.launch

run your program

• roseus tabletop-sample.l

run detection node

you can use any programs that publish /ObjectDetection topic now, we test programs by using 'rostopic pub'

• rostopic pub /ObjectDetection posedetection_msgs/ObjectDetection "header:
    seq: 0
    stamp:
      secs: 0
      nsecs: 0
    frame_id: '/base_link'
  objects:
  - pose:
      position:
        x: 0.4
        y: 0.1
        z: 0.6
      orientation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 0.0
    type: ''" -1

move command

then you can move robot by this command

• ;; in the terminal you run tabletop-simple node
  (pickup-pick :arm :rarm)

if the robot doesn't move, you may have to command

• (ros::spin-once)

before 'pickup-pick'