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/27 15:32]
amar Added an overview of some designators used
tutorials:intermediate:simple_mobile_manipulation_plan [2019/04/05 11:14] (current)
amar
Line 1: Line 1:
 **//Tested with Cram v0.7.0, ROS version: Kinetic, Ubuntu 16.04//** **//Tested with Cram v0.7.0, ROS version: Kinetic, Ubuntu 16.04//**
  
-====== Simple Mobile Manipulation Plan (Under Development) ​======+====== Simple Mobile Manipulation Plan ======
  
 This tutorial demonstrates how to write a simple plan for the mobile manipulation using the robot, how to write simple error handling and recovery behavior using a lightweight simulation. This tutorial demonstrates how to write a simple plan for the mobile manipulation using the robot, how to write simple error handling and recovery behavior using a lightweight simulation.
Line 160: Line 160:
       (pp-plans::​park-arms :arm ?​grasping-arm))))       (pp-plans::​park-arms :arm ?​grasping-arm))))
 </​code>​ </​code>​
 +
 +Note that the plan is nested under ''​pr2-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_pr2_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. Now run ''​(move-bottle)''​ and you will see that the robot successfully picks the bottle and places it on the counter.
Line 386: Line 388:
 <code lisp> <code lisp>
 (defun pick-up-object (?​perceived-object ?​object-type ?​grasping-arm) (defun pick-up-object (?​perceived-object ?​object-type ?​grasping-arm)
-  (let((?​possible-grasp +  (let ((?​possible-arms '(:​right ​:left))) 
-           ​(cram-object-interfaces:​get-object-type-grasps ?​object-type nil nil nil ?​grasping-arm)) +    ​;;Retry by changing the 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-retry-counters ((arm-change-retry 1))
         (cpl:​with-failure-handling         (cpl:​with-failure-handling
-            (((or cram-common-failures:manipulation-pose-unreachable +            ((common-fail:object-unreachable (e) 
-                  cram-common-failures:​gripper-closed-completely) ​(e) +               ​(roslisp:​ros-warn (arm-failure) "Manipulation failed: ​~a~%" e)
-               ​(roslisp:​ros-warn (grasp-failure) +
-                                 "~a~%Failed to grasp from ~a using ~a arm " +
-                                 ?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                ​(cpl:​do-retry arm-change-retry
-                 (setf ?​grasping-arm (first (remove ?​grasping-arm ?​possible-arms))) +                 (setf ?​grasping-arm (car (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:​retry))
-               ​(cpl:​fail '​common-fail:​object-unreachable))) +               (roslisp:​ros-warn (arm-failures) "No more retries left"​))) 
-          (exe:​perform (desig:an action +              
-                                 ​(type picking-up) +          ;; Retry by changing the grasp 
-                                 ​(arm ?​grasping-arm) +          (let* ((?​possible-grasp 
-                                 ​(grasp ?grasp) +                   ​(cram-object-interfaces:​get-object-type-grasps ?​object-type nil nil nil ?​grasping-arm)) 
-                                 ​(object ?​perceived-object))))) +                 ​(?​grasp (cut:​lazy-car ?​possible-grasp))) 
-    ?​grasping-arm))+            (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)
 </​code>​ </​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. 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.+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 upso 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.+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. Alsonote 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: Also let's redefine ''​move-bottle''​ again to include these:
Line 500: Line 504:
  
 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. 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.
 +
 +=== Useful Macros ===
 +Many a time, you will find yourselves writing very similar failure handling strategies, because the solution in most cases is to iterate safely between the solutions of a designator or the elements of a list. CRAM provides a couple of useful macros to deal with this to avoid the cumbersome process of rewriting the same logic again and again.
 +You will find these macros under ''​cram_common_failures''​. Some examples are:
 +  * ''​common-fail:​retry-with-designator-solutions''​ - Will retry by iterating over different solutions of the specified designator upon failure.
 +  * ''​common-fail:​retry-with-loc-designator-solutions''​ - Same as the previous case but specialized to location designators. This macro also ensures that the location used is unique (outside a specified threshold) in successive iterations.
 +  * ''​common-fail:​retry-with-list-solutions''​ - Will retry by iterating over elements of a list upon failure. This also supports lazy lists.
 +
 +To demonstrate this, let's take a look at our existing ''​pick-up-object'':​
 +<code lisp>
 +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)
 +</​code>​
 +
 +We can see that we do iterate through different grasp poses in a list in the inner failure handling clause. Let us rewrite this using the macro:
 +
 +<code lisp>
 +(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)
 +                     ​(common-fail:​retry-with-list-solutions
 +                         ?​possible-grasp
 +                         ​grasp-retries
 +                         ​(:​error-object-or-string (format nil 
 +                                                          "​~a~%Failed to grasp from ~a using ~a arm"
 +                                                          e ?grasp ?​grasping-arm)
 +                          :​warning-namespace (grasp-failure)
 +                          :​rethrow-failure '​common-fail:​object-unreachable)
 +                        (setf ?grasp (cut:​lazy-car ?​possible-grasp)))))
 +                ​
 +                (exe:​perform (desig:an action
 +                                       (type picking-up)
 +                                       (arm ?​grasping-arm)
 +                                       ​(grasp ?grasp)
 +                                       ​(object ?​perceived-object)))))))))
 +  ?​grasping-arm)
 +</​code>​
 +This code will behave exactly like before but all the repetitive logic has been moved to this macro. The first two arguments are always the iterating list (in this case, the list containing possible grasp pose) and the number of retries. The keyword ''​error-object-or-string''​ can accept a warning string or just the plain error object and it will be used for logging. ''​warning-namespace''​ will accept the name in which the logging information is classified into. Since our method rethrows ''​common-fail:​object-unreachable''​ we pass that along into ''​rethrow-failure'',​ and if none is passed, it will rethrow the same error it received.