Difference between revisions of "Zero Prerequisites Demo Tutorial: Simple Fetch and Place"

From David Vernon's Wiki
Jump to: navigation, search
(Update pick-and-place.lisp)
(Update pick-and-place.lisp)
(30 intermediate revisions by the same user not shown)
Line 30: Line 30:
 
   
 
   
 
  (defparameter *downward-look-coordinate*
 
  (defparameter *downward-look-coordinate*
     ;;(make-pose "base_footprint" '((0.65335 0.076 0.758) (0 0 0 1))))
+
     (make-pose "base_footprint" '((0.65335 0.076 0.758) (0 0 0 1))))
    (make-pose "base_footprint" '((0.8 -0.3 0.758) (0 0 0 1))))
+
 
   
 
   
 
  (defparameter *base-pose-near-counter*
 
  (defparameter *base-pose-near-counter*
Line 37: Line 36:
 
   
 
   
 
  (defparameter *final-object-destination*
 
  (defparameter *final-object-destination*
   (make-pose "map" '((-0.8 2 0.9) (0 0 0 1))))
+
   (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*
 
  (defparameter *left-downward-look-coordinate*
Line 69: Line 68:
 
   :grasp-rot-matrix *diagonal-rotation*
 
   :grasp-rot-matrix *diagonal-rotation*
 
   :pregrasp-offsets `(,*bottle-pregrasp-xy-offset* ,*bottle-pregrasp-xy-offset* ,*lift-z-offset*)
 
   :pregrasp-offsets `(,*bottle-pregrasp-xy-offset* ,*bottle-pregrasp-xy-offset* ,*lift-z-offset*)
   :lift-offsets *lift-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
 
  ;; failure handling: different looking directions
 
  ;; ==============================================
 
  ;; ==============================================
Line 123: Line 123:
 
   
 
   
 
  (defun pick-up-object (?perceived-object ?grasping-arm)
 
  (defun pick-up-object (?perceived-object ?grasping-arm)
   (let* ((?possible-grasps '(:left-side :right-side :front :back))
+
   (let* ((?possible-grasps '(:left-side :right-side :front :back)) ;define all possible grasps
           (?grasp (first ?possible-grasps)))
+
           (?remaining-grasps (copy-list ?possible-grasps))           ;make a copy to work though when trying each grasp
    (setf ?possible-grasps (rest ?possible-grasps))
+
          (?grasp (first ?remaining-grasps)))                        ;this is the first one to try
 
+
   
    (cpl:with-retry-counters ((arm-change-retry 1))
+
      (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
+
          ;; Outer handle-failure handling arm change
          ;; Inner handle-failure handling grasp change
+
          (handle-failure object-unreachable
          ((handle-failure (or manipulation-pose-unreachable gripper-closed-completely)
+
            ((setf ?remaining-grasps (copy-list ?possible-grasps))  ;make sure to try all possible grasps for each arm
              ;; Try to perform the pick up
+
              (setf ?grasp (first ?remaining-grasps))                ;get the first grasp to try
              ((perform (an action
+
              (setf ?remaining-grasps (rest ?remaining-grasps))     ;update the remaining grasps to try
                            (type picking-up)
+
           
                            (arm ?grasping-arm)
+
            ;; Inner handle-failure handling grasp change
                            (grasp ?grasp)
+
            (handle-failure (or manipulation-pose-unreachable gripper-closed-completely)
                            (object ?perceived-object))))
+
                ;; Try to perform the pick up
              ;; When pick-up fails this block gets executed
+
                ((perform (an action
              (format t "Grasp failed! Error: ~a~%Grasp: ~a~%Arm: ~a~%"
+
                              (type picking-up)
                      e ?grasp ?grasping-arm)
+
                              (arm ?grasping-arm)
              ;; Checks if we have any possible grasps left.
+
                              (grasp ?grasp)
              ;; If yes, then the block nested to it gets executed, which will
+
                              (object ?perceived-object))))
              ;; set the grasp that is used to the new value and trigger retry
+
              (when (first ?possible-grasps)
+
                ;; When pick-up fails this block gets executed  
                (format t "Retyring! Trying to grasp from ~a using ~a arm"
+
                (format t "~%Grasping failed with ~a arm and ~a grasp~%" ?grasping-arm ?grasp)
                        ?grasp ?grasping-arm)
+
                ;;(format t "~%Error: ~a~%" e)                       ;uncomment to see the error message
                (setf ?grasp (first ?possible-grasps))
+
                (setf ?possible-grasps (rest ?possible-grasps))
+
                ;; Check if we have any remaining grasps left.
                (park-arms)
+
                ;; If yes, then the block nested to it gets executed, which will
                (cpl:retry))
+
                ;; set the grasp that is used to the new value and trigger 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
+
                (when (first ?remaining-grasps)                     ;if there is a grasp remaining
              ;; which will be caught by the outer handle-failure
+
                  (setf ?grasp (first ?remaining-grasps))           ;get it
              (print  "No more grasp retries left :(")
+
                  (setf ?remaining-grasps (rest ?remaining-grasps)) ;update the remaining grasps to try
              (cpl:fail 'object-unreachable)))
+
                  (format t "Retrying with ~a arm and ~a grasp~%" ?grasping-arm ?grasp)
 
+
                  (park-arms)
        ;; This is the failure management of the outer handle-failure call
+
                  (cpl:retry))
        ;; It changes the arm that is used to grasp
+
                ;; This will get executed when there are no more elements in the  
        (print "Manipulation failed!!")
+
                ;; ?possible-grasps list. We print the error message and throw a new error
        (print e)
+
                ;; which will be caught by the outer handle-failure
 
+
                (print  "No more grasp retries left :(")
        ;; Here we use the retry counter we defined. The value is decremented automatically
+
                (cpl:fail 'object-unreachable)))
        (cpl:do-retry arm-change-retry
+
          ;; if the current grasping arm is right set left, else set right
+
              ;; This is the failure management of the outer handle-failure call
          (setf ?grasping-arm (if (eq ?grasping-arm :right)  
+
              ;; It changes the arm that is used to grasp
                                  :left
+
              (format t "Manipulation failed with the ~a arm"?grasping-arm)
                                  :right))
+
              ;; (print e)                                           ;uncomment if you want to see the error
          (cpl:retry))
+
              ;; Here we use the retry counter we defined. The value is decremented automatically
        ;; When all retries are exhausted print the error message.
+
              (cpl:do-retry arm-change-retry
        (print "No more arm change retries left :("))))
+
                ;; if the current grasping arm is right set left, else set right
  ?grasping-arm)
+
                (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)))
 
  ;; baseline plan - (move-bottle '((-1.6 -0.9 0.82) (0 0 0 1)))
 
  ;; ===========================================================
 
  ;; ===========================================================
 
+
 
  (defun move-bottle (bottle-spawn-pose)
 
  (defun move-bottle (bottle-spawn-pose)
 
   (spawn-object bottle-spawn-pose)
 
   (spawn-object bottle-spawn-pose)
Line 413: Line 420:
 
==Do the Tutorial==
 
==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.
+
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.
  
===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.
 
 
 
===Environment Setup===
 
===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.
 
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.
Line 429: Line 427:
 
  $ roslaunch cram_pick_place_tutorial world.launch
 
  $ roslaunch cram_pick_place_tutorial world.launch
  
There is no need to do this if you've already done it for the Bullet World Demonstration check in the previous section.
+
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===
 
===REPL Setup===
Now, let's load the package in the REPL (in case you have forgotten, REPL stands for Read-Eval-Print Loop).
+
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
 
  $ roslisp_repl
Line 443: Line 441:
 
  PP-TUT> (roslisp-utilities:startup-ros)
 
  PP-TUT> (roslisp-utilities:startup-ros)
  
Note again: this can take some time (a few minutes).
+
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===
 
===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].  
 
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].  
Line 458: Line 458:
  
 
====Simple Fetch and Place====
 
====Simple Fetch and Place====
Run <code>(move-bottle)</code>
+
Run <code>move-bottle</code>
  
 
  PP-TUT> (move-bottle '((-1.6 -0.9 0.82) (0 0 0 1)))
 
  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====
 
====Recovery from Failures====
Run <code>(move-bottle2)</code>
+
Run <code>move-bottle2</code>
  
 
  PP-TUT> (move-bottle2 '((-2 -0.9 0.860) (0 0 0 1)))
 
  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====
 
====Handling More Failures====
Run <code>(move-bottle3)</code>
+
Run <code>move-bottle3</code>
  
 
  PP-TUT> (move-bottle3 '((-1.0 -0.75 0.860) (0 0 0 1)))
 
  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====
 
====Defining a New Grasp====
Run <code>(move-bottle4)</code>
+
Run <code>move-bottle4</code>
  
 
  PP-TUT> (move-bottle4 '((-1.6 -0.9 0.82) (0 0 0 1)))
 
  PP-TUT> (move-bottle4 '((-1.6 -0.9 0.82) (0 0 0 1)))
Line 481: Line 486:
  
 
====Exercise 1====
 
====Exercise 1====
Run <code>(move-bottle-exercise-1)</code>
+
Run <code>move-bottle-exercise-1</code>
  
 
  PP-TUT> (move-bottle-exercise-1 '((-1.0 0.75 0.860) (0 0 0 1)))
 
  PP-TUT> (move-bottle-exercise-1 '((-1.0 0.75 0.860) (0 0 0 1)))
  
 
====Exercise 2====
 
====Exercise 2====
Run <code>(move-bottle-exercise-2)</code>
+
Run <code>move-bottle-exercise-2</code>
  
 
  PP-TUT> (move-bottle-exercise-2 '((-1.0 -0.75 0.860) (0 0 0 1)))
 
  PP-TUT> (move-bottle-exercise-2 '((-1.0 -0.75 0.860) (0 0 0 1)))
  
 
====Exercise 3====
 
====Exercise 3====
Run <code>(move-bottle-exercise-3)</code>
+
Run <code>move-bottle-exercise-3</code>
  
 
  PP-TUT> (move-bottle-exercise-3 '((-2 -0.9 0.860) (0 0 0 1)))
 
  PP-TUT> (move-bottle-exercise-3 '((-2 -0.9 0.860) (0 0 0 1)))
  
 
====Exercise 4====
 
====Exercise 4====
Run <code>(move-bottle-exercise-4)</code>
+
Run <code>move-bottle-exercise-4</code>
  
  PP-TUT> (move-bottle-exercise-4 '((-1.6 -0.9 0.82) (0 0 0 1)))
+
  PP-TUT> (move-bottle-exercise-4 '((-1.6 -0.8 0.82) (0 0 0 1)))

Revision as of 05:30, 18 July 2021

This page provides a consolidated version of the code required for the 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 pick-and-place.lisp 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 move-bottle, one for each example: move-bottle, move-bottle2, and move-bottle3. 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, move-botte4, 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.

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 cover the material in the section "Simple Fetch and Place".


Update pick-and-place.lisp

First, let's copy the example code.

Move into the src directory:

$ cd workspace/ros/src/cram/cram_tutorials/cram_pick_place_tutorial/src

Edit pick-and-place.lisp

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

(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 move-bottle(): move-bottle2(), move-bottle3(), and move-bottle4().

These correspond to the four steps in the tutorial

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

in which the move-bottle() 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 move-bottle4 function.

Do the Tutorial

We are now in a position to do the 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 roscore 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 Ctrl Shift v when pasting to a terminal. Use Ctrl y 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 Zero prerequisites demo tutorial: Simple fetch and place.

These correspond to the four steps in the tutorial

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

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

Simple Fetch and Place

Run move-bottle

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 Option p (i.e. M-p in Emacs-speak).

Recovery from Failures

Run move-bottle2

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

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

Handling More Failures

Run move-bottle3

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 move-bottle4

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 move-bottle-exercise-1

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

Exercise 2

Run move-bottle-exercise-2

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

Exercise 3

Run move-bottle-exercise-3

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

Exercise 4

Run move-bottle-exercise-4

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