Zero Prerequisites Demo Tutorial: Simple Fetch and Place
From David Vernon's Wiki
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".
Contents
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.9) (0 0 0 1))))
(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*)
: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
- 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 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.
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
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
There is no need to do this if you've already done it for the Bullet World Demonstration check in the previous section.
REPL Setup
Now, 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 again: this can take some time (a few minutes).
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
- 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 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)))
Recovery from Failures
Run move-bottle2
PP-TUT> (move-bottle2 '((-2 -0.9 0.860) (0 0 0 1)))
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)))
