Creating a CRAM Package for the Simple Mobile Manipulation Plan

From David Vernon's Wiki
Jump to: navigation, search

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

  1. Constructing Plans
  2. Recovery from Failures
  3. 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)