View source for Zero Prerequisites Demo Tutorial: Simple Fetch and Place

This page provides a consolidated version of the code required for the  [http://cram-system.org/tutorials/demo/fetch_and_place Zero prerequisites demo tutorial: Simple fetch and place].  You normally do this tutorial in an interactive manner, leading to the creation of the code for the move-bottle function that is pasted into the <code>pick-and-place.lisp</code> file for the first example. The second and third examples on failure handling modify this code.  

Here, we provide the code for three versions of <code>move-bottle</code>, one for each example: <code>move-bottle</code>, <code>move-bottle2</code>, and <code>move-bottle3</code>.  This allows you to add code to the pick-and-place.lisp just once and so that you can simply do the tutorial by invoking the example commands, i.e. by evaluating the three example forms in REPL, each one exemplifying one specific aspect of the plan.  

We also include a fourth version, <code>move-bottle4</code>, which covers the example of defining a new grasp, directly after Exercise 3. 

For convenience, we also include four dummy functions to use when doing exercises 1 - 4.
 
<code></code>

Note that here we don't cover the material in the first two sections of the tutorial, i.e. "Setting Up" and "Understanding the Basics".   You need to go through these yourself.  Here, we just cover the material in Tutorial 1: Simple Fetch and Place, Tutorial 2 & 3: Failure Handling, and Tutorial 4: Alternative Grasping Poses (Advanced).


== Update <code>pick-and-place.lisp</code> ==

First, let's copy the example code.  

Move into the src directory:

<!-- $ cd ~/workspace/ros/src/cram/cram_tutorials/cram_pick_place_tutorial/src-->
 $ cd ~/workspace/cram_tutorial/src/cram/cram_tutorials/cram_pick_place_tutorial/src


Edit <code>pick-and-place.lisp</code> 
 
Copy and paste the code below.
 
 (in-package :pp-tut)
 
 (defparameter *base-pose-near-table*
   (make-pose "map" '((-1.447 -0.15 0.0) (0.0 0.0 -0.7071 0.7071))))
 
 (defparameter *downward-look-coordinate*
    (make-pose "base_footprint" '((0.65335 0.076 0.758) (0 0 0 1))))
 
 (defparameter *base-pose-near-counter*
   (make-pose "map" '((-0.15 2 0) (0 0 -1 0))))
 
 (defparameter *final-object-destination*
   (make-pose "map" '((-0.8 2 0.95) (0 0 0 1)))) ;; NB: changed z value to 0.95 since 0.9 (as specified in the tutorial) is below the counter top
 
 (defparameter *left-downward-look-coordinate*
   (make-pose "base_footprint" '((0.65335 0.76 0.758) (0 0 0 1))))
 
 (defparameter *right-downward-look-coordinate*
   (make-pose "base_footprint" '((0.65335 -0.76 0.758) (0 0 0 1))))
 
 (defparameter *lift-z-offset* 0.15 "in meters")
 
 (defparameter *lift-offset* `(0.0 0.0 ,*lift-z-offset*))
 
 
 ;; definition of front-left diagonal grasp
 ;; =======================================
 
 (defparameter *bottle-pregrasp-xy-offset* 0.15 "in meters")
 (defparameter *bottle-grasp-xy-offset* 0.02 "in meters")
 (defparameter *bottle-grasp-z-offset* 0.005 "in meters") 
 
 (defparameter *sin-pi/4* (sin (/ pi 4)))
 (defparameter *-sin-pi/4* (- (sin (/ pi 4))))
  
 (defparameter *diagonal-rotation*
               `((,*sin-pi/4* 0 ,*-sin-pi/4*)
                 (,*-sin-pi/4* 0 ,*-sin-pi/4*)
                 (0 1 0)))
 
 (cram-object-interfaces:def-object-type-to-gripper-transforms '(:drink :bottle) '(:left :right) :fl-diagonal
   :grasp-translation `(,(- *bottle-grasp-xy-offset*) ,(- *bottle-grasp-xy-offset*) ,*bottle-grasp-z-offset*)
   :grasp-rot-matrix *diagonal-rotation*
   :pregrasp-offsets `(,*bottle-pregrasp-xy-offset* ,*bottle-pregrasp-xy-offset* ,*lift-z-offset*)
   :2nd-pregrasp-offsets `(,*bottle-pregrasp-xy-offset* ,*bottle-pregrasp-xy-offset* 0.0)
   :lift-offsets *lift-offset*
   :2nd-lift-offsets *lift-offset*)
 
 ;; failure handling: different looking directions
 ;; ==============================================
 
 (defun find-object (?object-type)
   (let* ((possible-look-directions `(,*downward-look-coordinate*
                                      ,*left-downward-look-coordinate*
                                      ,*right-downward-look-coordinate*))
          (?looking-direction (first possible-look-directions)))
     (setf possible-look-directions (rest possible-look-directions))
     ;; Look towards the first direction
     (perform (an action
                  (type looking)
                  (target (a location 
                             (pose ?looking-direction)))))
  
     ;; perception-object-not-found is the error that we get when the robot cannot find the object.
     ;; Now we're wrapping it in a failure handling clause to handle it
     (handle-failure perception-object-not-found
         ;; Try the action
         ((perform (an action
                       (type detecting)
                       (object (an object 
                                  (type ?object-type))))))
  
       ;; If the action fails, try the following:
       ;; try different look directions until there is none left.
       (when possible-look-directions
         (print "Perception error happened! Turning head.")
         ;; Resetting the head to look forward before turning again
         (perform (an action
                      (type looking) 
                      (direction forward)))
         (setf ?looking-direction (first possible-look-directions))
         (setf possible-look-directions (rest possible-look-directions))
         (perform (an action 
                      (type looking)
                      (target (a location
                                 (pose ?looking-direction)))))
         ;; This statement retries the action again
         (cpl:retry))
       ;; If everything else fails, error out
       ;; Reset the neck before erroring out
       (perform (an action
                    (type looking)
                    (direction forward)))      
       (cpl:fail 'object-nowhere-to-be-found))))
 
 ;; pick up object with a specified arm
 ;; ===================================
 
 (defun pick-up-object (?perceived-object ?grasping-arm)
   (let* ((?possible-grasps '(:left-side :right-side :front :back))  ;define all possible grasps
          (?remaining-grasps (copy-list ?possible-grasps))           ;make a copy to work though when trying each grasp
          (?grasp (first ?remaining-grasps)))                        ;this is the first one to try
    
       (cpl:with-retry-counters ((arm-change-retry 1))               ;there is just one alternative arm if the first one fails
      
          ;; Outer handle-failure handling arm change
          (handle-failure object-unreachable
             ((setf ?remaining-grasps (copy-list ?possible-grasps))  ;make sure to try all possible grasps for each arm
              (setf ?grasp (first ?remaining-grasps))                ;get the first grasp to try
              (setf ?remaining-grasps (rest ?remaining-grasps))	    ;update the remaining grasps to try
             
             ;; Inner handle-failure handling grasp change
             (handle-failure (or manipulation-pose-unreachable gripper-closed-completely)
                ;; Try to perform the pick up
                ((perform (an action
                              (type picking-up)
                              (arm ?grasping-arm)
                              (grasp ?grasp)
                              (object ?perceived-object))))
 
                ;; When pick-up fails this block gets executed 
                (format t "~%Grasping failed with ~a arm and ~a grasp~%" ?grasping-arm ?grasp)
                ;;(format t "~%Error: ~a~%" e)                       ;uncomment to see the error message
 
                ;; Check if we have any remaining grasps left.
                ;; If yes, then the block nested to it gets executed, which will
                ;; set the grasp that is used to the new value and trigger retry
 
                (when (first ?remaining-grasps)                      ;if there is a grasp remaining
                   (setf ?grasp (first ?remaining-grasps))           ;get it
                   (setf ?remaining-grasps (rest ?remaining-grasps)) ;update the remaining grasps to try
                   (format t "Retrying with ~a arm and ~a grasp~%" ?grasping-arm ?grasp)
                   (park-arms)
                   (cpl:retry))
                 ;; This will get executed when there are no more elements in the 
                 ;; ?possible-grasps list. We print the error message and throw a new error
                 ;; which will be caught by the outer handle-failure
                 (print  "No more grasp retries left :(")
                 (cpl:fail 'object-unreachable)))
 
              ;; This is the failure management of the outer handle-failure call
              ;; It changes the arm that is used to grasp
              (format t "Manipulation failed with the ~a arm"?grasping-arm)
              ;; (print e)                                           ;uncomment if you want to see the error
              ;; Here we use the retry counter we defined. The value is decremented automatically
              (cpl:do-retry arm-change-retry
                 ;; if the current grasping arm is right set left, else set right
                 (setf ?grasping-arm (if (eq ?grasping-arm :right) 
                                         :left
                                         :right))
                 (park-arms)
                 (cpl:retry))
              ;; When all retries are exhausted print the error message.
              (print "No more arm change retries left :("))))
  ?grasping-arm) ; function value is the arm that was used to grasp the object
 
 
 ;; baseline plan - (move-bottle '((-1.6 -0.9 0.82) (0 0 0 1)))
 ;; ===========================================================
 
 (defun move-bottle (bottle-spawn-pose)
   (spawn-object bottle-spawn-pose)
   (with-simulated-robot
     (let ((?navigation-goal *base-pose-near-table*))
       (cpl:par
         ;; Moving the robot near the table.
         (perform (an action
                      (type going)
                      (target (a location 
                                 (pose ?navigation-goal)))))
         (perform (a motion 
                     (type moving-torso)
                     (joint-angle 0.3)))
         (park-arms)))
     ;; Looking towards the bottle before perceiving.
     (let ((?looking-direction *downward-look-coordinate*))
       (perform (an action
                    (type looking)
                    (target (a location 
                               (pose ?looking-direction))))))
     ;; Detect the bottle on the table.
     (let ((?grasping-arm :right)
           (?perceived-bottle (perform (an action
                                           (type detecting)
                                           (object (an object 
                                                      (type bottle)))))))
       ;; Pick up the bottle
       (perform (an action
                    (type picking-up)
                    (arm ?grasping-arm)
                    (grasp left-side)
                    (object ?perceived-bottle)))
       (park-arm ?grasping-arm)
       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))
          (perform (an action
                       (type going)
                       (target (a location 
                                  (pose ?nav-goal))))))
       ;; Setting the bottle down on the counter
       (let ((?drop-pose *final-object-destination*))
          (perform (an action
                       (type placing)
                       (arm ?grasping-arm)
                       (object ?perceived-bottle)
                       (target (a location 
                                  (pose ?drop-pose))))))
        (park-arm ?grasping-arm))))
 
 
 ;; failure handling: different looking directions - (move-bottle2 '((-2 -0.9 0.860) (0 0 0 1)))
 ;; ============================================================================================
 
 (defun move-bottle2 (bottle-spawn-pose)
   (spawn-object bottle-spawn-pose)
   (with-simulated-robot
     (let ((?navigation-goal *base-pose-near-table*))
       (cpl:par
         ;; Moving the robot near the table.
         (perform (an action
                      (type going)
                      (target (a location 
                                 (pose ?navigation-goal)))))
         (perform (a motion 
                     (type moving-torso)
                     (joint-angle 0.3)))
         (park-arms)))
     ;; Find and detect the bottle on the table. We use the new method here
     (let ((?perceived-bottle (find-object :bottle))
           (?grasping-arm :right))
       (perform (an action
                    (type picking-up)
                    (arm ?grasping-arm)
                    (grasp left-side)
                    (object ?perceived-bottle)))
       (park-arm ?grasping-arm)
       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))
         (perform (an action
                      (type going)
                      (target (a location 
                                 (pose ?nav-goal))))))
       ;; Setting the object down on the counter
       (let ((?drop-pose *final-object-destination*))
         (perform (an action
                      (type placing)
                      (arm ?grasping-arm)
                      (object ?perceived-bottle)
                      (target (a location 
                                 (pose ?drop-pose))))))
       (park-arm ?grasping-arm))))
 
 
 ;; failure handling: choice of grasping arm - (move-bottle3 '((-1.0 -0.75 0.860) (0 0 0 1)))
 ;; =========================================================================================
 
 (defun move-bottle3 (bottle-spawn-pose)
   (spawn-object bottle-spawn-pose)
   (with-simulated-robot
     (let ((?navigation-goal *base-pose-near-table*))
       (cpl:par
         ;; Moving the robot near the table.
         (perform (an action
                      (type going)
                      (target (a location 
                                 (pose ?navigation-goal)))))
         (perform (a motion
                     (type moving-torso) 
                     (joint-angle 0.3)))
         (park-arms)))
  
    ( let ((?perceived-bottle (find-object :bottle))
           (?grasping-arm :right))
       ;; We update the value of ?grasping-arm according to what the method used
       (setf ?grasping-arm (pick-up-object ?perceived-bottle ?grasping-arm))
       (park-arm ?grasping-arm)
       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))
         (perform (an action
                      (type going)
                      (target (a location 
                                 (pose ?nav-goal))))))
        ;; Setting the object down on the counter
       (let ((?drop-pose *final-object-destination*))
         (perform (an action
                      (type placing)
                      (arm ?grasping-arm)
                      (object ?perceived-bottle)
                      (target (a location 
                                 (pose ?drop-pose))))))
       (park-arm ?grasping-arm))))
 
 
 ;; left-front diagonal grasp - (move-bottle4 '((-1.6 -0.9 0.82) (0 0 0 1)))
 ;;=========================================================================
 
 (defun move-bottle4 (bottle-spawn-pose)
   (spawn-object bottle-spawn-pose)
   (with-simulated-robot
     (let ((?navigation-goal *base-pose-near-table*))
       (cpl:par
         ;; Moving the robot near the table.
         (perform (an action
                      (type going)
                      (target (a location 
                                 (pose ?navigation-goal)))))
         (perform (a motion 
                     (type moving-torso)
                     (joint-angle 0.3)))
         (park-arms)))
     ;; Looking towards the bottle before perceiving.
     (let ((?looking-direction *downward-look-coordinate*))
       (perform (an action
                    (type looking)
                    (target (a location 
                               (pose ?looking-direction))))))
     ;; Detect the bottle on the table.
     (let ((?grasping-arm :right)
           (?perceived-bottle (perform (an action
                                           (type detecting)
                                           (object (an object 
                                                       (type bottle)))))))
       ;; Pick up the bottle
       (perform (an action
                    (type picking-up)
                    (arm ?grasping-arm)
                    (grasp fl-diagonal) ; new left-front diagonal grasp
                    (object ?perceived-bottle)))
       (park-arm ?grasping-arm)
       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))
         (perform (an action
                      (type going)
                      (target (a location 
                                 (pose ?nav-goal))))))
       ;; Setting the bottle down on the counter
      (let ((?drop-pose *final-object-destination*))
         (perform (an action
                      (type placing)
                      (arm ?grasping-arm)
                      (object ?perceived-bottle)
                      (target (a location 
                                 (pose ?drop-pose))))))
       (park-arm ?grasping-arm))))
 
 
 ;; Solution to exercise 1 with static poses - (move-bottle-exercise-1 '((-1.0 0.75 0.860) (0 0 0 1)))
 ;; ==================================================================================================
 ;; Simple solution involves adding some hardcoded base poses to search the bottle
 ;; Then nesting the find object with another failure handling that iterates different hardcoded
 ;; base poses to find the object.
 
 (defun move-bottle-exercise-1 (bottle-spawn-pose)
    (spawn-object bottle-spawn-pose)
    (print "Please insert code for Exercise 1."))
 
 
 ;; Solution to exercise 2 with static poses - (move-bottle-exercise-2 '((-1.0 -0.75 0.860) (0 0 0 1)))
 ;; ===================================================================================================
 ;; Solution for choosing the arm based on the position of the robot 
 
 (defun move-bottle-exercise-2 (bottle-spawn-pose)
    (spawn-object bottle-spawn-pose)
    (print "Please insert code for Exercise 2."))
 
 
 ;; Solution to exercise 3 - (move-bottle-exercise-3 '((-2 -0.9 0.860) (0 0 0 1)))
 ;; ===============================================================================
 ;; Modified find-object which changes torso link during perception failure
 
 (defun move-bottle-exercise-3 (bottle-spawn-pose)
    (spawn-object bottle-spawn-pose)
    (print "Please insert code for Exercise 3."))
 
 
 ;; Solution to exercise 4 - (move-bottle-exercise-4 '((-1.6 -0.9 0.82) (0 0 0 1)))
 ;; ================================================================================
 ;; modelled on baseline move-bottle but using top grasp
 ;; 
 ;; NB: for the new top grasp to be recognized during a placing action you must add the following
 ;;
 ;; (<- (object-type-grasp :bottle :top))
 ;;
 ;; on line 84, i.e. as the first bottle grasp rule, in 
 ;;
 <!-- ;; ~/ workspace/ros/src/cram/cram_knowrob/cram_knowrob_pick_place/src/grasping.lisp -->
 ;; ~/ workspace/cram_tutorial/src/cram/cram_knowrob/cram_knowrob_pick_place/src/grasping.lisp
 ;;
 ;; It will not be recognized if you add it after the other bottle grasp rules
 ;;
 ;; It will work fine for a picking-up action without doing this but not for a placing action
 
 
 (defun move-bottle-exercise-4 (bottle-spawn-pose)
    (spawn-object bottle-spawn-pose)
    (print "Please insert code for Exercise 4."))

There are four versions of  <code>move-bottle()</code>:  <code>move-bottle2()</code>,  <code>move-bottle3()</code>,  and <code>move-bottle4()</code>.

These correspond to the four steps in the tutorial

# Simple Fetch and Place
# Recovery from Failures
# Handling More Failures 
# Defining a New Grasp (this comes after Exercise 3, in preparation for Exercise 4)

in which the <code>move-bottle()</code> functions are defined (step 1), revised (step 2), revised again (step 3).  Step 4, defining a new grasp, involves direct evaluation of an action designator of type picking-up with the new front-left-diagonal grasp.  For convenience, we wrap this in the <code>move-bottle4</code> function.

==Do the Tutorial==

We are now in a position to do the [http://cram-system.org/tutorials/demo/fetch_and_place Zero prerequisites demo tutorial: Simple fetch and place] but without having to provide all of the function definitions interactively. This means we can simply invoke the example commands, i.e. by evaluating the various example functions in REPL.  

===Environment Setup===
First, let's set up the environment in our terminal by calling the launch file.  This invokes <code>roscore</code> so there is no need to do it manually from a terminal.

 $ roslaunch cram_pick_place_tutorial world.launch

Reminder: you can simply copy and paste this command and the following commands.  Use <code>Ctrl Shift v</code> when pasting to a terminal. Use <code>Ctrl y</code> when pasting to Emacs.

===REPL Setup===
Now, in a different terminal, let's load the package in the REPL (in case you have forgotten, REPL stands for Read-Eval-Print Loop).

 $ roslisp_repl
 CL-USER> (ros-load:load-system "cram_pick_place_tutorial" :cram-pick-place-tutorial)
 CL-USER> (in-package :cram-pick-place-tutorial)

===Bullet World Initialization===
Finally, initialize everything.

 PP-TUT> (roslisp-utilities:startup-ros)

Note: this can take some time (a few minutes).

If everything is works as it should, the kitchen and PR2 robot should appear in the Bullet World window.
 
===Simple Fetch and Place Plan===
We are ready to do the four parts of the tutorial.  These four headings are the same as the headings in the [http://cram-system.org/tutorials/demo/fetch_and_place Zero prerequisites demo tutorial: Simple fetch and place]. 

These correspond to the four tutorials

# Simple Fetch and Place
# Recovery from Failures
# Handling More Failures 
# Defining a New Grasp (this comes after Exercise 3, in preparation for Exercise 4)

so do refer to these tutorials to understand what is going on.

====Simple Fetch and Place====
Run <code>move-bottle</code>

 PP-TUT> (move-bottle '((-1.6 -0.9 0.82) (0 0 0 1)))

Reminder: you can recall previous commands in Emacs by repeatedly pressing <code>Option p</code> (i.e. <code>M-p</code> in Emacs-speak).

====Recovery from Failures====
Run <code>move-bottle2</code>

 PP-TUT> (move-bottle2 '((-2 -0.9 0.860) (0 0 0 1)))

If you encounter an unexpected error (e.g. <code>MANIPULATION-POSE-UNREACHABLE</code>), reinitialize the Bullet world with <code>(init-projection)</code> and try again.

====Handling More Failures====
Run <code>move-bottle3</code>

 PP-TUT> (move-bottle3 '((-1.0 -0.75 0.860) (0 0 0 1)))
 PP-TUT> (move-bottle3 '((-0.9 -0.75 0.860) (0 0 0 1)))

====Defining a New Grasp====
Run <code>move-bottle4</code>

 PP-TUT> (move-bottle4 '((-1.6 -0.9 0.82) (0 0 0 1)))

===Exercises===
As mentioned above, there are four dummy functions to help you get started with exercises 1 - 4.  At the moment, they don't do anything but you can run them to check.

====Exercise 1====
Run <code>move-bottle-exercise-1</code>

 PP-TUT> (move-bottle-exercise-1 '((-1.0 0.75 0.860) (0 0 0 1)))

====Exercise 2====
Run <code>move-bottle-exercise-2</code>

 PP-TUT> (move-bottle-exercise-2 '((-1.0 -0.75 0.860) (0 0 0 1)))

====Exercise 3====
Run <code>move-bottle-exercise-3</code>

 PP-TUT> (move-bottle-exercise-3 '((-2 -0.9 0.860) (0 0 0 1)))

====Exercise 4====
Run <code>move-bottle-exercise-4</code>

 PP-TUT> (move-bottle-exercise-4 '((-1.6 -0.8 0.82) (0 0 0 1)))

NB: for the new top grasp to be recognized during a placing action you must add the following

 (<- (object-type-grasp :bottle :top))

on line 84, i.e. as the first bottle grasp rule, in 

<!-- <code>~/workspace/ros/src/cram/cram_knowrob/cram_knowrob_pick_place/src/grasping.lisp</code> -->
<code>~/workspace/cram_tutorial/src/cram/cram_knowrob/cram_knowrob_pick_place/src/grasping.lisp</code>

It will not be recognized if you add it after the other bottle grasp rules.

It will work fine for the picking-up action without doing this, but not for the placing action.

Note that when you do insert the new top grasp as suggested, Exercise 4 will work. However it causes the other exercises to fail. It appears that the pre-grasp and lift poses are extracted using the first rule in the list and these are not valid for the side grasps.