Creating a CRAM Package for the Simple Mobile Manipulation Plan
In this exercise we will create a CRAM package for the Simple Mobile Manipulation Plan intermediate tutorial. You normally do this tutorial in an entirely interactive manner. The goal here is to mimic the way that you do the Beginner Tutorials, i.e. by creating a CRAM package and adding files with the Lisp code for the various tutorials. These beginner tutorials require you to progressively develop the functionality by writing and updating the Lisp code in several files. Here, however, we will have just one single file and we will only add code to it once. Since there are three steps in the Simple Mobile Manipulation Plan intermediate tutorial, we have three different variants of the two relevant functions. This will be explained in detail below.
The purpose of this exercise is to avoid having to provide all of the function definitions interactively so that we 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.
Since the mobile manipulation plan is itself based on the Bullet World Demonstration tutorial, after creating the new CRAM package we will first copy the Bullet World Demonstration files and then we will add a new Lisp file for the Mobile Manipulation Plan.
Ideally, you should have completed the all of the CRAM Beginner Tutorials and the Bullet World Demonstration tutorial before proceeding here but it's not absolutely necessary.
Creating the CRAM Package
Creating the ROS package
Just as with the beginner tutorial, first we need to create a ROS package that depends on cram_language.
In the src subdirectory of your ROS workspace execute the following command:
$ catkin_create_pkg cram_my_intermediate_tutorial cram_language
If you have followed the CRAM installation instructions faithfully, the src subdirectory will be ~/workspace/ros/src:
~/workspace/ros/src$ catkin_create_pkg cram_my_intermediate_tutorial cram_language
For the rest of the tutorial, we will leave out the ~/workspace/ros/src.
Copying the Bullet World Demonstration Files
Move the the cram_my_intermediate_tutorial directory:
$ cd cram_my_intermediate_tutorial
The catkin_create_pkg command above creates two files:
CMakeLists.txt package.xml
We would overwrite these when we copy the Bullet World Demonstration files so let's first take the precaution of changing their names, viz.
$ mv CMakeList.txt CMakeLists.txt.tmp $ mv package.xml package.txt.tmp
Now, copy the Bullet World Demonstration files:
$ cp -rf ~/workspace/ros/src/cram/cram_tutorials/cram_bullet_world_tutorial/* .
Customizing the Bullet World Demonstration Files
Now, let's customize the package so that it refers to cram_my_intermediate tutorial instead of cram_bullet_world_tutorial.
Rename cram-bullet-world-tutorial.asd to cram-my-intermediate-tutorial.asd
$ mv cram-bullet-world-tutorial.asd cram-my-intermediate-tutorial.asd
Edit CMakeLists.txt.
Change project(cram_bullet_world_tutorial) to project(cram_my_intermediate_tutorial).
Edit package.xml.
Change <name>cram_bullet_world_tutorial</name> to <name>cram_my_intermediate_tutorial</name>.
Change <description>Tutorial code for the bullet world.</description> to <description>Tutorial code for the simple mobile manipulation plan</description>.
Edit cram-my-intermediate-tutorial.asd.
Change (defsystem cram-bullet-world-tutorial to (defsystem cram-my-intermediate-tutorial.
Move into the src directory:
$ cd src
Edit package.lisp
Change (defpackage cram-bullet-world-tutorial to (defpackage cram-my-intermediate-tutorial.
Change (:nicknames #:btw-tutl) to (:nicknames #:smmp-tut).
Edit setup.lisp
Change (in-package :bwt-tut) to (in-package :smmp-tut).
Edit tutorial.lisp
Change (in-package :bwt-tut) to (in-package :smmp-tut).
Checking That It Still Works
At this point, we should just have a renamed working version of the Bullet World Demonstration tutorial.
Before adding the simple mobile manipulation plan Lisp code, let's make sure that everything still works by compiling and running the package (the following instructions are adapted from Bullet World Demonstration tutorial).
Build
First, we need to build the package
$ roscd; cd .. $ catkin_make
Environment Setup
Now, 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_my_intermediate_tutorial world.launch
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_my_intermediate_tutorial" :cram-my-intermediate-tutorial) CL-USER> (in-package :cram-my-intermediate-tutorial)
Bullet World Initialization
Finally, initialize everything.
SMMP-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.
Now, let's move on to adding the Lisp code for the Simple Mobile Manipulation Plan intermediate tutorial.
Adding the Simple Mobile Manipulation Plan
At this point, we need to add a new Lisp file simple-mobile-manipulation-plan.lisp with the code that is provided in the Simple Mobile Manipulation Plan intermediate tutorial.
Again, the purpose of this exercise is to avoid having to provide all of the function definitions interactively so that we can simply do the tutorial by invoking the example commands, i.e. by evaluating the various example forms in REPL (see next section).
We also need to add it to the cram-my-intermediate-tutorial.asd file.
Let's do that first.
Edit cram-my-intermediate-tutorial.asd (recall: it is in ~/workspace/ros/src/cram_my_intermediate_tutorial).
Add (:file "simple-mobile-manipulation-plan" :depends-on ("package")) so that the :componentspart looks like this.
:components
((:module "src"
:components
((:file "package")
(:file "setup" :depends-on ("package"))
(:file "tutorial" :depends-on ("package"))
(:file "simple-mobile-manipulation-plan" :depends-on ("package"))
))))
Now, change to the src directory. Edit (and thereby create) simple-mobile-manipulation-plan.lisp and add the following code.
(in-package :smmp-tut)
(defparameter *final-object-destination*
(cl-transforms-stamped:make-pose-stamped
"map" 0.0
(cl-transforms:make-3d-vector -0.8 2 0.9)
(cl-transforms:make-identity-rotation)))
(defparameter *base-pose-near-table*
(cl-transforms-stamped:make-pose-stamped
"map" 0.0
(cl-transforms:make-3d-vector -1.447d0 -0.150d0 0.0d0)
(cl-transforms:axis-angle->quaternion (cl-transforms:make-3d-vector 0 0 1) (/ pi -2))))
(defparameter *downward-look-coordinate*
(cl-transforms-stamped:make-pose-stamped
"base_footprint" 0.0
(cl-transforms:make-3d-vector 0.65335d0 0.076d0 0.758d0)
(cl-transforms:make-identity-rotation)))
(defparameter *base-pose-near-counter*
(cl-transforms-stamped:make-pose-stamped
"base_footprint" 0.0
(cl-transforms:make-3d-vector -0.150d0 2.0d0 0.0d0)
(cl-transforms:make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))
(defparameter *left-downward-look-coordinate*
(cl-transforms-stamped:make-pose-stamped
"base_footprint" 0.0
(cl-transforms:make-3d-vector 0.65335d0 0.76d0 0.758d0)
(cl-transforms:make-identity-rotation)))
(defparameter *right-downward-look-coordinate*
(cl-transforms-stamped:make-pose-stamped
"base_footprint" 0.0
(cl-transforms:make-3d-vector 0.65335d0 -0.76d0 0.758d0)
(cl-transforms:make-identity-rotation)))
;;spawn-bottle to be used in the first exercise, i.e. successfully picking up the bottle
;;--------------------------------------------------------------------------------------
(defun spawn-bottle ()
(unless (assoc :bottle btr::*mesh-files*)
(add-objects-to-mesh-list))
(btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-1.6 -0.9 0.82) (0 0 0 1)))
(btr:simulate btr:*current-bullet-world* 10))
;;spawn-bottle which can be used in the first exercise on recovering from failures
;;--------------------------------------------------------------------------------
(defun spawn-bottle2 ()
(unless (assoc :bottle btr::*mesh-files*)
(add-objects-to-mesh-list))
(btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-2 -0.9 0.860) (0 0 0 1)))
(btr:simulate btr:*current-bullet-world* 10))
;;spawn-bottle which can be used in the second exercise on recovering from failures
;;---------------------------------------------------------------------------------
(defun spawn-bottle3 ()
(unless (assoc :bottle btr::*mesh-files*)
(add-objects-to-mesh-list))
(btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-1.1 -0.75 0.860) (0 0 0 1)))
(btr:simulate btr:*current-bullet-world* 10))
;;move-bottle which can be used in the first exercise successfully picking up a bottle
;;------------------------------------------------------------------------------------
(defun move-bottle ()
(spawn-bottle)
(pr2-proj:with-simulated-robot
(let ((?navigation-goal *base-pose-near-table*))
(cpl:par
(exe:perform (desig:a motion
(type moving-torso)
(joint-angle 0.3)))
(pp-plans::park-arms)
;; Moving the robot near the table.
(exe:perform (desig:a motion
(type going)
(target (desig:a location
(pose ?navigation-goal)))))))
;; Looking towards the bottle before perceiving.
(let ((?looking-direction *downward-look-coordinate*))
(exe:perform (desig:a motion
(type looking)
(target (desig:a location
(pose ?looking-direction))))))
;; Detect the bottle on the table.
(let ((?grasping-arm :right)
(?perceived-bottle (exe:perform (desig:a motion
(type detecting)
(object (desig:an object
(type :bottle)))))))
;; Pick up the bottle
(exe:perform (desig:an action
(type picking-up)
(arm ?grasping-arm)
(grasp left-side)
(object ?perceived-bottle)))
(pp-plans::park-arms :arm ?grasping-arm)
;; Moving the robot near the counter.
(let ((?nav-goal *base-pose-near-counter*))
(exe:perform (desig:a motion
(type going)
(target (desig:a location
(pose ?nav-goal))))))
(coe:on-event (make-instance 'cpoe:robot-state-changed))
;; Setting the bottle down on the counter
(let ((?drop-pose *final-object-destination*))
(exe:perform (desig:an action
(type placing)
(arm ?grasping-arm)
(object ?perceived-bottle)
(target (desig:a location
(pose ?drop-pose))))))
(pp-plans::park-arms :arm ?grasping-arm))))
;;move-bottle which can be used in the first exercise on recovering from failures
;;--------------------------------------------------------------------------------
(defun move-bottle2 ()
(spawn-bottle2) ;NB spawning second bottle
(pr2-proj:with-simulated-robot
(let ((?navigation-goal *base-pose-near-table*))
(cpl:par
(exe:perform (desig:a motion
(type moving-torso)
(joint-angle 0.3)))
(pp-plans::park-arms)
;; Moving the robot near the table.
(exe:perform (desig:a motion
(type going)
(target (desig:a location
(pose ?navigation-goal)))))))
;; Find and detect the bottle on the table.
(multiple-value-bind (?perceived-bottle ?grasping-arm)
(find-object :bottle)
(exe:perform (desig:an action
(type picking-up)
(arm ?grasping-arm)
(grasp left-side)
(object ?perceived-bottle)))
(pp-plans::park-arms :arm ?grasping-arm)
;; Moving the robot near the counter.
(let ((?nav-goal *base-pose-near-counter*))
(exe:perform (desig:a motion
(type going)
(target (desig:a location
(pose ?nav-goal))))))
(coe:on-event (make-instance 'cpoe:robot-state-changed))
;; Setting the object down on the counter
(let ((?drop-pose *final-object-destination*))
(exe:perform (desig:an action
(type placing)
(arm ?grasping-arm)
(object ?perceived-bottle)
(target (desig:a location
(pose ?drop-pose))))))
(pp-plans::park-arms :arm ?grasping-arm))))
;;move-bottle which can be used in the second exercise on recovering from failures
;;--------------------------------------------------------------------------------
(defun move-bottle3 ()
(spawn-bottle3) ;NB spawning third bottle
(pr2-proj:with-simulated-robot
(let ((?navigation-goal *base-pose-near-table*))
(cpl:par
(exe:perform (desig:a motion
(type moving-torso)
(joint-angle 0.3)))
(pp-plans::park-arms)
;; Moving the robot near the table.
(exe:perform (desig:a motion
(type going)
(target (desig:a location
(pose ?navigation-goal)))))))
(multiple-value-bind (?perceived-bottle ?grasping-arm)
(find-object :bottle)
(setf ?grasping-arm (pick-up-object ?perceived-bottle :bottle ?grasping-arm))
(pp-plans::park-arms :arm ?grasping-arm)
;; Moving the robot near the counter.
(let ((?nav-goal *base-pose-near-counter*))
(exe:perform (desig:a motion
(type going)
(target (desig:a location
(pose ?nav-goal))))))
(coe:on-event (make-instance 'cpoe:robot-state-changed))
;; Setting the object down on the counter
(let ((?drop-pose *final-object-destination*))
(exe:perform (desig:an action
(type placing)
(arm ?grasping-arm)
(object ?perceived-bottle)
(target (desig:a location
(pose ?drop-pose))))))
(pp-plans::park-arms :arm ?grasping-arm))))
(defun get-preferred-arm-for-direction (direction-looked)
(let ((preferred-arm :RIGHT))
(when (eq direction-looked *left-downward-look-coordinate*)
(setf preferred-arm :LEFT))
preferred-arm))
(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 (cdr possible-look-directions))
(exe:perform (desig:a motion
(type looking)
(target (desig:a location
(pose ?looking-direction)))))
(cpl:with-failure-handling
((cram-common-failures:perception-object-not-found (e)
;; Try different look directions until there is none left.
(when possible-look-directions
(roslisp:ros-warn (perception-failure) "~a~%Turning head." e)
(exe:perform (desig:a motion
(type looking)
(direction forward)))
(setf ?looking-direction (first possible-look-directions))
(setf possible-look-directions (cdr possible-look-directions))
(exe:perform (desig:a motion
(type looking)
(target (desig:a location
(pose ?looking-direction)))))
(cpl:retry))
(cpl:fail 'common-fail:looking-high-level-failure)))
(let ((?perceived-bottle
(exe:perform (desig:a motion
(type detecting)
(object (desig:an object
(type ?object-type)))))))
(values ?perceived-bottle (get-preferred-arm-for-direction ?looking-direction))))))
(defun pick-up-object (?perceived-object ?object-type ?grasping-arm)
(let ((?possible-arms '(:right :left)))
;;Retry by changing the arm
(cpl:with-retry-counters ((arm-change-retry 1))
(cpl:with-failure-handling
((common-fail:object-unreachable (e)
(roslisp:ros-warn (arm-failure) "Manipulation failed: ~a~%" e)
(cpl:do-retry arm-change-retry
(setf ?grasping-arm (car (remove ?grasping-arm ?possible-arms)))
(cpl:retry))
(roslisp:ros-warn (arm-failures) "No more retries left")))
;; Retry by changing the grasp
(let* ((?possible-grasp
(cram-object-interfaces:get-object-type-grasps ?object-type nil nil nil ?grasping-arm))
(?grasp (cut:lazy-car ?possible-grasp)))
(cpl:with-retry-counters ((grasp-retries 3))
(cpl:with-failure-handling
(((or cram-common-failures:manipulation-pose-unreachable
cram-common-failures:gripper-closed-completely) (e)
(roslisp:ros-warn (grasp-failure)
"~a~%Failed to grasp from ~a using ~a arm "
e ?grasp ?grasping-arm)
(cpl:do-retry grasp-retries
(when (cut:lazy-car ?possible-grasp)
(roslisp:ros-info (trying-new-grasp)
"Trying to grasp from ~a using ~a arm"
?grasp ?grasping-arm)
(setf ?possible-grasp (cut:lazy-cdr ?possible-grasp))
(pp-plans::park-arms)
(setf ?grasp (cut:lazy-car ?possible-grasp))
(cpl:retry)))
(roslisp:ros-warn (grasp-failures) "No more retries left")
(cpl:fail 'common-fail:object-unreachable)))
;; Perform the grasp
(exe:perform (desig:an action
(type picking-up)
(arm ?grasping-arm)
(grasp ?grasp)
(object ?perceived-object)))))))))
?grasping-arm)
Note that there are three versions of spawn-bottle(): spawn-bottle(), spawn-bottle2(), and spawn-bottle3()
Similarly, there are three versions of move-bottle(): move-bottle(), move-bottle2(), and move-bottle3().
These correspond to the three exercises in the tutorial
- Constructing Plans
- Recovery from Failures
- Expanding Failure Management Capabilities
in which the spawn-bottle() and move-bottle() functions are defined (Exercise 1), revised (Exercise 2), and revised again (Exercise 3).
Doing the Tutorial
We are now in a position to do the Simple Mobile Manipulation Plan intermediate tutorial 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 forms 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_my_intermediate_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 (re-)load the package in the REPL
$ roslisp_repl CL-USER> (ros-load:load-system "cram_my_intermediate_tutorial" :cram-my-intermediate-tutorial) CL-USER> (in-package :cram-my-intermediate-tutorial)
Bullet World Initialization
Finally, initialize everything.
SMMP-TUT> (roslisp-utilities:startup-ros)
Note again: this can take some time (a few minutes).
Simple Mobile Manipulation Plan
Finally, we are ready to do the three parts of the tutorial. These three headings are the same as the headings in the Simple Mobile Manipulation Plan tutorial
These correspond to the three exercises in the tutorial
- Constructing Plans
- Recovery from Failures
- Expanding Failure Management Capabilities
so do refer to these sections to understand what is going on.
Constructing Plans
Run (move-bottle)
SMMP-TUT> (move-bottle)
Recovery from Failures
Run (move-bottle2)
SMMP-TUT> (move-bottle2)
Expanding Failure Management Capabilities
Run (move-bottle3)
SMMP-TUT> (move-bottle3)
