Simple Mobile Manipulation Plan

This tutorial demonstrates how to write a simple mobile manipulation plan and error handling and recovery behaviors for it. In this tutorial we will be using the Bullet world as a lightweight simulator. But as plans in CRAM are independent of the hardware platform, the same plan that can be executed in simulation can just as well be executed on the real robot, if we stick to motion and action designators defined in CRAM.

This tutorial assumes that you have a basic understanding of plans, and have undergone the Bullet World tutorial, and are now familiar with manipulating various elements in the Bullet world simulation.

As the system we will be working with will get quite complex things might not go as expected. If you get into trouble while following this tutorial feel free to consult the Support page or contact the person who last edited this tutorial, you can find the username in the footer.

If you followed the installation instructions for the full CRAM on the Installation page, you should be ready to go.

For this tutorial we need to set up the environment similar to how it is done for the real system: we will need an occupancy map for navigation, an IK solver for manipulation, a map of the static parts of the environment with the furniture etc.

First, let's set up the environment in our terminal by calling the launch file. It already brings up a roscore from within:

$ roslaunch cram_bullet_world_tutorial world.launch

Now, let's load the package in the REPL (start the REPL with $ roslisp_repl):

CL-USER> (ros-load:load-system "cram_bullet_world_tutorial" :cram-bullet-world-tutorial)
CL-USER> (in-package :cram-bullet-world-tutorial)
BTW-TUT> (roslisp-utilities:startup-ros)

You'll have your bullet world launched with the PR2 in the kitchen. Now you're good to go.

Before we proceed further let's look at some of the designators that are supported by the robot in the bullet world (and this works in the real world robot as well). To refresh your memory on designators, visit the tutorial Creating motion designators for the TurtleSim. Here are some of the supported motion and action designators:

• Motion Designators - They describe low-level atomic motions that the robot can execute.

  •  (desig:a motion (type moving-torso) (joint-angle ?angle-val))

    is a motion for moving the torso of the robot and expects a key joint-angle with a value of the required joint angle.

  •  (desig:a motion (type going) (target ?loc-desig)) 

    is a motion that moves the body of the robot and expects a key target, whose value should be of type location-designator.

  •  (desig:a motion (type looking) (target ?loc-desig))

    is a motion that moves the head of the robot to look at the specfied location. It expects a key target with value of type location designator.

  •  (desig:a motion (type detecting) (object ?obj-desig)) 

    is a a motion that perceives and detects the specified object. It expects a key object with a value of type object-designator. An example is

          (desig:a motion (type detecting) (desig:an object (type bowl))) 

    . This motion perceives and tries to find an object of type bowl in the field of view where the camera of the robot is pointed to. The resultant of this is also an object designator, but it has much more information. A sample is given below

        #<A OBJECT
        (TYPE BOWL)
        (NAME BOWL-1)
        (POSE ((:POSE
                #<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
       FRAME-ID: "base_footprint", STAMP: 1.564755796743144d9
       #<3D-VECTOR (0.7672472770564228d0 -0.35741216776250767d0 0.888642116546631d0)>
       #<QUATERNION (-0.005783746257358676d0 -0.0013544535828327665d0 -0.26923602469246516d0 0.9630558655492675d0)>>)
               (:TRANSFORM
                #<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED 
       FRAME-ID: "base_footprint", CHILD-FRAME-ID: "bowl_1", STAMP: 1.564755796743144d9
       #<3D-VECTOR (0.7672472770564228d0 -0.35741216776250767d0 0.888642116546631d0)>
       #<QUATERNION (-0.005783746257358676d0 -0.0013544535828327665d0 -0.26923602469246516d0 0.9630558655492675d0)>>)
               (:POSE-IN-MAP
                #<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
       FRAME-ID: "map", STAMP: 1.564755796743144d9
       #<3D-VECTOR (1.5255411783854167d0 0.761638069152832d0 0.8886421203613282d0)>
       #<QUATERNION (-0.005410938989371061d0 -0.0024511234369128942d0 -0.07722260057926178d0 0.9969961643218994d0)>>)
               (:TRANSFORM-IN-MAP
                #<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED 
       FRAME-ID: "map", CHILD-FRAME-ID: "bowl_1", STAMP: 1.564755796743144d9
       #<3D-VECTOR (1.5255411783854167d0 0.761638069152832d0 0.8886421203613282d0)>
       #<QUATERNION (-0.005410938989371061d0 -0.0024511234369128942d0 -0.07722260057926178d0 0.9969961643218994d0)>>)))> 

    Note that this resulting object designator has the name of the object along with the coordinate of the object w.r.t the robot coordinate frame and other useful coordinate transformations which can later be used to calculate the pose of the end effector for grasping.

• Action Designators - These describe the high-level actions which may consist of multiple calls to different low-level motion to carry out a small plan.

  •  (desig:an action (type picking-up) (arm ?grasp-arm) (grasp ?grasp-pose-identifier) (object ?obj-desig)) 

    is an action that picks an object with the specified arm and grasp pose. It expects an object key with value of type object-designator, an arm with the value of type keyword for the arm to choose, and a grasp key whose value specifies a keyword which is an identifier of the grasp pose to pick the object with.

  •  (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ?loc-desig)) 

    is an action that places the object specified to the target. It expects an optional key arm with the keyword of the arm to use, an object of type object-designator and a target key with the value of type location-designator.

We will use these designators throughout this tutorial.

The goal of this chapter is to write a plan for a simple task: pick and place an object from one position to another position in the world. Let's go ahead and dive into the code:

Any time you want a clean environment, you can run:

BTW-TUT> (init-projection)

Now, Let's define a method to spawn a bottle into the world and apply the physics using the Bullet physics engine and simulate the world for 10 seconds, and run it:

BTW-TUT>
(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 -1 0.82) (0 0 0 1)))
  (btr:simulate btr:*current-bullet-world* 10))
BTW-TUT> (spawn-bottle)

You should now see a red bottle on the table. To make our lives easier, let's define some parameters. The *final-object-destination* is the place where the pr2 has to place the red bottle in the end. The *base-pose-near-table* is the pose, where pr2 should stand near the table, when he approaches the bottle and *base-pose-near-counter* is the pose to access the counter to place the bottle.

BTW-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-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))
 
(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)))

Let's note down the steps that would involve picking the bottle from the table and placing it on the counter.

• Move the robot near the table.
• Move the arms out of the perception range, so that the robot can see the bottle without obstruction.
• Look towards the object area.
• Detect the object that has to be picked.
• Pick up the object and once again, keep the arms away from the front.
• Move the robot near the counter.
• Place the bottle on the destination.

Implementing all the above steps in code, will look something like as shown below:

(defun move-bottle ()
  (spawn-bottle)
  (urdf-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:an action
                              (type going)
                              (target (desig:a location 
                                               (pose ?navigation-goal)))))))
    ;; Looking towards the bottle before perceiving.
    (let ((?looking-direction *downward-look-coordinate*))
      (exe:perform (desig:an action 
                            (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:an action
                              (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))))

Note that the plan is nested under urdf-proj:with-simulated-robot indicating that all the methods resolved by the designators is being called for the projection environment (You can see these low level methods being called under the cram_urdf_projections package under low-level.lisp). If pr2-pms:with-real-robot is used to replace this, the functions from the real robot ROS interfaces will be called. Also note that it is possible to execute actions which are independent in parallel (In this case, moving the robot torso up and moving the robot base are independent actions done in parallel)

Now run (move-bottle) and you will see that the robot successfully picks the bottle and places it on the counter.

The previous example worked perfectly, because we knew the exact coordinates to look for the bottle. This is hardly true for the real life scenario, especially since we are dealing with a kitchen environment, far from being precise compared to a factory floor. What would happen if the bottle was moved a little bit to the right of its previous spawn position? For this, let's redefine our spawn-bottle

BTW-TUT>
(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 '((-2 -1 0.82) (0 0 0 1)))
  (btr:simulate btr:*current-bullet-world* 10))

Now try running (move-bottle) again. And the output will look like this.

BTW-TUT> (move-bottle)
; Evaluation aborted on #<CRAM-COMMON-FAILURES:PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>.

Clearly the robot cannot find the object anymore because even though the robot is near the bottle, it is out of the field of vision of the robot due to the predefined base and object pose in our code. Let's try fixing this issue.

To understand the syntax and get a refresher on failure handling, you can refer to Failure Handling. We're going to add some code into fixing perception in our case, so that the robot would still be able to find the bottle. Let's list down a plan of action for this.

1. Tilt the head of the robot downwards
2. Try to detect the bottle
3. If,
   • successful in finding the bottle - continue with the rest of the code.
   • failed to find the bottle - turn the head to a different configuration (eg., left/right) and try detecting again.
4. When all possible directions fail, error out.

Let's define some additional parameters to aid us:

BTW-TUT>
(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)))
 

We defined two coordinates *left-downward-look-coordinate* and *right-downward-look-coordinate* with respective to our robot base footprint as alternative directions to look for when downward look fails.

Now we define a method find-object which encapsulates our plan with failure handling :

(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:an action
                          (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:an action
                                   (type looking) 
                                   (direction forward)))
             (setf ?looking-direction (first possible-look-directions))
             (setf possible-look-directions (cdr possible-look-directions))
             (exe:perform (desig:an action 
                                   (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))))))
 

Let's see what this method does. The with-failure-handling clause here deals with cram-common-failures:perception-object-not-found which if you remember, was the error raised when the robot couldn't find the bottle. So instead of only looking downwards, the code will now iterate between downwards, left and right, and only upon a failure in all these, will an error be bubbled up - thus increasing our effective field of view. Also note that the find-object suggests an arm to use, depending on the direction it looked. Let us also update our move-bottle to use this method.

 (defun move-bottle ()
  (spawn-bottle)
  (urdf-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:an action
                              (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:an action
                              (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))))

Run (move-bottle) again, and this time, you'll find that the robot succeeds in transporting the bottle.

Everything is good so far, but let's call this a lucky coincidence. For the robot, knowing which arm to use to pick up the bottle is not always enough. There are many positions with which we can grasp objects - from the object's front, back, left, right, etc. One might think that since the bottle is a rotationally symmetric object, it doesn't matter which side you approach from. But consider the bottle as an object model, which has a specific front, back and sides according to its orientation with respect to the bullet world. In which case, the side with which the arm approaches the bottle greatly matters, accounting for the configuration of the joints the robot arm will make while trying to grasp - some poses may be unachievable, while others may result in the collision of the arm with the table.

Let's try to visualize this issue, by spawning the bottle in yet another position and changing the grasp position slightly:

(defparameter *base-pose-near-table*
  (cl-transforms-stamped:make-pose-stamped
   "map" 0.0
   (cl-transforms:make-3d-vector -2.447d0 -0.150d0 0.0d0)
   (cl-transforms:axis-angle->quaternion (cl-transforms:make-3d-vector 0 0 1) (/ pi -2))))
 
 (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.8 -1 0.860) (0 0 0 1)))
  (btr:simulate btr:*current-bullet-world* 10))

Now run move-bottle once more, and and the output should look like this [Some messages that would come up are suppressed here for readability.]

BTW-TUT> (move-bottle)
 
[(PICK-PLACE PICK-UP) INFO] 1649925566.714: Looking
[(PICK-PLACE PICK-UP) INFO] 1649925566.758: Opening gripper and reaching
[(URDF-PROJ MOVE-TCP) ERROR] 1649925567.115: #<POSE-STAMPED 
   FRAME-ID: "torso_lift_link", STAMP: 0.0
   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.1556749380156398d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
Failing.
[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1649925567.119: #<POSE-STAMPED 
   FRAME-ID: "torso_lift_link", STAMP: 0.0
   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.1556749380156398d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
Ignoring.
[(PERFORM MOTION) INFO] 1649925567.125: 
#<A MOTION
    (TYPE MOVING-TCP)
    (RIGHT-POSE #<POSE-STAMPED 
   FRAME-ID: "map", STAMP: 0.0
   #<3D-VECTOR (-1.7999999507109004d0 -0.8699999493007575d0 0.8650000342205167d0)>
   #<QUATERNION (0.0d0 0.0d0 -0.7071067690849304d0 0.7071067690849304d0)>>)
    (COLLISION-MODE ALLOW-ALL)
    (MOVE-BASE T)>
[(URDF-PROJ MOVE-TCP) ERROR] 1649925567.483: #<POSE-STAMPED 
   FRAME-ID: "torso_lift_link", STAMP: 0.0
   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.22567493831366303d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
Failing.
[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1649925567.484: #<POSE-STAMPED 
   FRAME-ID: "torso_lift_link", STAMP: 0.0
   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.22567493831366303d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
Failing.
[(PP-PLANS PICK-UP) WARN] 1649925567.484: Manipulation messed up: #<POSE-STAMPED 
   FRAME-ID: "torso_lift_link", STAMP: 0.0
   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.22567493831366303d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
Ignoring.
[(PICK-PLACE PICK-UP) INFO] 1649925567.484: Grasping
[(PICK-PLACE PICK-UP) INFO] 1649925567.845: Gripping
[(PICK-AND-PLACE GRIP) WARN] 1649925567.875: There was no object to grip
Retrying
[(PICK-AND-PLACE GRIP) WARN] 1649925567.888: No retries left. Propagating up.
; Evaluation aborted on #<CRAM-COMMON-FAILURES:GRIPPER-CLOSED-COMPLETELY {10058EA443}>.

The robot has failed to grasp again, even though the bottle is well within perception and grasping range.

So what went wrong? If you go back to definition of get-preferred-arm-for-direction:

(defun get-preferred-arm-for-direction (direction-looked)
  (let ((preferred-arm :RIGHT))
    ;; Always prefers right arm unless looking at the left side
    (when (eq direction-looked *left-downward-look-coordinate*)
      (setf preferred-arm :LEFT))
    preferred-arm))

And the part where we pick up the object:

        (exe:perform (desig:an action
                               (type picking-up)
                               (arm ?grasping-arm)
                               (grasp left-side)
                               (object ?perceived-bottle)))

We see that the robot defaults the right arm when the object is in the center and will always try to grasp the left side of the bottle, even when the object is slightly favoring the left side under it (Refer the figure below).

Note: We have been using left-side here a lot. In the context of (grasp left-side), it refers to the left-side of the bottle. But what defines the “left-side”?

Before we understand it, we have to acknowledge that every object spawned in the bullet world has its own coordinate axes - even rotationally symmetric objects like the bottle here. And the definition for left-side is the side of the -ve Y-axis in the bottle coordinate frame. Similarly right-side is +ve Y-axis, front is denoted by +ve X-axis and the back by -ve X-axis. With this the “left-side” grasp can change based on how the object is oriented in the world. But it remains the w.r.t the object coordinate frame.

The file at <your workspace>/cram/cram_common/cram_object_knowledge/src/household.lisp holds some definitions for grasps supported by the household objects supporte in CRAM.

Once again, let's formulate a strategy like the previous case here. The plan we are going for will look something like this:

1. Choose the favored arm.
2. Get all possible grasp poses for the given type of the object and the arm.
3. Choose one grasp from the possible grasp list.
4. Try to grasp the object.
5. If,
   • Grasp succeeds - Continue with the rest of the code
   • Grasp Fails - Choose a different grasp pose from the list.
6. When no more possible grasp poses, try changing the arm used to grasp Once and try resuming from Step (2)
7. When attempted with all arms and grasps, error out.

Let's encapsulate all this in a method called pick-up-object:

 (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-manipulation-interfaces:get-action-grasps ?object-type ?grasping-arm ?perceived-object))
                 (?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)

With this, the pick-up-object can now iterate through all possible grasp configurations stored in ?possible-grasp. The cram-object-interaces:get-object-type-grasps is a useful method which will give these values as a lazy list, provided the grasping arm and the object type.

There are two nested failure-handling clauses here. The inner failure-handling part will take care of cram-common-failures:manipulation-pose-unreachable and cram-common-failures:gripper-closed-completely - both of which are errors which can occur during a failed grasp. The recovery method will iterate through all possible grasp configurations for a given arm. Upon failure of this, the outer failure handling clause will take care of the common-fail:object-unreachable error thrown by the inner part and change the arm it's trying to grasp the object with and then retry the entire procedure again.

And only upon the failure of all these will the error be bubbled up. The method also returns the grasping arm it used to pick the object up so that the rest of the code is aware of the arm in which the bottle rests. Also, note that the arm we decided on earlier in find-object now acts as a bias to reduce the number of searches we do to find a successful configuration. We have also written appropriate warning statements to be informed about the actions the robot is taking.

Also let's redefine move-bottle again to include these:

(defun move-bottle ()
  (spawn-bottle)
  (urdf-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:an action
                              (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:an action
                              (type going)
                              (target (desig:a location 
                                               (pose ?nav-goal))))))
 
      (coe:on-event (make-instance 'cpoe:robot-state-changed))
      (exe:perform (desig:a motion 
                              (type moving-torso)
                              (joint-angle 0.3)))
      ;; 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))))

You should see a result that looks like the one below. [Some messages that would come up are suppressed here for readability.]

BTW-TUT> (init-projection)
BTW-TUT> (move-bottle)
[(PERFORM ACTION) INFO] 1649927731.920: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved.
[(PICK-PLACE PICK-UP) INFO] 1649927732.109: Looking
[(PICK-PLACE PICK-UP) INFO] 1649927732.148: Opening gripper and reaching
[(PICK-PLACE PICK-UP) INFO] 1649927732.789: Grasping
[(PICK-PLACE PICK-UP) INFO] 1649927733.055: Gripping
[(PICK-PLACE GRIP) INFO] 1649927733.089: Assert grasp into knowledge base
[(PICK-PLACE PICK-UP) INFO] 1649927733.097: Lifting
[(PICK-PLACE PLACE) INFO] 1649927733.610: Parking
[(PERFORM ACTION) INFO] 1649927733.834: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved.
[(PICK-PLACE PLACE) INFO] 1649927733.945: Looking
[(PICK-PLACE PLACE) INFO] 1649927734.003: Reaching
[(PICK-PLACE PLACE) INFO] 1649927734.430: Putting
[(PICK-PLACE PLACE) INFO] 1649927734.677: Opening gripper
[(PICK-PLACE PLACE) INFO] 1649927734.718: Retract grasp in knowledge base
[(PICK-PLACE PLACE) INFO] 1649927734.807: Updating object location in knowledge base
[(PICK-PLACE PLACE) INFO] 1649927734.807: Retracting
[(PICK-PLACE PLACE) INFO] 1649927735.214: Parking
[(PERFORM ACTION) INFO] 1649927735.398: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved.

The robot has once again succeeded in grasping the object.

You can now try spawning the bottle in different points on the table and observing how the robot resolves the arm and grasp for it. Subsequently, you will notice that our plans will work as long as the bottle is within reach of the robot. You could try implementing the adjustment of the robot's position to facilitate the fetch.

Instead of a hardcoded pose for the base of the robot, you now would now need to obtain a list of poses where the robot will be able to grasp the bottle from. This is exactly where location costmaps come into play. CRAM provides an easy way to handle these where the user does not have to bother with hardcoded poses. A way to obtain this would be through the following designator

(a location 
   (reachable-for pr2)
   (object (an object
               (type bottle))))

Referencing this will give you locations where our robot PR2 can successfully reach an object of type bottle. Note that the object designator used here is pretty generic and will apply to all the bottles in the robot's belief state. To make it more specific we can provide the name and the location designator where the bottle is expected to be in. Eg:

;; This is the location designator used to reference locations where the pr2 should be to reach an object 
;; named bottle-1 of type bottle which is situated on the counter-top named sink-area-surface
(a location
   (reachable-for pr2)
   (object (an object
               (type bottle)
               (name bottle-1)
               (location (a location 
                            (on (an object 
                                    (type counter-top)
                                    (urdf-name sink-area-surface)
                                    (part-of kitchen))))))))

You can see that the designator is not that intimidating as it looks, but provides a structured way to reference objects and locations. You can use these to write plans without having to worry about the actual coordinates.

Since this is a simple tutorial in formulating and understanding mobile plans using CRAM, developing advanced plans and recovery behaviors is left up to you.