Differences

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

Link to this comparison view

tutorials:intermediate:simple_mobile_manipulation_plan [2019/04/02 18:02]
amar [Increasing the Effectiveness by Improving the plan] Reimplemented the pick-up-object and using it to demonstrate new macros
tutorials:intermediate:simple_mobile_manipulation_plan [2019/04/05 11:14] (current)
amar
Line 506: Line 506:
  
 === Useful Macros === === 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 - which is exactly what we did in ''​pick-up-object''​ method in this example. CRAM provides a couple of useful macros to deal with this to avoid the cumbersome process of rewriting the same logic again and again.+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: 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-designator-solutions''​ - Will retry by iterating over different solutions of the specified designator upon failure.
Line 512: Line 512:
   * ''​common-fail:​retry-with-list-solutions''​ - Will retry by iterating over elements of a list upon failure. This also supports lazy lists.   * ''​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 ​rewrite ​our ''​pick-up-object'' ​method:+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> <code lisp>
 (defun pick-up-object (?​perceived-object ?​object-type ?​grasping-arm) (defun pick-up-object (?​perceived-object ?​object-type ?​grasping-arm)
Line 551: Line 597:
   ?​grasping-arm)   ?​grasping-arm)
 </​code>​ </​code>​
-This code will behave exactly like before ​because ​all the repetitive logic has been moved to this macro. The first two arguments ​for these methods ​are always the iterating ​designator or list (in this case, the possible grasp list) and the amount ​of retries. +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.
-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.+