Differences

This shows you the differences between two versions of the page.

Link to this comparison view

tutorials:intermediate:simple_mobile_manipulation_plan [2019/02/13 13:19]
amar Added more content
tutorials:intermediate:simple_mobile_manipulation_plan [2019/02/19 18:28] (current)
amar
Line 50: Line 50:
    `(and (btr:​bullet-world ?world)    `(and (btr:​bullet-world ?world)
          ​(assert (btr:object ?world :mesh bottle-1          ​(assert (btr:object ?world :mesh bottle-1
-                             ((--0.9 0.860) (0 0 0 1)on the)+                             ((-1.6 -0.9 0.860) (0 0 0 1))
                              :mass 0.2 :color (1 0 0) :mesh :bottle))                              :mass 0.2 :color (1 0 0) :mesh :bottle))
          ​(btr:​simulate ?world 100))))          ​(btr:​simulate ?world 100))))
Line 56: Line 56:
 </​code>​ </​code>​
  
-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 *table-access-pose* is the pose, where pr2 should stand, when he approaches the bottle and *counter-access-pose* is the pose, to access the the counter to place the 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 the counter to place the bottle.
  
 <code lisp> <code lisp>
Line 66: Line 66:
    ​(cl-transforms:​make-identity-rotation)))    ​(cl-transforms:​make-identity-rotation)))
  
-(defparameter *table-access-pose*+(defparameter *base-pose-near-table*
   (cl-transforms-stamped:​make-pose-stamped   (cl-transforms-stamped:​make-pose-stamped
    "​map"​ 0.0    "​map"​ 0.0
-   ​(cl-transforms:​make-3d-vector -1.8547d0 ​-0.150d0 0.0d0)+   ​(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))))    ​(cl-transforms:​axis-angle->​quaternion (cl-transforms:​make-3d-vector 0 0 1) (/ pi -2))))
 +   
 (defparameter *downward-look-coordinate* (defparameter *downward-look-coordinate*
   (cl-transforms-stamped:​make-pose-stamped   (cl-transforms-stamped:​make-pose-stamped
Line 78: Line 78:
    ​(cl-transforms:​make-identity-rotation)))    ​(cl-transforms:​make-identity-rotation)))
  
-(defparameter *counter-access-pose*+(defparameter *base-pose-near-counter*
   (cl-transforms-stamped:​make-pose-stamped   (cl-transforms-stamped:​make-pose-stamped
    "​base_footprint"​ 0.0    "​base_footprint"​ 0.0
-   ​(cl-transforms; Evaluation aborted on #<​CRAM-COMMON-FAILURES:​PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>​.:​make-3d-vector -0.150d0 2.0d0 0.0d0)+   ​(cl-transforms:​make-3d-vector -0.150d0 2.0d0 0.0d0)
    ​(cl-transforms:​make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))    ​(cl-transforms:​make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))
 </​code>​ </​code>​
  
-Now our whole procedure ​would include going near the table, finding the bottle, ​and pick it up. Then go to the counter ​and set it down in the specified destinationWe can define some functions to carry out small steps sepratelyto keep the top-level readable. +Let's note down the steps that would involve picking the bottle from the table and placing ​it on the counter
-<code lisp> +  * Move the robot near the table. 
-(defun navigate-to (?​navigation-goal) +  * Move the arms out of the perception rangeso that the robot can see the bottle without obstruction
-  ​(exe:​perform (desig:a motion +  ​* Look towards the object area. 
-                        (type going) +  ​* Detect the object ​that has to be picked. 
-                        (target (desig:a location (pose ?​navigation-goal)))))) +  ​* Pick up the object ​and once again, keep the arms away from the front. 
-  +  ​* Move the robot near the counter. 
-(defun look-at (?​point-of-interest) +  * Place the bottle on the destination.
-  ​(exe:​perform (desig:a motion +
-                        (type looking) +
-                        (target (desig:a location (pose ?​point-of-interest)))))) +
-                         +
-(defun pick-up-object ​(?​object-designator &​optional (?arm :right)) +
-  ​(exe:​perform (desig:an action +
-                         (type picking-up+
-                         (arm ?arm) +
-                         (object ​?​object-designator)))) +
-                          +
-(defun place-down (?pose ?object ?arm) +
-  ​(exe:​perform (desig:an action +
-                         (type placing) +
-                         (arm ?arm) +
-                         ​(object ?object) +
-                         ​(target (desig:a location (pose ?​pose)))))) +
-                          +
-(defun perceive-object (?​object-type) +
-  ​(let ((?​perceive-direction ​*downward-look-coordinate*)) +
-    (look-at ?​perceive-direction) +
-    (exe:​perform (desig:a motion +
-                          (type detecting) +
-                          (object (desig:an object (type ?​object-type))))))) +
-</​code>​ +
- +
-Now let's try using all these methods in writing a top-level plan for pick and place.+
  
 +Implementing all the above steps in code, will look something like as shown below:
 <code lisp> <code lisp>
 (defun move-bottle () (defun move-bottle ()
   (spawn-bottle)   (spawn-bottle)
-  (proj:​with-projection-environment ​pr2-proj::pr2-bullet-projection-environment +  (pr2-proj:with-simulated-robot 
-    ​(cpl:​top-level +    (let ((?​navigation-goal *base-pose-near-table*)) 
-      ​(let ((?​navigation-goal *table-access-pose*)) +      (cpl:par 
-        (cpl:par +        (exe:​perform (desig:a motion ​ 
-          (exe:​perform (desig:a motion (type moving-torso) (joint-angle 0.3))) +                              ​(type moving-torso) 
-          (pp-plans::​park-arms) +                              ​(joint-angle 0.3))) 
-          (navigate-to ​?​navigation-goal))) +        (pp-plans::​park-arms) 
-      (let ((?​grasping-arm :right) +        ;; Moving the robot near the table. 
-            (?​perceived-bottle (perceive-object :​bottle))) +        ​(exe:perform (desig:a motion 
-        (pick-up-object ?​perceived-bottle) +                              (type going) 
-        (pp-plans::​park-arms :arm ?​grasping-arm) +                              (target (desig:a location  
-        (let ((?nav-goal *counter-access-pose*)) +                                               ​(pose ​?​navigation-goal))))))) 
-          (navigate-to ​?​nav-goal)) +    ;; Looking towards the bottle before perceiving. 
-        (coe:​on-event (make-instance '​cpoe:​robot-state-changed)) +    (let ((?​looking-direction *downward-look-coordinate*)) 
-        (let ((?​drop-pose *final-object-destination*)) +      ​(exe:​perform (desig:a motion  
-            (place-down ​?drop-pose ?​perceived-bottle ?grasping-arm)) +                            (type looking) 
-        (pp-plans::​park-arms :arm ?​grasping-arm)))))+                            (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))))
 </​code>​ </​code>​
  
 Now run ''​(move-bottle)''​ and you will see that the robot successfully picks the bottle and places it on the counter. Now run ''​(move-bottle)''​ and you will see that the robot successfully picks the bottle and places it on the counter.
-Note:- Every time you run ''​move-bottle''​ you need to reset the world with ''​(init-projection)''​. You can include the call at the top of the test function, but mabe you want to comment some parts out to see how the plan behaves. 
  
-==== Handling Failures ​==== +// 
-The previous example worked perfectly because, we as the developer, ​knew the exact coordinates to look for the bottle. ​Now let's try a situation where we approach same table but at different position. For that, let's redefine ​*table-access-pose*+Note:- Every time you run ''​move-bottle''​ you need to reset the world with ''​(init-projection)''​. You can include the call at the top of the test function, but maybe you want to comment some parts out to see how the plan behaves.//​ 
 +===== Increasing the Effectiveness by Improving the plan ===== 
 +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 kitchen environment,​ far from being precise compared to a factory floorWhat 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''​
  
 <code lisp> <code lisp>
 BTW-TUT> BTW-TUT>
-(defparameter ​*table-access-pose*+(defun spawn-bottle () 
 +  (unless (assoc :bottle btr::*mesh-files*) 
 +    (add-objects-to-mesh-list)) 
 +  (prolog:​prolog 
 +   `(and (btr:​bullet-world ?world) 
 +         ​(assert (btr:object ?world :mesh bottle-1 
 +                             ((-2 -0.9 0.860) (0 0 0 1)) 
 +                             :mass 0.2 :color (1 0 0) :mesh :bottle)) 
 +         ​(btr:​simulate ?world 100)))) 
 +</​code>​ 
 +Now try running ''​(move-bottle)''​ again. Don't forget to clean up the projection before running. 
 +And the output will look like this. 
 +<code lisp> 
 +BTW-TUT> (move-bottle) 
 +; Evaluation aborted on #<​CRAM-COMMON-FAILURES:​PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>​. 
 +</​code>​ 
 + 
 +{{:​tutorials:​intermediate:​btw-tut-cant-see-bottle.png?​800|}} 
 + 
 +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.  
 + 
 +=== Recovering from Failures === 
 +To understand the syntax and get a refresher on failure handling, you can refer to [[tutorials:​beginner:​failure_handling|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. 
 +  - Tilt the head of the robot downwards 
 +  - Try to detect the bottle 
 +  - 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. 
 +   - When all possible directions fail, error out. 
 + 
 +Let's define some additional parameters to aid us: 
 +<code lisp> 
 +BTW-TUT>​ 
 +(defparameter *left-downward-look-coordinate*
   (cl-transforms-stamped:​make-pose-stamped   (cl-transforms-stamped:​make-pose-stamped
-   "​map" 0.0 +   "​base_footprint" 0.0 
-   ​(cl-transforms:​make-3d-vector ​-1.447d0 -0.150d0 0.0d0+   ​(cl-transforms:​make-3d-vector ​0.65335d0 ​0.76d0 0.758d0
-   ​(cl-transforms:​axis-angle->​quaternion ​(cl-transforms:​make-3d-vector 0 0 1) (/ pi -2))))+   ​(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))) 
 +   
 </​code>​ </​code>​
-Now try running ​''​(move-bottle)'' ​again.(Don't forget ​to clean up the projection.)+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 :
 +<code lisp>
 +(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))))))
 +        </​code>​
 +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.
 +<code lisp>
 +(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)))))))
 +    ;; 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))))
 +</​code>​
 +
 +Clean up and run ''​(move-bottle)''​ again, and this time, you'll find that the robot succeeds in transporting the bottle.
 +{{:​tutorials:​intermediate:​btw-tut-found-bottle-again.png?​800|}}
 +
 +=== Expanding Failure Management Capabilities ===
 +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:
 +<code lisp>
 +(defun spawn-bottle ()
 +  (unless (assoc :bottle btr::​*mesh-files*)
 +    (add-objects-to-mesh-list))
 +  (prolog:​prolog
 +   `(and (btr:​bullet-world ?world)
 +         ​(assert (btr:object ?world :mesh bottle-1
 +                             ​((-1.1 -0.75 0.860) (0 0 0 1))
 +                             :mass 0.2 :color (1 0 0) :mesh :bottle))
 +         ​(btr:​simulate ?world 100))))
 +</​code>​
 +Now run ''​move-bottle''​ once more, and the output should be something similar to as below:
 <code lisp> <code lisp>
 BTW-TUT> (move-bottle) BTW-TUT> (move-bottle)
-; Evaluation aborted on #<​CRAM-COMMON-FAILURES:​PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>.+[(PICK-PLACE PICK-UP) INFO] 1550502686.470:​ Opening gripper 
 +[(PICK-PLACE PICK-UP) INFO] 1550502686.470:​ Reaching 
 +[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1550502686.797:​ #<​POSE-STAMPED  
 +   ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0 
 +   #<​3D-VECTOR (0.6819813024319948d0 0.4206671881870251d0 -0.11278482277792945d0)>​ 
 +   #<​QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>>​ is unreachable for EE. 
 +Ignoring. 
 +[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1550502687.092:​ #<​POSE-STAMPED  
 +   ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0 
 +   #<​3D-VECTOR (0.6797228574484719d0 0.4210222500057509d0 -0.2627674055849005d0)>​ 
 +   #<​QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>>​ is unreachable for EE. 
 +Failing. 
 +[(PP-PLANS PICK-UP) WARN] 1550502687.092:​ Manipulation messed up: #<​POSE-STAMPED  
 +   ​FRAME-ID:​ "​torso_lift_link",​ STAMP: 0.0 
 +   #<​3D-VECTOR (0.6797228574484719d0 0.4210222500057509d0 -0.2627674055849005d0)>​ 
 +   #<​QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>>​ is unreachable for EE. 
 +Ignoring. 
 +; Evaluation aborted on #<​CRAM-COMMON-FAILURES:​MANIPULATION-POSE-UNREACHABLE ​{10100E64C3}>.
 </​code>​ </​code>​
-[TODOAdd Image] + 
-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. ​Let's try fixing ​this.+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''​
 +<code lisp> 
 +(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)) 
 +</​code>​ 
 +And the part where we pick up the object
 +<code lisp> 
 +        (exe:​perform (desig:an action 
 +                               (type picking-up) 
 +                               (arm ?​grasping-arm) 
 +                               ​(grasp left-side) 
 +                               ​(object ?​perceived-bottle))) 
 +</​code>​ 
 +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). 
 + 
 +{{:​tutorials:​intermediate:​btw-tut-grasp-config-fail.png?​800|}} 
 + 
 +Once again, let's formulate a strategy like the previous case here. The plan we are going for will look something like this: 
 +  - Choose the favored arm. 
 +  - Get all possible grasp poses for the given type of the object and the arm. 
 +  - Choose one grasp from the possible grasp list. 
 +  - Try to grasp the object. 
 +  - If, 
 +     * Grasp succeeds - Continue with the rest of the code 
 +     * Grasp Fails - Choose a different grasp pose from the list. 
 +  - When no more possible grasp poses, try changing the arm used to grasp **Once** and try resuming from  Step (2) 
 +  - When attempted with all arms and grasps, error out
 + 
 +Let's encapsulate all this in a method called ''​pick-up-object'':​ 
 +<code lisp> 
 +(defun pick-up-object (?​perceived-object ?​object-type ?​grasping-arm) 
 +  (let* ((?​possible-grasp 
 +           ​(cram-object-interfaces:​get-object-type-grasps ?​object-type nil nil nil ?​grasping-arm)) 
 +         ​(?​grasp (cut:​lazy-car ?​possible-grasp)) 
 +         ​(?​possible-arms '​(:​left :right))) 
 +    (setf ?​possible-grasp (cut:​lazy-cdr ?​possible-grasp)) 
 +    (cpl:​with-retry-counters ((arm-change-retry 1)) 
 +        (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) 
 + 
 +               ​(pp-plans::​park-arms) 
 +               ;; Try different possible grasp-configurations 
 +               ​(unless (null (cut:​lazy-car ?​possible-grasp)) 
 +                 (setf ?grasp (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)) 
 +                 ​(cpl:​retry)) 
 +               ;; Retry by changing the arm 
 +               ​(cpl:​do-retry arm-change-retry 
 +                 (setf ?​grasping-arm (first (remove ?​grasping-arm ?​possible-arms))) 
 +                 (setf ?​possible-grasp 
 +                       ​(cram-object-interfaces:​get-object-type-grasps 
 +                        ?​object-type nil nil nil ?​grasping-arm)) 
 +                 (setf ?grasp (cut:​lazy-car ?​possible-grasp)) 
 +                 (setf ?​possible-grasp (cut:​lazy-cdr ?​possible-grasp)) 
 +                 ​(cpl:​retry)) 
 +               ​(cpl:​fail '​common-fail:​object-unreachable))) 
 +          (exe:​perform (desig:an action 
 +                                 (type picking-up) 
 +                                 (arm ?​grasping-arm) 
 +                                 ​(grasp ?grasp) 
 +                                 ​(object ?​perceived-object))))) 
 +    ?​grasping-arm)) 
 +</​code>​ 
 +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. 
 +  
 +The failure handling part will takes 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 and then will retry the entire procedure once more after changing the arm which is the reason why our ''​arm-change-retry''​ is set to 1. 
 + 
 +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: 
 +<code lisp> 
 +(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))))))) 
 +       
 +    (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)))) 
 +</​code>​ 
 +You should see a result that looks like the one below. //[Some messages that would come up are suppressed here for readability.]//​ 
 +<code lisp>  
 +BTW-TUT> (init-projection) 
 +BTW-TUT> (move-bottle) 
 +[(PICK-PLACE PICK-UP) INFO] 1550504321.279:​ Opening gripper 
 +[(PICK-PLACE PICK-UP) INFO] 1550504321.279:​ Reaching 
 +[(GRASP-FAILURE) WARN] Failed to grasp from LEFT-SIDE using RIGHT arm  
 +[(TRYING-NEW-GRASP) INFO] 1550504800.749:​ Trying to grasp from RIGHT-SIDE using RIGHT arm 
 +[(PICK-PLACE PICK-UP) INFO] 1550504800.789:​ Opening gripper 
 +[(PICK-PLACE PICK-UP) INFO] 1550504800.789:​ Reaching 
 +[(GRASP-FAILURE) WARN] Failed to grasp from RIGHT-SIDE using RIGHT arm  
 +[(TRYING-NEW-GRASP) INFO] 1550504801.577:​ Trying to grasp from BACK using RIGHT arm 
 +[(PICK-PLACE PICK-UP) INFO] 1550504801.601:​ Opening gripper 
 +[(PICK-PLACE PICK-UP) INFO] 1550504801.602:​ Reaching 
 +[(PICK-PLACE PICK-UP) INFO] 1550504801.939:​ Gripping 
 +[(PICK-PLACE PICK-UP) INFO] 1550504801.973:​ Assert grasp into knowledge base 
 +[(PICK-PLACE PICK-UP) INFO] 1550504801.974:​ Lifting 
 +[(PICK-PLACE PLACE) INFO] 1550504802.356:​ Reaching 
 +[(PICK-PLACE PLACE) INFO] 1550504802.508:​ Putting 
 +[(PICK-PLACE PLACE) INFO] 1550504802.619:​ Opening gripper 
 +[(PICK-PLACE PLACE) INFO] 1550504802.655:​ Retract grasp in knowledge base 
 +[(PICK-PLACE PLACE) INFO] 1550504802.660:​ Retracting 
 +</​code>​ 
 +{{:​tutorials:​intermediate:​btw-tut-grasp-again.png?​800|}} 
 + 
 +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. 
 + 
 +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.