Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
Last revisionBoth sides next revision
playground:playground [2019/07/09 07:54] – tlippsplayground:playground [2019/07/10 11:58] – tlipps
Line 16: Line 16:
 After that the robot gets correctly positioned and kitchen objects spawned before starting. After that the robot gets correctly positioned and kitchen objects spawned before starting.
  
-{{playground:start.png?500|}}+{{ playground:start.png?500 |}}
  
 Since we passed ''(demo-random)'' the object of type bowl the actions searching and transporting will be executed: Since we passed ''(demo-random)'' the object of type bowl the actions searching and transporting will be executed:
 <code lisp> <code lisp>
 (when (eq ?object-type :bowl) (when (eq ?object-type :bowl)
-(cpl:with-failure-handling +  (cpl:with-failure-handling 
-((common-fail:high-level-failure (e) +    ((common-fail:high-level-failure (e) 
-(roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping the search" e) +      (roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping the search" e) 
-(return))) +      (return))) 
-(let ((?loc (cdr (assoc :breakfast-cereal object-fetching-locations)))) +    (let ((?loc (cdr (assoc :breakfast-cereal object-fetching-locations)))) 
-(exe:perform +      (exe:perform 
-(desig:an action +        (desig:an action 
-(type searching) +                  (type searching) 
-(object (desig:an object (type breakfast-cereal))) +                  (object (desig:an object (type breakfast-cereal))) 
-(location ?loc))))))+                  (location ?loc))))))
  
 (cpl:with-failure-handling (cpl:with-failure-handling
-((common-fail:high-level-failure (e) +    ((common-fail:high-level-failure (e) 
-(roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping..." e) +        (roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping..." e) 
-(return))) +        (return))) 
-(if (eq ?object-type :bowl) +  (if (eq ?object-type :bowl) 
-(exe:perform +      (exe:perform 
-(desig:an action +       (desig:an action 
-(type transporting) +                 (type transporting) 
-(object ?object-to-fetch) +                 (object ?object-to-fetch) 
-;; (arm right) +                 (location ?fetching-location) 
-(location ?fetching-location) +                 (target ?delivering-location)))
-(target ?delivering-location)))+
 </code> </code>
  
Line 52: Line 51:
 <code lisp> <code lisp>
 (defun search-for-object (&key (defun search-for-object (&key
-((:object ?object-designator)) +                           ((:object ?object-designator)) 
-((:location ?search-location)) +                           ((:location ?search-location)) 
-((:robot-location ?robot-location)) +                           ((:robot-location ?robot-location)) 
-(retries 3) +                           (retries 3) 
-&allow-other-keys)+                          &allow-other-keys)
 (declare (type desig:object-designator ?object-designator) (declare (type desig:object-designator ?object-designator)
 ;; location desigs can turn NILL in the course of execution ;; location desigs can turn NILL in the course of execution
Line 70: Line 69:
 <code lisp> <code lisp>
 (cpl:with-failure-handling (cpl:with-failure-handling
-((desig:designator-error (e) +    ((desig:designator-error (e) 
-(roslisp:ros-warn (fd-plans search-for-object) +      (roslisp:ros-warn (fd-plans search-for-object) 
-"Desig ~a could not be resolved: ~a~%Propagating up." +                        "Desig ~a could not be resolved: ~a~%Propagating up." 
-?search-location e) +                        ?search-location e) 
-(cpl:fail 'common-fail:object-nowhere-to-be-found +      (cpl:fail 'common-fail:object-nowhere-to-be-found 
-:description "Search location designator could not be resolved.")))+                :description "Search location designator could not be resolved.")))
 </code> </code>
  
-If the referencing of below designators fail the error ''object-nowhere-to-be-found'' will be thrown.+If the referencing of below designators fails the error ''object-nowhere-to-be-found'' will be thrown.
  
-The basic idea is the following: First the robot tries to navigate to the location ''?robot-location''. If this is succeeded within a number of retries the robot tries now to turn towards the location ''?search-location'' and detect the object within in the number of specific retries too.+The basic idea is the following: First the robot tries to navigate to the location ''?robot-location''. If this is succeeded within a number of retries the robot tries now to turn towards the location ''?search-location'' and detect the object within the number of specific retries too.
 Now if the robot cannot detect the object or cannot navigate to the ''?robot-location'', we jump above the navigation part, set a new ''?search-location'' and try all above again. This is done maximal ''outer-search-location-retries'' times. Now if the robot cannot detect the object or cannot navigate to the ''?robot-location'', we jump above the navigation part, set a new ''?search-location'' and try all above again. This is done maximal ''outer-search-location-retries'' times.
  
Line 86: Line 85:
 ;; take new `?search-location' sample if a failure happens and retry ;; take new `?search-location' sample if a failure happens and retry
 (cpl:with-retry-counters ((outer-search-location-retries 2)) (cpl:with-retry-counters ((outer-search-location-retries 2))
-(cpl:with-failure-handling +  (cpl:with-failure-handling 
-((common-fail:object-nowhere-to-be-found (e) +    ((common-fail:object-nowhere-to-be-found (e) 
-(common-fail:retry-with-loc-designator-solutions +      (common-fail:retry-with-loc-designator-solutions 
-?search-location +          ?search-location 
-outer-search-location-retries +          outer-search-location-retries 
-(:error-object-or-string e +          (:error-object-or-string e 
-:warning-namespace (fd-plans search-for-object) +          :warning-namespace (fd-plans search-for-object) 
-:reset-designators (list ?robot-location) +          :reset-designators (list ?robot-location) 
-:rethrow-failure 'common-fail:object-nowhere-to-be-found) +          :rethrow-failure 'common-fail:object-nowhere-to-be-found) 
-(roslisp:ros-warn (fd-plans search-for-object) +       (roslisp:ros-warn (fd-plans search-for-object) 
-"Search is about to give up. Retrying~%"))))+                         "Search is about to give up. Retrying~%"))))
 </code> </code>
  
Line 104: Line 103:
 ;; if the going action fails, pick another `?robot-location' sample and retry ;; if the going action fails, pick another `?robot-location' sample and retry
 (cpl:with-retry-counters ((robot-location-retries 10)) (cpl:with-retry-counters ((robot-location-retries 10))
-(cpl:with-failure-handling +  (cpl:with-failure-handling 
-(((or common-fail:navigation-goal-in-collision +      (((or common-fail:navigation-goal-in-collision 
-common-fail:looking-high-level-failure +            common-fail:looking-high-level-failure 
-common-fail:perception-low-level-failure) (e) +            common-fail:perception-low-level-failure) (e) 
-(common-fail:retry-with-loc-designator-solutions +         (common-fail:retry-with-loc-designator-solutions 
-?robot-location +             ?robot-location 
-robot-location-retries +             robot-location-retries 
-(:error-object-or-string e +             (:error-object-or-string e 
-:warning-namespace (fd-plans search-for-object) +              :warning-namespace (fd-plans search-for-object) 
-:reset-designators (list ?search-location) +              :reset-designators (list ?search-location) 
-:rethrow-failure 'common-fail:object-nowhere-to-be-found))))+              :rethrow-failure 'common-fail:object-nowhere-to-be-found))))
  
-;; navigate +     ;; navigate 
-(exe:perform (desig:an action +     (exe:perform (desig:an action 
-(type navigating) +                            (type navigating) 
-(location ?robot-location)))+                            (location ?robot-location)))
 </code> </code>
  
Line 200: Line 199:
 ;; if perception action fails, try another `?search-location' and retry ;; if perception action fails, try another `?search-location' and retry
 (cpl:with-retry-counters ((search-location-retries retries)) (cpl:with-retry-counters ((search-location-retries retries))
-(cpl:with-failure-handling +  (cpl:with-failure-handling 
-(((or common-fail:perception-low-level-failure +      (((or common-fail:perception-low-level-failure 
-common-fail:looking-high-level-failure) (e) +            common-fail:looking-high-level-failure) (e) 
-(common-fail:retry-with-loc-designator-solutions +         (common-fail:retry-with-loc-designator-solutions 
-?search-location +             ?search-location 
-search-location-retries +             search-location-retries 
-(:error-object-or-string e +             (:error-object-or-string e 
-:warning-namespace (fd-plans search-for-object) +              :warning-namespace (fd-plans search-for-object) 
-:reset-designators (list ?robot-location)))))+              :reset-designators (list ?robot-location)))))
  
-(exe:perform (desig:an action +       (exe:perform (desig:an action 
-(type turning-towards) +                              (type turning-towards) 
-(target ?search-location))) +                              (target ?search-location))) 
-(exe:perform (desig:an action +       (exe:perform (desig:an action 
-(type detecting) +                              (type detecting) 
-(object ?object-designator)))))))))))+                              (object ?object-designator)))))))))))
 </code> </code>
  
 Here we try to turn towards the location to search for and try to detect the object. If on of these action fails, we try like the parameter ''retries'' says times again. If it stills fails we throw the error ''object-nowhere-to-be-found'' and try therefore then first to navigate the robot before detecting again the object. Here we try to turn towards the location to search for and try to detect the object. If on of these action fails, we try like the parameter ''retries'' says times again. If it stills fails we throw the error ''object-nowhere-to-be-found'' and try therefore then first to navigate the robot before detecting again the object.
 The detection action calls the ''perceive'' function in ''cram_mobile_pick_place_plans/src/atomic-action-plans'' to detect the object and returns a motion designator. The function ''turn-towards'' will create a looking action designator which calls the ''look-at'' function in the above file too. The detection action calls the ''perceive'' function in ''cram_mobile_pick_place_plans/src/atomic-action-plans'' to detect the object and returns a motion designator. The function ''turn-towards'' will create a looking action designator which calls the ''look-at'' function in the above file too.
-At the end the function ''search-for-object'' returns an object designator with a pose like this e.g.:+At the end the function ''search-for-object'' returns an object designator with a pose like this e. g.: 
 <code lisp> <code lisp>
 #<A OBJECT #<A OBJECT
 (LOCATION #<A LOCATION (LOCATION #<A LOCATION
-(ON #<A OBJECT + (ON #<A OBJECT 
-(TYPE COUNTER-TOP) + (TYPE COUNTER-TOP) 
-(URDF-NAME SINK-AREA-SURFACE) + (URDF-NAME SINK-AREA-SURFACE) 
-(OWL-NAME "kitchen_sink_block_counter_top") + (OWL-NAME "kitchen_sink_block_counter_top") 
-(PART-OF KITCHEN)>) + (PART-OF KITCHEN)>) 
-(SIDE LEFT) + (SIDE LEFT) 
-(SIDE FRONT) + (SIDE FRONT) 
-(RANGE-INVERT 0.5)>)+ (RANGE-INVERT 0.5)>)
 (TYPE BOWL) (TYPE BOWL)
 (NAME BOWL-1) (NAME BOWL-1)
Line 260: Line 260:
 <code lisp> <code lisp>
 (defun transport (&key (defun transport (&key
-((:object ?object-designator)) +                    ((:object ?object-designator)) 
-((:search-location ?search-location)) +                    ((:search-location ?search-location)) 
-((:search-robot-location ?search-base-location)) +                    ((:search-robot-location ?search-base-location)) 
-((:fetch-robot-location ?fetch-robot-location)) +                    ((:fetch-robot-location ?fetch-robot-location)) 
-((:arm ?arm)) +                    ((:arm ?arm)) 
-((:grasp ?grasp)) +                    ((:grasp ?grasp)) 
-((:arms ?arms)) +                    ((:arms ?arms)) 
-((:grasps ?grasps)) +                    ((:grasps ?grasps)) 
-((:deliver-location ?delivering-location)) +                    ((:deliver-location ?delivering-location)) 
-((:deliver-robot-location ?deliver-robot-location)) +                    ((:deliver-robot-location ?deliver-robot-location)) 
-search-location-accessible +                    search-location-accessible 
-delivery-location-accessible +                    delivery-location-accessible 
-&allow-other-keys)+                  &allow-other-keys)
 </code> </code>
-text...+
 <code lisp> <code lisp>
 (unless search-location-accessible (unless search-location-accessible
-(exe:perform (desig:an action +    (exe:perform (desig:an action 
-(type accessing) +                           (type accessing) 
-(location ?search-location) +                           (location ?search-location) 
-(distance 0.3))))+                           (distance 0.48)))) 
 +</code>
  
 +If the object is not accessible e. g. if you want to pick up a fork in a drawer, this drawer has to be first 
 +be opened by the robot. For this the action ''accessing'' is called. 
 +
 +<code lisp>
 (unwind-protect (unwind-protect
-(let ((?perceived-object-designator +       (let ((?perceived-object-designator 
-(exe:perform (desig:an action +               (exe:perform (desig:an action 
-(type searching) +                                      (type searching) 
-(object ?object-designator) +                                      (object ?object-designator) 
-(location ?search-location) +                                      (location ?search-location) 
-(desig:when ?search-base-location +                                      (desig:when ?search-base-location 
-(robot-location ?search-base-location))))) +                                        (robot-location ?search-base-location))))) 
-(?robot-name +</code> 
-(cut:var-value '?robot-name +To fetch the object the object first has to be searched. For this we create 
-(car (prolog:prolog '(rob-int:robot ?robot-name)))))) +and resolve an action designator of type searching with set object-designator, 
-(roslisp:ros-info (pp-plans transport) +the robot-location if available and the search-location like for the bowl e. g. this: 
-"Found object of type ~a." +<code lisp> 
-(desig:desig-prop-value ?perceived-object-designator :type))+(desig:a location 
 +                               (on (desig:an object 
 +                                             (type counter-top) 
 +                                             (urdf-name sink-area-surface) 
 +                                             (owl-name "kitchen_sink_block_counter_top") 
 +                                             (part-of kitchen))) 
 +                               (side left) 
 +                               (side front) 
 +                               (range-invert 0.5))) 
 +</code>
  
 +<code lisp>
 (unless ?fetch-robot-location (unless ?fetch-robot-location
-(setf ?fetch-robot-location +           (setf ?fetch-robot-location 
-(desig:a location +                 (desig:a location 
-(reachable-for ?robot-name) +                          (reachable-for ?robot-name) 
-(desig:when ?arm +                          (desig:when ?arm 
-(arm ?arm)) +                            (arm ?arm)) 
-(object ?perceived-object-designator)))) +                          (object ?perceived-object-designator)))) 
-(unless ?deliver-robot-location +         (unless ?deliver-robot-location 
-(setf ?deliver-robot-location +           (setf ?deliver-robot-location 
-(desig:a location +                 (desig:a location 
-(reachable-for ?robot-name) +                          (reachable-for ?robot-name) 
-(location ?delivering-location))))+                          (location ?delivering-location)))) 
 +</code> 
 +Moreover, we need the fetch- and deliver-robot-locations. The ?fetch-robot-location location-designator take the the perceived object and the deliver-robot-location location designator take the location where it should be delivered to.
  
-;; If running on the real robot, execute below task tree in projection +<code lisp> 
-;; N times first, then pick the best parameterization +         ;; If running on the real robot, execute below task tree in projection 
-;; and use that parameterization in the real world. +         ;; N times first, then pick the best parameterization 
-;; If running in projection, just execute the task tree below as normal. +         ;; and use that parameterization in the real world. 
-(let (?fetch-pick-up-action ?deliver-place-action) +         ;; If running in projection, just execute the task tree below as normal. 
-(proj-reasoning:with-projected-task-tree +         (let (?fetch-pick-up-action ?deliver-place-action) 
-(?fetch-robot-location ?fetch-pick-up-action +           (proj-reasoning:with-projected-task-tree 
-?deliver-robot-location ?deliver-place-action) +               (?fetch-robot-location ?fetch-pick-up-action 
-3 +                                      ?deliver-robot-location ?deliver-place-action) 
-#'proj-reasoning:pick-best-parameters-by-distance+               3 
 +               #'proj-reasoning:pick-best-parameters-by-distance
  
-(let ((?fetched-object +             (let ((?fetched-object 
-(exe:perform (desig:an action +                     (exe:perform (desig:an action 
-(type fetching) +                                            (type fetching) 
-(desig:when ?arm +                                            (desig:when ?arm 
-(arm ?arm)) +                                              (arm ?arm)) 
-(desig:when ?grasp +                                            (desig:when ?grasp 
-(grasp ?grasp)) +                                              (grasp ?grasp)) 
-(desig:when ?arms +                                            (desig:when ?arms 
-(arms ?arms)) +                                              (arms ?arms)) 
-(desig:when ?grasps +                                            (desig:when ?grasps 
-(grasps ?grasps)) +                                              (grasps ?grasps)) 
-(object ?perceived-object-designator) +                                            (object ?perceived-object-designator) 
-(robot-location ?fetch-robot-location) +                                            (robot-location ?fetch-robot-location) 
-(pick-up-action ?fetch-pick-up-action)))))+                                            (pick-up-action ?fetch-pick-up-action))))) 
 +</code> 
 +Now we create the action designator fetching and execute it. If the arms and graps are given we set them so they will be used by ''fetch''. Moreover, the perceived object is given and the ''?fetch-robot-location'' like the example above. For the low level function ''pick-up'' a placeholder is given too.
  
-(roslisp:ros-info (pp-plans transport) "Fetched the object.") +<code lisp>
-(cpl:with-failure-handling +
-((common-fail:object-undeliverable (e) +
-(declare (ignore e)) +
-(drop-at-sink) +
-;; (return) +
-))+
 (unless delivery-location-accessible (unless delivery-location-accessible
-(exe:perform (desig:an action +                   (exe:perform (desig:an action 
-(type accessing) +                                          (type accessing) 
-(location ?delivering-location) +                                          (location ?delivering-location) 
-(distance 0.3)))) +                                          (distance 0.3)))) 
-(unwind-protect +                 (unwind-protect 
-(exe:perform (desig:an action +                      (exe:perform (desig:an action 
-(type delivering) +                                             (type delivering) 
-(desig:when ?arm +                                             (desig:when ?arm 
-(arm ?arm)) +                                               (arm ?arm)) 
-(object ?fetched-object) +                                             (object ?fetched-object) 
-(target ?delivering-location) +                                             (target ?delivering-location) 
-(robot-location ?deliver-robot-location) +                                             (robot-location ?deliver-robot-location) 
-(place-action ?deliver-place-action))) +                                             (place-action ?deliver-place-action))) 
-(unless delivery-location-accessible +                   (unless delivery-location-accessible 
-(exe:perform (desig:an action +                     (exe:perform (desig:an action 
-(type sealing) +                                            (type sealing) 
-(location ?delivering-location) +                                            (location ?delivering-location) 
-(distance 0.3))))))))))+                                            (distance 0.3)))))))))) 
 +</code> 
 +In the last part of this function we first check if the deliver location is accessible (''delivery-location-accessible'') and if not get access with executing an action. Then we deliver the fetched object to the ''?delivering-location'' by moving the robot_base to ''?deliver-robot-location'' and given an placeholder for the low-level function ''place''.  
  
-(unless search-location-accessible +<code lisp> 
-(exe:perform (desig:an action + (unless search-location-accessible 
-(type sealing) +      (exe:perform (desig:an action 
-(location ?search-location) +                             (type sealing) 
-(distance 0.3))))))+                             (location ?search-location) 
 +                             (distance 0.48))))))
 </code> </code>
-more text...+ 
 +At the end we seal the e. g. drawer we got our fork from with a specific distance towards the drawer.
  
 **go-without-collisions** **go-without-collisions**
Line 375: Line 393:
 <code lisp> <code lisp>
 (defun go-without-collisions (&key (defun go-without-collisions (&key
-((:location ?navigation-location)) +                                ((:location ?navigation-location)) 
-&allow-other-keys) +                              &allow-other-keys) 
-(declare (type desig:location-designator ?navigation-location)) +  (declare (type desig:location-designator ?navigation-location)) 
-"Check if navigation goal is in reach, if not propagate failure up,+  "Check if navigation goal is in reach, if not propagate failure up,
 if yes, perform GOING action while ignoring failures." if yes, perform GOING action while ignoring failures."
-\end{lstlisting} + 
-The only input parameter is the location going to which has to be a location designator. +  (exe:perform (desig:an action 
-\begin{lstlisting} +                         (type positioning-arm) 
-(exe:perform (desig:an action +                         (left-configuration park) 
-(type positioning-arm) +                         (right-configuration park)))
-(left-configuration park) +
-(right-configuration park)))+
 </code> </code>
  
-First we move the arms in the park position, so the don't hit anything on their way.+First we move the arms in the park position, so they do not hit anything on their way. 
 <code lisp> <code lisp>
 (proj-reasoning:check-navigating-collisions ?navigation-location) (proj-reasoning:check-navigating-collisions ?navigation-location)
Line 399: Line 416:
 (setf ?navigation-location (desig:current-desig ?navigation-location)) (setf ?navigation-location (desig:current-desig ?navigation-location))
  
-(cpl:with-failure-handling +  (cpl:with-failure-handling 
-((common-fail:navigation-low-level-failure (e) +      ((common-fail:navigation-low-level-failure (e) 
-(roslisp:ros-warn (pp-plans navigate) +         (roslisp:ros-warn (pp-plans navigate) 
-"Low-level navigation failed: ~a~%.Ignoring anyway." e) +                           "Low-level navigation failed: ~a~%.Ignoring anyway." e) 
-(return))) +         (return))) 
-(exe:perform (desig:an action +    (exe:perform (desig:an action 
-(type going) +                           (type going) 
-(target ?navigation-location)))))+                           (target ?navigation-location)))))
 </code> </code>
  
Line 415: Line 432:
 <code lisp> <code lisp>
 (defun turn-towards (&key (defun turn-towards (&key
-((:target ?look-target)) +                       ((:target ?look-target)) 
-((:robot-location ?robot-location)) +                       ((:robot-location ?robot-location)) 
-&allow-other-keys) +                     &allow-other-keys) 
-(declare (type desig:location-designator ?look-target ?robot-location)) +  (declare (type desig:location-designator ?look-target ?robot-location)) 
-"Perform a LOOKING action, if looking target twists the neck, +  "Perform a LOOKING action, if looking target twists the neck, 
-turn the robot base such that it looks in the direction of target  +turn the robot base such that it looks in the direction of target and look again."
-and look again."+
 </code> </code>
  
Line 427: Line 443:
  
 <code lisp> <code lisp>
-(cpl:with-failure-handling + (cpl:with-failure-handling 
-((desig:designator-error (e) +      ((desig:designator-error (e) 
-(roslisp:ros-warn (fd-plans turn-towards) +         (roslisp:ros-warn (fd-plans turn-towards) 
-"Desig ~a could not be resolved: ~a~%Cannot look." +                           "Desig ~a could not be resolved: ~a~%Cannot look." 
-?look-target e) +                           ?look-target e) 
-(cpl:fail 'common-fail:looking-high-level-failure))+         (cpl:fail 'common-fail:looking-high-level-failure))
  
-(common-fail:navigation-high-level-failure (e) +       (common-fail:navigation-high-level-failure (e) 
-(roslisp:ros-warn (fd-plans turn-towards) +         (roslisp:ros-warn (fd-plans turn-towards) 
-"When turning around navigation failure happened: ~a~%~ +                           "When turning around navigation failure happened: ~a~%~ 
-Cannot look." +                              Cannot look." 
-e) +                           e) 
-(cpl:fail 'common-fail:looking-high-level-failure)))+         (cpl:fail 'common-fail:looking-high-level-failure)))
 </code> </code>
  
-If the referencing below of the designators with type looking or navigation fail ''designator-error'' or if especially the navigation fails with a ''navigation-high-level-failure'' error, although this is not implemented since errors will be ignored (see ''go-without-collisions''), the error ''looking-high-level-failure'' will be thrown.+If the referencing of the designators below with type looking or navigation fail ''designator-error'' or if especially the navigation fails with a ''navigation-high-level-failure'' error, although this is not implemented since errors will be ignored (see ''go-without-collisions''), the error ''looking-high-level-failure'' will be thrown.
  
 <code lisp> <code lisp>
-(cpl:with-retry-counters ((turn-around-retries 1)) + (cpl:with-retry-counters ((turn-around-retries 1)) 
-(cpl:with-failure-handling +      (cpl:with-failure-handling 
-((common-fail:ptu-low-level-failure (e) +          ((common-fail:ptu-low-level-failure (e) 
-(roslisp:ros-warn (pp-plans turn-towards) "~a~%Turning around." e) +             (roslisp:ros-warn (pp-plans turn-towards) "~a~%Turning around." e) 
-(cpl:do-retry turn-around-retries +             (cpl:do-retry turn-around-retries 
-(cpl:par +               (cpl:par 
-(exe:perform (desig:an action +                 (exe:perform (desig:an action 
-(type navigating) +                                        (type navigating) 
-(location ?robot-location))) +                                        (location ?robot-location))) 
-(exe:perform (desig:an action +                 (exe:perform (desig:an action 
-(type looking) +                                        (type looking) 
-(direction forward)))) +                                        (direction forward)))) 
-(cpl:retry)) +               (cpl:retry)) 
-(roslisp:ros-warn (pp-plans turn-towards) "Turning around didn't work :'(~%") +             (roslisp:ros-warn (pp-plans turn-towards) "Turning around didn't work :'(~%") 
-(cpl:fail 'common-fail:looking-high-level-failure))) +             (cpl:fail 'common-fail:looking-high-level-failure)))
-(exe:perform (desig:an action +
-(type looking) +
-(target ?look-target)))))))+
 </code> </code>
  
 Now the robot tries with  Now the robot tries with 
 <code lisp> <code lisp>
-(exe:perform (desig:an action + (exe:perform (desig:an action 
-(type looking) +                               (type looking) 
-(target ?look-target))+                               (target ?look-target)))))))
 </code> </code>
 to look at a location. If this fails with an ''ptu-low-level-failure'' error the robot tries ''turn-around-retries'' times to turn around and look forward. Then ''cpl:retry'' will be called and the robot tries to look at the location ''?look-target'' again. to look at a location. If this fails with an ''ptu-low-level-failure'' error the robot tries ''turn-around-retries'' times to turn around and look forward. Then ''cpl:retry'' will be called and the robot tries to look at the location ''?look-target'' again.
Line 477: Line 490:
 <code lisp> <code lisp>
 (defun fetch (&key (defun fetch (&key
-((:object ?object-designator)) +                ((:object ?object-designator)) 
-((:arms ?arms)) +                ((:arms ?arms)) 
-((:grasps ?grasps)) +                ((:grasps ?grasps)) 
-((:robot-location ?pick-up-robot-location)) +                ((:robot-location ?pick-up-robot-location)) 
-pick-up-action +                pick-up-action 
-&allow-other-keys) +              &allow-other-keys) 
-(declare (type desig:object-designator ?object-designator) +  (declare (type desig:object-designator ?object-designator) 
-(type list ?arms ?grasps) +           (type list ?arms ?grasps) 
-;; ?pick-up-robot-location should not be NULL at the beginning +           ;; ?pick-up-robot-location should not be NULL at the beginning 
-;; but can become NULL during execution of the plan +           ;; but can become NULL during execution of the plan 
-(type (or desig:location-designator null) ?pick-up-robot-location) +           (type (or desig:location-designator null) ?pick-up-robot-location) 
-(type (or desig:action-designator null) pick-up-action)) +           (type (or desig:action-designator null) pick-up-action)) 
-"Fetches a perceived object `?object-designator' with +  "Fetches a perceived object `?object-designator' with 
-one of arms in the `?arms' lazy list (if not NIL) and one of grasps +one of arms in the `?arms' lazy list (if not NIL) and one of grasps in `?grasps' if not NIL, 
-in `?grasps' if not NIL, while standing at `?pick-up-robot-location'+while standing at `?pick-up-robot-location'
 and using the grasp and arm specified in `pick-up-action' (if not NIL)." and using the grasp and arm specified in `pick-up-action' (if not NIL)."
 </code> </code>
Line 499: Line 512:
 <code lisp> <code lisp>
 (cpl:with-failure-handling (cpl:with-failure-handling
-((desig:designator-error (e) +      ((desig:designator-error (e) 
-(roslisp:ros-warn (fd-plans fetch) "~a~%Propagating up." e) +         (roslisp:ros-warn (fd-plans fetch) "~a~%Propagating up." e) 
-(cpl:fail 'common-fail:object-unfetchable +         (cpl:fail 'common-fail:object-unfetchable 
-:object ?object-designator +                   :object ?object-designator 
-:description "Some designator could not be resolved.")))+                   :description "Some designator could not be resolved.")))
 </code> </code>
  
Line 509: Line 522:
  
 <code lisp> <code lisp>
-;; take a new `?pick-up-robot-location' sample if a failure happens  + ;; take a new `?pick-up-robot-location' sample if a failure happens 
-(cpl:with-retry-counters ((relocation-for-ik-retries 20)) +    (cpl:with-retry-counters ((relocation-for-ik-retries 20)) 
-(cpl:with-failure-handling +      (cpl:with-failure-handling 
-(((or common-fail:navigation-goal-in-collision ;; from navigation action +          (((or common-fail:navigation-goal-in-collision 
-common-fail:looking-high-level-failure ;; from turning-towards action +                common-fail:looking-high-level-failure 
-common-fail:perception-low-level-failure +                common-fail:perception-low-level-failure 
-common-fail:object-unreachable ;; from picking-up +                common-fail:object-unreachable 
-common-fail:manipulation-low-level-failure) (e) ;; and this too +                common-fail:manipulation-low-level-failure) (e) 
-(common-fail:retry-with-loc-designator-solutions +             (common-fail:retry-with-loc-designator-solutions 
-?pick-up-robot-location +                 ?pick-up-robot-location 
-relocation-for-ik-retries +                 relocation-for-ik-retries 
-(:error-object-or-string +                 (:error-object-or-string 
-(format NIL "Object of type ~a is unreachable: ~a" +                  (format NIL "Object of type ~a is unreachable: ~a" 
-(desig:desig-prop-value ?object-designator :type) e) +                          (desig:desig-prop-value ?object-designator :type) e) 
-:warning-namespace (fd-plans fetch) +                  :warning-namespace (fd-plans fetch) 
-:rethrow-failure 'common-fail:object-unfetchable))))+                  :rethrow-failure 'common-fail:object-unfetchable))))
 </code> </code>
  
Line 530: Line 543:
  
 <code lisp> <code lisp>
-;; navigate, look, detect and pick-up + ;; navigate, look, detect and pick-up 
-(exe:perform (desig:an action +        (exe:perform (desig:an action 
-(type navigating) +                               (type navigating) 
-(location ?pick-up-robot-location)))+                               (location ?pick-up-robot-location)))
 </code> </code>
  
-The first action should move the robot without any collisions to the location ''?pick-up-robot-location''. The location designator looks e.g. like this: +The first action should move the robot without any collisions to the location ''?pick-up-robot-location''. The location designator looks e. g. like this: 
  
 <code lisp> <code lisp>
Line 584: Line 597:
 <code lisp> <code lisp>
 (exe:perform (desig:an action (exe:perform (desig:an action
-(type turning-towards) +                               (type turning-towards) 
-(target (desig:a location (of ?object-designator)))))+                               (target (desig:a location (of ?object-designator))))) 
 </code> </code>
  
Line 594: Line 608:
 (TYPE TURNING-TOWARDS) (TYPE TURNING-TOWARDS)
 (TARGET #<A LOCATION (TARGET #<A LOCATION
-(OF #<A OBJECT + (OF #<A OBJECT 
-(LOCATION #<A LOCATION + (LOCATION #<A LOCATION 
-(ON #<A OBJECT + (ON #<A OBJECT 
-(TYPE COUNTER-TOP) + (TYPE COUNTER-TOP) 
-(URDF-NAME SINK-AREA-SURFACE) + (URDF-NAME SINK-AREA-SURFACE) 
-(OWL-NAME kitchen_sink_block_counter_top) + (OWL-NAME kitchen_sink_block_counter_top) 
-(PART-OF KITCHEN)>) + (PART-OF KITCHEN)>) 
-(SIDE LEFT) + (SIDE LEFT) 
-(SIDE FRONT) + (SIDE FRONT) 
-(RANGE-INVERT 0.5)>) + (RANGE-INVERT 0.5)>) 
-(TYPE BOWL) + (TYPE BOWL) 
-(NAME BOWL-1)+ (NAME BOWL-1)
 (POSE ((POSE (POSE ((POSE
 #<POSE-STAMPED  #<POSE-STAMPED 
Line 633: Line 647:
 #<A LOCATION #<A LOCATION
 (OF #<A OBJECT (OF #<A OBJECT
-(LOCATION #<A LOCATION + (LOCATION #<A LOCATION 
-(ON #<A OBJECT + (ON #<A OBJECT 
-(TYPE COUNTER-TOP) + (TYPE COUNTER-TOP) 
-(URDF-NAME SINK-AREA-SURFACE) + (URDF-NAME SINK-AREA-SURFACE) 
-(OWL-NAME "kitchen_sink_block_counter_top") + (OWL-NAME "kitchen_sink_block_counter_top") 
-(PART-OF KITCHEN)>) + (PART-OF KITCHEN)>) 
-(SIDE LEFT) + (SIDE LEFT) 
-(SIDE FRONT) + (SIDE FRONT) 
-(RANGE-INVERT 0.5)>) + (RANGE-INVERT 0.5)>) 
-(TYPE BOWL) + (TYPE BOWL) 
-(NAME BOWL-1)+ (NAME BOWL-1)
 (POSE ((:POSE (POSE ((:POSE
 #<CL-TRANSFORMS-STAMPED:POSE-STAMPED  #<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
Line 666: Line 680:
 </code> </code>
  
- and returns a lazy list of pose-stamped with which then a looking action can be executed.+and returns a lazy list of pose-stamped with which then a looking action can be executed.
  
 <code lisp> <code lisp>
Line 672: Line 686:
 (TYPE LOOKING) (TYPE LOOKING)
 (TARGET #<A LOCATION (TARGET #<A LOCATION
-(OF #<A OBJECT + (OF #<A OBJECT 
-(LOCATION #<A LOCATION + (LOCATION #<A LOCATION 
-(ON #<A OBJECT + (ON #<A OBJECT 
-(TYPE COUNTER-TOP) + (TYPE COUNTER-TOP) 
-(URDF-NAME SINK-AREA-SURFACE) + (URDF-NAME SINK-AREA-SURFACE) 
-(OWL-NAME kitchen_sink_block_counter_top) + (OWL-NAME kitchen_sink_block_counter_top) 
-(PART-OF KITCHEN)>) + (PART-OF KITCHEN)>) 
-(SIDE LEFT) + (SIDE LEFT) 
-(SIDE FRONT) + (SIDE FRONT) 
-(RANGE-INVERT 0.5)>) + (RANGE-INVERT 0.5)>) 
-(TYPE BOWL) + (TYPE BOWL) 
-(NAME BOWL-1) + (NAME BOWL-1) 
-(POSE ((POSE+ (POSE ((POSE
 #<POSE-STAMPED  #<POSE-STAMPED 
 FRAME-ID: "base_footprint", STAMP: 1.560865813733836d9 FRAME-ID: "base_footprint", STAMP: 1.560865813733836d9
Line 707: Line 721:
  
 <code lisp> <code lisp>
-(cpl:with-retry-counters ((regrasping-retries 1)) + (cpl:with-retry-counters ((regrasping-retries 1)) 
-(cpl:with-failure-handling +          (cpl:with-failure-handling 
-((common-fail:gripper-low-level-failure (e) +              ((common-fail:gripper-low-level-failure (e) 
-(roslisp:ros-warn (fd-plans fetch) "Misgrasp happened: ~a~%" e) +                 (roslisp:ros-warn (fd-plans fetch) "Misgrasp happened: ~a~%" e) 
-(cpl:do-retry regrasping-retries +                 (cpl:do-retry regrasping-retries 
-(roslisp:ros-info (fd-plans fetch) "Reperceiving and repicking...") +                   (roslisp:ros-info (fd-plans fetch) "Reperceiving and repicking...") 
-(exe:perform (desig:an action +                   (exe:perform (desig:an action 
-(type positioning-arm) +                                          (type positioning-arm) 
-(left-configuration park) +                                          (left-configuration park) 
-(right-configuration park))) +                                          (right-configuration park))) 
-(cpl:retry)) +                   (cpl:retry)) 
-(roslisp:ros-warn (fd-plans fetch) "No more regrasping retries left :'(") +                 (roslisp:ros-warn (fd-plans fetch) "No more regrasping retries left :'(") 
-(cpl:fail 'common-fail:object-unreachable +                 (cpl:fail 'common-fail:object-unreachable 
-:description "Misgrasp happened and retrying didn't help.")))+                           :description "Misgrasp happened and retrying didn't help.")))
 </code> </code>
  
Line 726: Line 740:
  
 <code lisp> <code lisp>
-(let ((?more-precise-perceived-object-desig + (let ((?more-precise-perceived-object-desig 
-(exe:perform (desig:an action +                    (exe:perform (desig:an action 
-(type detecting) +                                           (type detecting) 
-(object ?object-designator)))))+                                           (object ?object-designator)))))
 </code> </code>
  
Line 735: Line 749:
  
 <code lisp> <code lisp>
-(let ((?arm (cut:lazy-car ?arms))) + (let ((?arm (cut:lazy-car ?arms))) 
-;; if picking up fails, try another arm +                ;; if picking up fails, try another arm 
-(cpl:with-retry-counters ((arm-retries 1)) +                (cpl:with-retry-counters ((arm-retries 1)) 
-(cpl:with-failure-handling +                  (cpl:with-failure-handling 
-(((or common-fail:manipulation-low-level-failure +                      (((or common-fail:manipulation-low-level-failure 
-common-fail:object-unreachable +                            common-fail:object-unreachable 
-desig:designator-error) (e) +                            desig:designator-error) (e) 
-(common-fail:retry-with-list-solutions +                         (common-fail:retry-with-list-solutions 
-?arms +                             ?arms 
-arm-retries +                             arm-retries 
-(:error-object-or-string +                             (:error-object-or-string 
-(format NIL "Manipulation failed: ~a.~%Next." e) +                              (format NIL "Manipulation failed: ~a.~%Next." e) 
-:warning-namespace (kvr plans) +                              :warning-namespace (kvr plans) 
-:rethrow-failure 'common-fail:object-unreachable) +                              :rethrow-failure 'common-fail:object-unreachable) 
-(setf ?arm (cut:lazy-car ?arms)))))+                           (setf ?arm (cut:lazy-car ?arms)))))
  
-(let ((?grasp (cut:lazy-car ?grasps))) +                    (let ((?grasp (cut:lazy-car ?grasps))) 
-;; if picking up fails, try another grasp orientation +                      ;; if picking up fails, try another grasp orientation 
-(cpl:with-retry-counters ((grasp-retries 4)) +                      (cpl:with-retry-counters ((grasp-retries 4)) 
-(cpl:with-failure-handling +                        (cpl:with-failure-handling 
-(((or common-fail:manipulation-low-level-failure +                            (((or common-fail:manipulation-low-level-failure 
-common-fail:object-unreachable +                                  common-fail:object-unreachable 
-desig:designator-error) (e) +                                  desig:designator-error) (e) 
-(common-fail:retry-with-list-solutions +                               (common-fail:retry-with-list-solutions 
-?grasps +                                   ?grasps 
-grasp-retries +                                   grasp-retries 
-(:error-object-or-string +                                   (:error-object-or-string 
-(format NIL "Picking up failed: ~a.~%Next" e) +                                    (format NIL "Picking up failed: ~a.~%Next" e) 
-:warning-namespace (kvr plans)) +                                    :warning-namespace (kvr plans)) 
-(setf ?grasp (cut:lazy-car ?grasps)))))+                                 (setf ?grasp (cut:lazy-car ?grasps)))))
 </code> </code>
  
-To allow the robotor to access the object with both arms and all possible grasps, the lists ''arms'' and ''grasps'' will be traversed like explained [[http://cram-system.org/tutorials/intermediate/simple_mobile_manipulation_plan#increasing_the_effectiveness_by_improving_the_plan|here]].+To allow the robot to access the object with both arms and all possible grasps, the lists ''arms'' and ''grasps'' will be traversed like explained [[http://cram-system.org/tutorials/intermediate/simple_mobile_manipulation_plan#increasing_the_effectiveness_by_improving_the_plan|here]].
  
 <code lisp> <code lisp>
 (let ((pick-up-action (let ((pick-up-action
-;; if pick-up-action already exists, +                                  ;; if pick-up-action already exists, 
-;; use its params for picking up +                                  ;; use its params for picking up 
-(or (when pick-up-action +                                  (or (when pick-up-action 
-(let* ((referenced-action-desig +                                        (let* ((referenced-action-desig 
-(desig:reference pick-up-action)) +                                                 (desig:reference pick-up-action)) 
-(?arm +                                               (?arm 
-(desig:desig-prop-value +                                                 (desig:desig-prop-value 
-referenced-action-desig +                                                  referenced-action-desig 
-:arm)) +                                                  :arm)) 
-(?grasp +                                               (?grasp 
-(desig:desig-prop-value +                                                 (desig:desig-prop-value 
-referenced-action-desig +                                                  referenced-action-desig 
-:grasp))) +                                                  :grasp))) 
-(desig:an action +                                          (desig:an action 
-(type picking-up) +                                                    (type picking-up) 
-(arm ?arm) +                                                    (arm ?arm) 
-(grasp ?grasp) +                                                    (grasp ?grasp) 
-(object +                                                    (object 
-?more-precise-perceived-object-desig)))) +                                                     ?more-precise-perceived-object-desig))))
-(desig:an action +
-(type picking-up) +
-(desig:when ?arm +
-(arm ?arm)) +
-(desig:when ?grasp +
-(grasp ?grasp)) +
-(object +
-?more-precise-perceived-object-desig)))))+
 </code> </code>
  
Line 817: Line 823:
  
 <code lisp> <code lisp>
-(exe:perform (desig:an action + (exe:perform (desig:an action 
-(type positioning-arm) +                                                   (type positioning-arm) 
-(left-configuration park) +                                                   (left-configuration park) 
-(right-configuration park))) +                                                   (right-configuration park))) 
-(desig:current-desig ?object-designator)))))))))))))))+                            (desig:current-desig ?object-designator)))))))))))))))
 </code> </code>
  
Line 831: Line 837:
  
 **deliver** **deliver**
 +
 <code lisp> <code lisp>
 (defun deliver (&key (defun deliver (&key
-((:object ?object-designator)) +                  ((:object ?object-designator)) 
-((:arm ?arm)) +                  ((:arm ?arm)) 
-((:target ?target-location)) +                  ((:target ?target-location)) 
-((:robot-location ?target-robot-location)) +                  ((:robot-location ?target-robot-location)) 
-place-action +                  place-action 
-&allow-other-keys) +                &allow-other-keys) 
-(declare (type desig:object-designator ?object-designator) +  (declare (type desig:object-designator ?object-designator) 
-(type (or keyword null) ?arm) +           (type (or keyword null) ?arm) 
-;; don't pass NULL as ?target-location or ?target-robot-location! +           ;; don't pass NULL as ?target-location or ?target-robot-location! 
-;; they can turn NULL during execution but not at the beginning +           ;; they can turn NULL during execution but not at the beginning 
-(type (or desig:location-designator null) ?target-location ?target-robot-location) +           (type (or desig:location-designator null) ?target-location ?target-robot-location) 
-(type (or desig:action-designator null) place-action)) +           (type (or desig:action-designator null) place-action)) 
-"Delivers `?object-designator' to `?target-location', where object is held in `?arm'+  "Delivers `?object-designator' to `?target-location', where object is held in `?arm'
 and the robot should stand at `?target-robot-location' when placing the object. and the robot should stand at `?target-robot-location' when placing the object.
 If a failure happens, try a different `?target-location' or `?target-robot-location'." If a failure happens, try a different `?target-location' or `?target-robot-location'."
 </code> </code>
 +The documentation of this method describes the input parameters and in the declare section you see which types the designators have.
 <code lisp> <code lisp>
-;; Reference the `?target-location' to see if that works at all +  ;; Reference the `?target-location' to see if that works at all 
-;; If not, delivering is impossible so throw a OBJECT-UNDERLIVERABLE failure +  ;; If not, delivering is impossible so throw a OBJECT-UNDERLIVERABLE failure 
-(cpl:with-failure-handling +  (cpl:with-failure-handling 
-((desig:designator-error (e) +      ((desig:designator-error (e) 
-(roslisp:ros-warn (fd-plans deliver) "~a~%Propagating up." e) +         (roslisp:ros-warn (fd-plans deliver) "~a~%Propagating up." e) 
-(cpl:fail 'common-fail:object-undeliverable +         (cpl:fail 'common-fail:object-undeliverable 
-:description "Some designator could not be resolved.")))+                   :description "Some designator could not be resolved.")))
 </code> </code>
  
-If the resolving from a designator fails we throw the above error.+If the resolving from a designator below fails we throw the above error.
  
 <code lisp> <code lisp>
-(cpl:with-retry-counters ((outer-target-location-retries 2)) +    (cpl:with-retry-counters ((outer-target-location-retries 2)) 
-(cpl:with-failure-handling +      (cpl:with-failure-handling 
-(((or desig:designator-error +          (((or desig:designator-error 
-common-fail:object-undeliverable) (e) +                common-fail:object-undeliverable) (e) 
-(common-fail:retry-with-loc-designator-solutions +             (common-fail:retry-with-loc-designator-solutions 
-?target-location +                 ?target-location 
-outer-target-location-retries +                 outer-target-location-retries 
-(:error-object-or-string +                 (:error-object-or-string 
-(format NIL "Undeliverable. Trying another target location.~%~a" e) +                  (format NIL "Undeliverable. Trying another target location.~%~a" e) 
-:warning-namespace (fd-plans deliver) +                  :warning-namespace (fd-plans deliver) 
-:reset-designators (list ?target-robot-location) +                  :reset-designators (list ?target-robot-location) 
-:rethrow-failure 'common-fail:object-undeliverable))))+                  :rethrow-failure 'common-fail:object-undeliverable))))
 </code> </code>
  
-Again this method uses same thinking as [[playground:search-for-object|playground:search-for-object]] with an 'outer-error-handler' (catching ''common-fail:object-undeliverable'') since we need a new ''target-robot-location'' too after we changed the ''target-location''.+Again this method uses same thinking as [[playground:search-for-object|playground:search-for-object]] with an 'outer-error-handler' (catching ''common-fail:object-undeliverable'') since we need a new ''?target-robot-location'' too after we changed the ''?target-location''.
  
 <code lisp> <code lisp>
 ;; take a new `?target-robot-location' sample if a failure happens ;; take a new `?target-robot-location' sample if a failure happens
-(cpl:with-retry-counters ((relocation-for-ik-retries 4)) +        (cpl:with-retry-counters ((relocation-for-ik-retries 4)) 
-(cpl:with-failure-handling +          (cpl:with-failure-handling 
-(((or common-fail:navigation-goal-in-collision +              (((or common-fail:navigation-goal-in-collision 
-common-fail:object-undeliverable +                    common-fail:object-undeliverable 
-common-fail:manipulation-low-level-failure) (e) +                    common-fail:manipulation-low-level-failure) (e) 
-(common-fail:retry-with-loc-designator-solutions +                 (common-fail:retry-with-loc-designator-solutions 
-?target-robot-location +                     ?target-robot-location 
-relocation-for-ik-retries +                     relocation-for-ik-retries 
-(:error-object-or-string +                     (:error-object-or-string 
-(format NIL "Object is undeliverable from base location.~%~a" e) +                      (format NIL "Object is undeliverable from base location.~%~a" e) 
-:warning-namespace (fd-plans deliver) +                      :warning-namespace (fd-plans deliver) 
-:rethrow-failure 'common-fail:object-undeliverable))))+                      :rethrow-failure 'common-fail:object-undeliverable)))) 
 + 
 +            ;; navigate 
 +            (exe:perform (desig:an action 
 +                                   (type navigating) 
 +                                   (location ?target-robot-location)))
  
-;; navigate 
-(exe:perform (desig:an action 
-(type navigating) 
-(location ?target-robot-location))) 
 </code> </code>
  
Line 919: Line 927:
  (RANGE-INVERT 0.5)>)>   (RANGE-INVERT 0.5)>)> 
 </code> </code>
-As you can see we have again like in [[playground:search-for-object|playground:search-for-object]] a nested location designator. Therefore, cram calls ''get-location-pose'' again with the outer and inner location designator and returns two times a lazy list of poses.+As you can see we have again like in [[playground:search-for-object|playground:search-for-object]] a nested location designator. Therefore, CRAM calls ''get-location-pose'' again with the outer and inner location designator and returns two times a lazy list of poses.
 If the navigation fails, we try four times another robot location where it can place the object. At the fifth retry the delivering method fails with an ''common-fail:navigation-goal-in-collision'' or ''common-fail:manipulation-low-level-failure'' error. The ''common-fail:common-fail:object-undeliverable'' error will be thrown from below code snippet. If the navigation fails, we try four times another robot location where it can place the object. At the fifth retry the delivering method fails with an ''common-fail:navigation-goal-in-collision'' or ''common-fail:manipulation-low-level-failure'' error. The ''common-fail:common-fail:object-undeliverable'' error will be thrown from below code snippet.
 If everything goes as intended the robot will stay in front of his target robot location like seen here. If everything goes as intended the robot will stay in front of his target robot location like seen here.
Line 928: Line 936:
  
 <code lisp> <code lisp>
-;; take a new `?target-location' sample if a failure happens + ;; take a new `?target-location' sample if a failure happens 
-(cpl:with-retry-counters ((target-location-retries 9)) +            (cpl:with-retry-counters ((target-location-retries 9)) 
-(cpl:with-failure-handling +              (cpl:with-failure-handling 
-(((or common-fail:looking-high-level-failure +                  (((or common-fail:looking-high-level-failure 
-common-fail:object-unreachable +                        common-fail:object-unreachable 
-common-fail:high-level-failure) (e) +                        common-fail:high-level-failure) (e) 
-(common-fail:retry-with-loc-designator-solutions +                     (common-fail:retry-with-loc-designator-solutions 
-?target-location +                         ?target-location 
-target-location-retries +                         target-location-retries 
-(:error-object-or-string (format NIL "Placing failed: ~a" e) +                         (:error-object-or-string (format NIL "Placing failed: ~a" e) 
-:warning-namespace (fd-plans deliver) +                          :warning-namespace (fd-plans deliver) 
-:reset-designators (list ?target-robot-location) +                          :reset-designators (list ?target-robot-location) 
-:rethrow-failure 'common-fail:object-undeliverable) +                          :rethrow-failure 'common-fail:object-undeliverable) 
-(roslisp:ros-warn (fd-plans deliver) +                       (roslisp:ros-warn (fd-plans deliver) 
-"Retrying with new placing location ...~%"))))+                                         "Retrying with new placing location ...~%"))))
  
-;; look +                ;; look 
-(exe:perform (desig:an action +                (exe:perform (desig:an action 
-(type turning-towards) +                                       (type turning-towards) 
-(target ?target-location)))+                                       (target ?target-location)))
 </code> </code>
  
Line 959: Line 967:
  
 <code lisp> <code lisp>
-;; place + ;; place 
-(let ((place-action +                (let ((place-action 
-(or (when place-action +                        (or (when place-action 
-(let* ((referenced-action-desig +                              (let* ((referenced-action-desig 
-(desig:reference place-action)) +                                       (desig:reference place-action)) 
-(?arm +                                     (?arm 
-(desig:desig-prop-value referenced-action-desig :arm)) +                                       (desig:desig-prop-value referenced-action-desig :arm)) 
-(?projected-target-location +                                     (?projected-target-location 
-(desig:desig-prop-value referenced-action-desig :target))) +                                       (desig:desig-prop-value referenced-action-desig :target))) 
-(desig:an action +                                (desig:an action 
-(type placing) +                                          (type placing) 
-(arm ?arm) +                                          (arm ?arm) 
-(object ?object-designator) +                                          (object ?object-designator) 
-(target ?projected-target-location)))) +                                          (target ?projected-target-location)))) 
-(desig:an action +                            (desig:an action 
-(type placing) +                                      (type placing) 
-(desig:when ?arm +                                      (desig:when ?arm 
-(arm ?arm)) +                                        (arm ?arm)) 
-(object ?object-designator) +                                      (object ?object-designator) 
-(target ?target-location)))))+                                      (target ?target-location)))))
 </code> </code>
  
Line 1000: Line 1008:
 ;; test function throws a high-level-failure if not good pose ;; test function throws a high-level-failure if not good pose
 (proj-reasoning:check-placing-pose-stability (proj-reasoning:check-placing-pose-stability
-?object-designator ?target-location)+ ?object-designator ?target-location)
 </code>  </code> 
  
Line 1010: Line 1018:
 (TYPE PLACING) (TYPE PLACING)
 (OBJECT #<A OBJECT (OBJECT #<A OBJECT
-(LOCATION #<A LOCATION + (LOCATION #<A LOCATION 
-(ON #<A OBJECT + (ON #<A OBJECT 
-(TYPE COUNTER-TOP) + (TYPE COUNTER-TOP) 
-(URDF-NAME SINK-AREA-SURFACE) + (URDF-NAME SINK-AREA-SURFACE) 
-(OWL-NAME "kitchen_sink_block_counter_top") + (OWL-NAME "kitchen_sink_block_counter_top") 
-(PART-OF KITCHEN)>) + (PART-OF KITCHEN)>) 
-(SIDE LEFT) + (SIDE LEFT) 
-(SIDE FRONT) + (SIDE FRONT) 
-(RANGE-INVERT 0.5)>) + (RANGE-INVERT 0.5)>) 
-(TYPE BOWL) + (TYPE BOWL) 
-(NAME BOWL-1) + (NAME BOWL-1) 
-(POSE ((:POSE+ (POSE ((:POSE
 #<CL-TRANSFORMS-STAMPED:POSE-STAMPED  #<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
 FRAME-ID: "base_footprint", STAMP: 1.560941941216607d9 FRAME-ID: "base_footprint", STAMP: 1.560941941216607d9
Line 1041: Line 1049:
 #<3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)> #<3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>
 #<QUATERNION (-0.005307710729539394d0 -0.00298106437548995d0 5.163229070603848d-4 0.9999813437461853d0)>>)))>) #<QUATERNION (-0.005307710729539394d0 -0.00298106437548995d0 5.163229070603848d-4 0.9999813437461853d0)>>)))>)
-(TARGET #<A LOCATION + (TARGET #<A LOCATION 
-(ON #<A OBJECT + (ON #<A OBJECT 
-(TYPE COUNTER-TOP) + (TYPE COUNTER-TOP) 
-(URDF-NAME KITCHEN-ISLAND-SURFACE) + (URDF-NAME KITCHEN-ISLAND-SURFACE) 
-(OWL-NAME "kitchen_island_counter_top") + (OWL-NAME "kitchen_island_counter_top") 
-(PART-OF KITCHEN)>) + (PART-OF KITCHEN)>) 
-(CONTEXT TABLE-SETTING) + (CONTEXT TABLE-SETTING) 
-(FOR #<A OBJECT + (FOR #<A OBJECT 
-(TYPE BOWL)>) + (TYPE BOWL)>) 
-(OBJECT-COUNT 3) + (OBJECT-COUNT 3) 
-(SIDE BACK) + (SIDE BACK) 
-(SIDE RIGHT) + (SIDE RIGHT) 
-(RANGE-INVERT 0.5)>)>  + (RANGE-INVERT 0.5)>)> 
-(exe:perform place-action))))))))))+
 </code> </code>
 +
 After all this we can finally execute the place action. After all this we can finally execute the place action.
 +
 +<code lisp>
 +(exe:perform place-action))))))))))
 +</code>
  
 {{playground:delivered.png?direct&600|}} {{playground:delivered.png?direct&600|}}