Differences

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

Link to this comparison view

Both sides previous revision Previous revision
Next revision
Previous revision
playground:playground [2019/07/10 10:32]
tlipps
playground:playground [2019/01/06 09:29] (current)
Line 1: Line 1:
-**Learning about the transport function with an example** +====== PlayGround ======
- +
-The transport function is a high order function which will use most of the functions in ''​cram_fetch_deliver_plans''​. Therefore we'll explain this function with an example and show the below function calls. Our goal with the PR2 is to put the bowl from the sink area on the table. To explain the other functions we're gonna follow the execution of the other high level functions in the deliver function. +
-We do this by loading the packages ''​cram_pr2_pick_and_place''​ and ''​cram_bullet_world_tutorial''​ ... +
-<code lisp> +
-(ros-load:​load-system "​cram_pr2_pick_and_place"​ :​cram_pr2_pick_and_place) +
-(ros-load:​load-system "​cram_bullet_world_tutorial"​ :​cram_bullet_world_tutorial) +
-</​code>​ +
-... and call the following functions:​ +
-<​code>​ +
-(in-package :​cram-bullet-world-tutorial) +
-(roslisp-utilities:​startup-ros) +
-(urdf-proj:​with-projected-robot  +
- (cram-pr2-pick-place-demo::​demo-random nil '​(:​bowl))) +
-</​code>​ +
-After that the robot gets correctly positioned and kitchen objects spawned before starting. +
- +
-{{playground:​start.png?​500|}} +
- +
-Since we passed ''​(demo-random)''​ the object of type bowl the actions searching and transporting will be executed: +
-<code lisp> +
-(when (eq ?​object-type :bowl) +
-  (cpl:​with-failure-handling +
-    ((common-fail:​high-level-failure (e) +
-      (roslisp:​ros-warn (pp-plans demo) "​Failure happened: ~a~%Skipping the search"​ e) +
-      (return))) +
-    (let ((?loc (cdr (assoc :​breakfast-cereal object-fetching-locations)))) +
-      (exe:​perform +
-        (desig:an action +
-                  (type searching) +
-                  (object (desig:an object (type breakfast-cereal))) +
-                  (location ?​loc)))))) +
- +
-(cpl:​with-failure-handling +
-    ((common-fail:​high-level-failure (e) +
-        (roslisp:​ros-warn (pp-plans demo) "​Failure happened: ~a~%Skipping..."​ e) +
-        (return))) +
-  (if (eq ?​object-type :bowl) +
-      (exe:​perform +
-       ​(desig:​an action +
-                 (type transporting) +
-                 ​(object ?​object-to-fetch) +
-                 ​(location ?​fetching-location) +
-                 ​(target ?​delivering-location))) +
-</​code>​ +
- +
-If any of the actions fails, we just skip them. Since in the code the searching actions is executed first, let's take a look at it. +
- +
-**search-for-object** +
- +
-<code lisp> +
-(defun search-for-object (&key +
-                           ​((:​object ?​object-designator)) +
-                           ​((:​location ?​search-location)) +
-                           ​((:​robot-location ?​robot-location)) +
-                           ​(retries 3) +
-                          &​allow-other-keys) +
-(declare (type desig:​object-designator ?​object-designator) +
-;; location desigs can turn NILL in the course of execution +
-;; but should not be passed as NILL to start with. +
-(type (or desig:​location-designator null) ?​search-location ?​robot-location)) +
-"​Searches for `?​object-designator'​ in its likely location `?​search-location'​. +
-If the object is not there or navigation location is unreachable,​ +
-retries with different search location or robot base location."​ +
-</​code>​ +
- +
-The input parameters are the object which should be searched from the location ''?​robot-location''​ at the location ''?​search-location''​. ''​retries''​ says how often we should try again to find a object at a specific robot location. +
- +
-<code lisp> +
-(cpl:​with-failure-handling +
-    ((desig:​designator-error (e) +
-      (roslisp:​ros-warn (fd-plans search-for-object) +
-                        "Desig ~a could not be resolved: ~a~%Propagating up." +
-                        ?​search-location e) +
-      (cpl:fail '​common-fail:​object-nowhere-to-be-found +
-                :​description "​Search location designator could not be resolved."​))) +
-</​code>​ +
- +
-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 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. +
- +
-<code lisp> +
-;; take new `?​search-location'​ sample if a failure happens and retry +
-(cpl:​with-retry-counters ((outer-search-location-retries 2)) +
-  (cpl:​with-failure-handling +
-    ((common-fail:​object-nowhere-to-be-found (e) +
-      (common-fail:​retry-with-loc-designator-solutions +
-          ?​search-location +
-          outer-search-location-retries +
-          (:​error-object-or-string e +
-          :​warning-namespace (fd-plans search-for-object) +
-          :​reset-designators (list ?​robot-location) +
-          :​rethrow-failure '​common-fail:​object-nowhere-to-be-found) +
-       ​(roslisp:​ros-warn (fd-plans search-for-object) +
-                         "​Search is about to give up. Retrying~%"​)))) +
-</​code>​ +
- +
-So here we set a new ''?​search-location''​ since we could not move to robot or/and find detect the object. +
- +
-<code lisp> +
-;; if the going action fails, pick another `?​robot-location'​ sample and retry +
-(cpl:​with-retry-counters ((robot-location-retries 10)) +
-  (cpl:​with-failure-handling +
-      (((or common-fail:​navigation-goal-in-collision +
-            common-fail:​looking-high-level-failure +
-            common-fail:​perception-low-level-failure) (e) +
-         ​(common-fail:​retry-with-loc-designator-solutions +
-             ?​robot-location +
-             ​robot-location-retries +
-             ​(:​error-object-or-string e +
-              :​warning-namespace (fd-plans search-for-object) +
-              :​reset-designators (list ?​search-location) +
-              :​rethrow-failure '​common-fail:​object-nowhere-to-be-found)))) +
- +
-     ;; navigate +
-     ​(exe:​perform (desig:an action +
-                            (type navigating) +
-                            (location ?​robot-location))) +
-</​code>​ +
- +
-Here we try to navigate the robot to ''?​robot-location''​ and if it fails we try ''​robot-location-retries''​ times again before throwing ''​object-nowhere-to-be-found''​ and jump in the above code snippet. To understand how this action will be resolved we take a look at an example action:  +
- +
-<code lisp> +
-#<A ACTION +
- (TYPE NAVIGATING) +
- (LOCATION #<A LOCATION +
- (VISIBLE-FOR PR2) +
- (LOCATION #<A LOCATION +
- (ON #<A OBJECT +
- (TYPE COUNTER-TOP) +
- (URDF-NAME SINK-AREA-SURFACE) +
- (OWL-NAME kitchen_sink_block_counter_top) +
- (PART-OF KITCHEN)>​) +
- (SIDE LEFT) +
- (SIDE FRONT) +
- (RANGE 0.5)>​)>​)>​ +
-</​code>​ +
- +
-To resolve the nested location designator ''?​robot-location''​ in this action the function ''​cram_manipulation_interfaces::​get-location-poses''​ will be called twice with the following location designators:​ +
- +
-<code lisp> +
-#<A LOCATION +
-(VISIBLE-FOR PR2) +
-(LOCATION #<A LOCATION +
- (ON #<A OBJECT +
- (TYPE COUNTER-TOP) +
- (URDF-NAME SINK-AREA-SURFACE) +
- (OWL-NAME "​kitchen_sink_block_counter_top"​) +
- (PART-OF KITCHEN)>​) +
- (SIDE LEFT) +
- (SIDE FRONT) +
- (RANGE 0.5)>​)>​  +
-</​code>​ +
- +
-This designator is a subset of the action designator above and ... +
- +
-<code lisp> +
-#<A LOCATION +
-(ON #<A OBJECT +
- (TYPE COUNTER-TOP) +
- (URDF-NAME SINK-AREA-SURFACE) +
- (OWL-NAME "​kitchen_sink_block_counter_top"​) +
- (PART-OF KITCHEN)>​) +
-(SIDE LEFT) +
-(SIDE FRONT) +
-(RANGE 0.5)> +
-</​code>​ +
- +
-this location designator is the subset of the above location designator. Since we have two location designators to resolve the function ''​cram_manipulation_interfaces::​get-location-poses''​ will be called twice returning both times a lazy list of poses: +
- +
-<code lisp> +
-(#<​CL-TRANSFORMS:​POSE  +
-#<​3D-VECTOR (1.5d0 0.9000000953674316d0 0.8500000258286794d0)>​ +
-#<​QUATERNION (0.0d0 0.0d0 0.9084313017742992d0 0.41803417319239833d0)>>​ +
-. #​S(CRAM-UTILITIES::​LAZY-CONS-ELEM +
-:GENERATOR #<​CLOSURE (LAMBDA () :IN CRAM-UTILITIES:​LAZY-MAPCAN) +
-{1018B51EBB}>​))  +
-</​code>​ +
- +
-Since the type of the action is ''​navigating''​ the function ''​cram_fetch_deliver_plans::​go-without-collisions''​ can be evaluated with the above pose. +
-The procedure can be visualized with the btr too: +
- +
-{{playground:​search-for-obj_robot_loc_object_location_resolved.png?​600|}} +
- +
-This is the costmap after we called get-location-poses the first time only with the object designator. +
- +
-{{playground:​search-for-obj-robot-location-fully-resolved.png?​600|}} +
- +
-This is the costmap after we called the nested location designator with get-location-poses. +
- +
-{{playground:​search-for-robot-moved.png?​600|}} +
- +
-This is after the execution of the navigation action designator. +
- +
- +
-<code lisp> +
-;; if perception action fails, try another `?​search-location'​ and retry +
-(cpl:​with-retry-counters ((search-location-retries retries)) +
-  (cpl:​with-failure-handling +
-      (((or common-fail:​perception-low-level-failure +
-            common-fail:​looking-high-level-failure) (e) +
-         ​(common-fail:​retry-with-loc-designator-solutions +
-             ?​search-location +
-             ​search-location-retries +
-             ​(:​error-object-or-string e +
-              :​warning-namespace (fd-plans search-for-object) +
-              :​reset-designators (list ?​robot-location))))) +
- +
-       ​(exe:​perform (desig:an action +
-                              (type turning-towards) +
-                              (target ?​search-location))) +
-       ​(exe:​perform (desig:an action +
-                              (type detecting) +
-                              (object ?​object-designator))))))))))) +
-</​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. +
-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.: +
- +
-<code lisp> +
-#<A OBJECT +
-(LOCATION #<A LOCATION +
- (ON #<A 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)>) +
-(TYPE BOWL) +
-(NAME BOWL-1) +
-(POSE ((:POSE +
-#<​CL-TRANSFORMS-STAMPED:​POSE-STAMPED  +
-FRAME-ID: "​base_footprint",​ STAMP: 1.560865249078827d9 +
-#<​3D-VECTOR (0.8618418535601576d0 0.028947115308607385d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.004970445237876797d0 -0.0035147137040045353d0 0.10407392205337866d0 0.994550963155759d0)>>​) +
-(:​TRANSFORM +
-#<​CL-TRANSFORMS-STAMPED:​TRANSFORM-STAMPED  +
-FRAME-ID: "​base_footprint",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865249078827d9 +
-#<​3D-VECTOR (0.8618418535601576d0 0.028947115308607385d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.004970445237876797d0 -0.0035147137040045353d0 0.10407392205337866d0 0.994550963155759d0)>>​) +
-(:​POSE-IN-MAP +
-#<​CL-TRANSFORMS-STAMPED:​POSE-STAMPED  +
-FRAME-ID: "​map",​ STAMP: 1.560865249078827d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​) +
-(:​TRANSFORM-IN-MAP +
-#<​CL-TRANSFORMS-STAMPED:​TRANSFORM-STAMPED  +
-FRAME-ID: "​map",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865249078827d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​)))>​  +
-</​code>​ +
- +
-**Transport** +
- +
-<code lisp> +
-(defun transport (&key +
-((:object ?​object-designator)) +
-((:​search-location ?​search-location)) +
-((:​search-robot-location ?​search-base-location)) +
-((:​fetch-robot-location ?​fetch-robot-location)) +
-((:arm ?arm)) +
-((:grasp ?grasp)) +
-((:arms ?arms)) +
-((:grasps ?grasps)) +
-((:​deliver-location ?​delivering-location)) +
-((:​deliver-robot-location ?​deliver-robot-location)) +
-search-location-accessible +
-delivery-location-accessible +
-&​allow-other-keys) +
-</​code>​ +
-text... +
-<code lisp> +
-(unless search-location-accessible +
-(exe:​perform (desig:an action +
-(type accessing) +
-(location ?​search-location) +
-(distance 0.3)))) +
-</​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 +
-(let ((?​perceived-object-designator +
-(exe:​perform (desig:an action +
-(type searching) +
-(object ?​object-designator) +
-(location ?​search-location) +
-(desig:when ?​search-base-location +
-(robot-location ?​search-base-location))))) +
-</​code>​ +
-To fetch the object the object first has to be searched. For this we create +
-and resolve an action designator of type searching with set object-designator,​ +
-the robot-location if available and the search-location like for the bowl e. g. this: +
-<code lisp> +
-(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 +
-(setf ?​fetch-robot-location +
-(desig:a location +
-(reachable-for ?​robot-name) +
-(desig:when ?arm +
-(arm ?arm)) +
-(object ?​perceived-object-designator)))) +
-(unless ?​deliver-robot-location +
-(setf ?​deliver-robot-location +
-(desig:a location +
-(reachable-for ?​robot-name) +
-(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. +
- +
-<code lisp> +
-;; If running on the real robot, execute below task tree in projection +
-;; N times first, then pick the best parameterization +
-;; and use that parameterization in the real world. +
-;; If running in projection, just execute the task tree below as normal. +
-(let (?​fetch-pick-up-action ?​deliver-place-action) +
-(proj-reasoning:​with-projected-task-tree +
-(?​fetch-robot-location ?​fetch-pick-up-action +
-?​deliver-robot-location ?​deliver-place-action) +
-+
-#'​proj-reasoning:​pick-best-parameters-by-distance +
- +
-(let ((?​fetched-object +
-(exe:​perform (desig:an action +
-(type fetching) +
-(desig:when ?arm +
-(arm ?arm)) +
-(desig:when ?grasp +
-(grasp ?grasp)) +
-(desig:when ?arms +
-(arms ?arms)) +
-(desig:when ?grasps +
-(grasps ?grasps)) +
-(object ?​perceived-object-designator) +
-(robot-location ?​fetch-robot-location) +
-(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. +
- +
-<code lisp> +
-(unless delivery-location-accessible +
-(exe:​perform (desig:an action +
-(type accessing) +
-(location ?​delivering-location) +
-(distance 0.3)))) +
-(unwind-protect +
-(exe:​perform (desig:an action +
-(type delivering) +
-(desig:when ?arm +
-(arm ?arm)) +
-(object ?​fetched-object) +
-(target ?​delivering-location) +
-(robot-location ?​deliver-robot-location) +
-(place-action ?​deliver-place-action))) +
-(unless delivery-location-accessible +
-(exe:​perform (desig:an action +
-(type sealing) +
-(location ?​delivering-location) +
-(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''​. ​  +
- +
-<code lisp> +
-(unless search-location-accessible +
-(exe:​perform (desig:an action +
-(type sealing) +
-(location ?​search-location) +
-(distance 0.3)))))) +
-</​code>​ +
- +
-At the end we seal the e. g. drawer we got our fork from with a specific distance towards the drawer. +
- +
-**go-without-collisions** +
- +
-<code lisp> +
-(defun go-without-collisions (&key +
-                                ((:location ?​navigation-location)) +
-                              &​allow-other-keys) +
-  (declare (type desig:​location-designator ?​navigation-location)) +
-  "Check if navigation goal is in reach, if not propagate failure up, +
-if yes, perform GOING action while ignoring failures."​ +
- +
-  (exe:​perform (desig:an action +
-                         (type positioning-arm) +
-                         ​(left-configuration park) +
-                         ​(right-configuration park))) +
-</​code>​ +
- +
-First we move the arms in the park position, so they do not hit anything on their way. +
- +
-<code lisp> +
-(proj-reasoning:​check-navigating-collisions ?​navigation-location) +
-</​code>​ +
- +
-Then we check if the requested location is in collision with the bullet world. +
- +
-<code lisp> +
-(setf ?​navigation-location (desig:​current-desig ?​navigation-location)) +
- +
-  (cpl:​with-failure-handling +
-      ((common-fail:​navigation-low-level-failure (e) +
-         ​(roslisp:​ros-warn (pp-plans navigate) +
-                           "​Low-level navigation failed: ~a~%.Ignoring anyway."​ e) +
-         ​(return))) +
-    (exe:​perform (desig:an action +
-                           (type going) +
-                           ​(target ?​navigation-location))))) +
-</​code>​ +
- +
-Then we try to go with the action designator to the desired location, if it fails, because it was an ''​navigation-low-level-failure'',​ we print a message and ignore it by returning. +
- +
- +
-**turn-towards** +
-<code lisp> +
-(defun turn-towards (&key +
-                       ​((:​target ?​look-target)) +
-                       ​((:​robot-location ?​robot-location)) +
-                     &​allow-other-keys) +
-  (declare (type desig:​location-designator ?​look-target ?​robot-location)) +
-  "​Perform a LOOKING action, if looking target twists the neck, +
-turn the robot base such that it looks in the direction of target and look again."​ +
-</​code>​ +
- +
-As input parameters we get the location where the robot should look and where it should stand for doing this. So both parameters have the type of location designator. +
- +
-<code lisp> +
- ​(cpl:​with-failure-handling +
-      ((desig:​designator-error (e) +
-         ​(roslisp:​ros-warn (fd-plans turn-towards) +
-                           "​Desig ~a could not be resolved: ~a~%Cannot look."​ +
-                           ?​look-target e) +
-         ​(cpl:​fail '​common-fail:​looking-high-level-failure)) +
- +
-       ​(common-fail:​navigation-high-level-failure (e) +
-         ​(roslisp:​ros-warn (fd-plans turn-towards) +
-                           "​When turning around navigation failure happened: ~a~%~ +
-                              Cannot look."​ +
-                           e) +
-         ​(cpl:​fail '​common-fail:​looking-high-level-failure))) +
-</​code>​ +
- +
-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> +
- ​(cpl:​with-retry-counters ((turn-around-retries 1)) +
-      (cpl:​with-failure-handling +
-          ((common-fail:​ptu-low-level-failure (e) +
-             ​(roslisp:​ros-warn (pp-plans turn-towards) "​~a~%Turning around."​ e) +
-             ​(cpl:​do-retry turn-around-retries +
-               ​(cpl:​par +
-                 ​(exe:​perform (desig:an action +
-                                        (type navigating) +
-                                        (location ?​robot-location))) +
-                 ​(exe:​perform (desig:an action +
-                                        (type looking) +
-                                        (direction forward)))) +
-               ​(cpl:​retry)) +
-             ​(roslisp:​ros-warn (pp-plans turn-towards) "​Turning around didn't work :'​(~%"​) +
-             ​(cpl:​fail '​common-fail:​looking-high-level-failure))) +
-</​code>​ +
- +
-Now the robot tries with  +
-<code lisp> +
- ​(exe:​perform (desig:an action +
-                               (type looking) +
-                               ​(target ?​look-target))))))) +
-</​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. +
- +
-**fetch** +
- +
-<code lisp> +
-(defun fetch (&key +
-                ((:object ?​object-designator)) +
-                ((:arms ?arms)) +
-                ((:grasps ?grasps)) +
-                ((:​robot-location ?​pick-up-robot-location)) +
-                pick-up-action +
-              &​allow-other-keys) +
-  (declare (type desig:​object-designator ?​object-designator) +
-           (type list ?arms ?grasps) +
-           ;; ?​pick-up-robot-location should not be NULL at the beginning +
-           ;; but can become NULL during execution of the plan +
-           (type (or desig:​location-designator null) ?​pick-up-robot-location) +
-           (type (or desig:​action-designator null) pick-up-action)) +
-  "​Fetches a perceived object `?​object-designator'​ with +
-one of arms in the `?​arms'​ lazy list (if not NIL) and one of grasps in `?​grasps'​ if not NIL, +
-while standing at `?​pick-up-robot-location'​ +
-and using the grasp and arm specified in `pick-up-action'​ (if not NIL)."​ +
-</​code>​ +
- +
-The documentation of this method describes the input parameters and in the declare section you see which types the designators have. +
- +
-<code lisp> +
-(cpl:​with-failure-handling +
-      ((desig:​designator-error (e) +
-         ​(roslisp:​ros-warn (fd-plans fetch) "​~a~%Propagating up." e) +
-         ​(cpl:​fail '​common-fail:​object-unfetchable +
-                   :​object ?​object-designator +
-                   :​description "Some designator could not be resolved."​))) +
-</​code>​ +
- +
-As always if any of the designators below cannot resolve the error ''​designator-error''​ will be thrown. +
- +
-<code lisp> +
- ;; take a new `?​pick-up-robot-location'​ sample if a failure happens +
-    (cpl:​with-retry-counters ((relocation-for-ik-retries 20)) +
-      (cpl:​with-failure-handling +
-          (((or common-fail:​navigation-goal-in-collision +
-                common-fail:​looking-high-level-failure +
-                common-fail:​perception-low-level-failure +
-                common-fail:​object-unreachable +
-                common-fail:​manipulation-low-level-failure) (e) +
-             ​(common-fail:​retry-with-loc-designator-solutions +
-                 ?​pick-up-robot-location +
-                 ​relocation-for-ik-retries +
-                 ​(:​error-object-or-string +
-                  (format NIL "​Object of type ~a is unreachable:​ ~a" +
-                          (desig:​desig-prop-value ?​object-designator :type) e) +
-                  :​warning-namespace (fd-plans fetch) +
-                  :​rethrow-failure '​common-fail:​object-unfetchable)))) +
-</​code>​ +
- +
-If the navigation, turning or picking up fails, a new location from which the robot should pick up the object will be referenced. If it needs more then ''​relocation-for-ik-retries''​ times a ''​object-unfetchable''​ error will be thrown. +
- +
-<code lisp> +
- ;; navigate, look, detect and pick-up +
-        (exe:​perform (desig:an action +
-                               (type navigating) +
-                               ​(location ?​pick-up-robot-location))) +
-</​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:  +
- +
-<code lisp> +
-#<A ACTION +
-(TYPE NAVIGATING) +
-(LOCATION #<A LOCATION +
- (REACHABLE-FOR PR2) +
- (OBJECT #<A OBJECT +
- (LOCATION #<A LOCATION +
- (ON #<A 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)>) +
- (TYPE BOWL) +
- (NAME BOWL-1) +
-(POSE ((POSE +
-#<​POSE-STAMPED  +
-FRAME-ID: "​base_footprint",​ STAMP: 1.560342793205295d9 +
-#<​3D-VECTOR (0.9527269311937743d0 0.04533943003760865d0 0.8886405270894369d0)>​ +
-#<​QUATERNION (-4.873585828813879d-4 -0.005999954466232327d0 0.8317066648737138d0 0.5551826897033202d0)>>​) +
-(TRANSFORM +
-#<​TRANSFORM-STAMPED  +
-FRAME-ID: "​base_footprint",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560342793205295d9 +
-#<​3D-VECTOR (0.9527269311937743d0 0.04533943003760865d0 0.8886405270894369d0)>​ +
-#<​QUATERNION (-4.873585828813879d-4 -0.005999954466232327d0 0.8317066648737138d0 0.5551826897033202d0)>>​) +
-(POSE-IN-MAP +
-#<​POSE-STAMPED  +
-FRAME-ID: "​map",​ STAMP: 1.560342793205295d9 +
-#<​3D-VECTOR (1.5135019938151042d0 0.625667953491211d0 0.8886405309041341d0)>​ +
-#<​QUATERNION (-0.001493357471190393d0 -0.005831539630889893d0 0.7260335087776184d0 0.6876329779624939d0)>>​) +
-(TRANSFORM-IN-MAP +
-#<​TRANSFORM-STAMPED  +
-FRAME-ID: "​map",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560342793205295d9 +
-#<​3D-VECTOR (1.5135019938151042d0 0.625667953491211d0 0.8886405309041341d0)>​ +
-#<​QUATERNION (-0.001493357471190393d0 -0.005831539630889893d0 0.7260335087776184d0 0.6876329779624939d0)>>​)))>​)>​)>​ +
-</​code>​ +
- +
-Since we already searched the object we now the pose of the object which is therefore in the location designator. Nevertheless,​ we need a pose were the robot can reach the object from. So the location designator needs to be resolved by calling the function ''​cram_manipulation_interfaces::​get-location-poses'',​ which then will return a pose. Here is a picture showing the position and costmap in the bullet world simulation after the execution of the navigation. +
- +
-{{playground:​fetch-after-navigating.png?​600|}} +
- +
- +
-<code lisp> +
-(exe:​perform (desig:an action +
-                               (type turning-towards) +
-                               ​(target (desig:a location (of ?​object-designator))))) +
- +
-</​code>​ +
- +
-The second actions turn the robot towards the location of the object ''?​object-designator''​. For this the following example actions will be created: +
- +
-<code lisp> +
-#<A ACTION +
-(TYPE TURNING-TOWARDS) +
-(TARGET #<A LOCATION +
- (OF #<A OBJECT +
- (LOCATION #<A LOCATION +
- (ON #<A 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)>) +
- (TYPE BOWL) +
- (NAME BOWL-1) +
-(POSE ((POSE +
-#<​POSE-STAMPED  +
-FRAME-ID: "​base_footprint",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (0.6370369976225629d0 -0.19510516177313175d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005410600376438884d0 -0.0027899711686998735d0 -0.0351292935672039d0 0.9993642272387178d0)>>​) +
-(TRANSFORM +
-#<​TRANSFORM-STAMPED  +
-FRAME-ID: "​base_footprint",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (0.6370369976225629d0 -0.19510516177313175d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005410600376438884d0 -0.0027899711686998735d0 -0.0351292935672039d0 0.9993642272387178d0)>>​) +
-(POSE-IN-MAP +
-#<​POSE-STAMPED  +
-FRAME-ID: "​map",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​) +
-(TRANSFORM-IN-MAP +
-#<​TRANSFORM-STAMPED  +
-FRAME-ID: "​map",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​)))>​)>​)>​ +
-</​code>​ +
- +
-And therefore ''​get-location-poses''​ will be called again with: +
- +
-<code lisp> +
-#<A LOCATION +
-(OF #<A OBJECT +
- (LOCATION #<A LOCATION +
- (ON #<A 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)>) +
- (TYPE BOWL) +
- (NAME BOWL-1) +
-(POSE ((:POSE +
-#<​CL-TRANSFORMS-STAMPED:​POSE-STAMPED  +
-FRAME-ID: "​base_footprint",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (0.6370369976225629d0 -0.19510516177313175d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005410600376438884d0 -0.0027899711686998735d0 -0.0351292935672039d0 0.9993642272387178d0)>>​) +
-(:​TRANSFORM +
-#<​CL-TRANSFORMS-STAMPED:​TRANSFORM-STAMPED  +
-FRAME-ID: "​base_footprint",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (0.6370369976225629d0 -0.19510516177313175d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005410600376438884d0 -0.0027899711686998735d0 -0.0351292935672039d0 0.9993642272387178d0)>>​) +
-(:​POSE-IN-MAP +
-#<​CL-TRANSFORMS-STAMPED:​POSE-STAMPED  +
-FRAME-ID: "​map",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​) +
-(:​TRANSFORM-IN-MAP +
-#<​CL-TRANSFORMS-STAMPED:​TRANSFORM-STAMPED  +
-FRAME-ID: "​map",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​)))>​)>​  +
-</​code>​ +
- +
-and returns a lazy list of pose-stamped with which then a looking action can be executed. +
- +
-<code lisp> +
-#<A ACTION +
-(TYPE LOOKING) +
-(TARGET #<A LOCATION +
- (OF #<A OBJECT +
- (LOCATION #<A LOCATION +
- (ON #<A 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)>) +
- (TYPE BOWL) +
- (NAME BOWL-1) +
- (POSE ((POSE +
-#<​POSE-STAMPED  +
-FRAME-ID: "​base_footprint",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (0.6370369976225629d0 -0.19510516177313175d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005410600376438884d0 -0.0027899711686998735d0 -0.0351292935672039d0 0.9993642272387178d0)>>​) +
-(TRANSFORM +
-#<​TRANSFORM-STAMPED  +
-FRAME-ID: "​base_footprint",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (0.6370369976225629d0 -0.19510516177313175d0 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005410600376438884d0 -0.0027899711686998735d0 -0.0351292935672039d0 0.9993642272387178d0)>>​) +
-(POSE-IN-MAP +
-#<​POSE-STAMPED  +
-FRAME-ID: "​map",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​) +
-(TRANSFORM-IN-MAP +
-#<​TRANSFORM-STAMPED  +
-FRAME-ID: "​map",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560865813733836d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>​)))>​)>​)>​ +
-</​code>​ +
- +
-<code lisp> +
- ​(cpl:​with-retry-counters ((regrasping-retries 1)) +
-          (cpl:​with-failure-handling +
-              ((common-fail:​gripper-low-level-failure (e) +
-                 ​(roslisp:​ros-warn (fd-plans fetch) "​Misgrasp happened: ~a~%" e) +
-                 ​(cpl:​do-retry regrasping-retries +
-                   ​(roslisp:​ros-info (fd-plans fetch) "​Reperceiving and repicking..."​) +
-                   ​(exe:​perform (desig:an action +
-                                          (type positioning-arm) +
-                                          (left-configuration park) +
-                                          (right-configuration park))) +
-                   ​(cpl:​retry)) +
-                 ​(roslisp:​ros-warn (fd-plans fetch) "No more regrasping retries left :'​("​) +
-                 ​(cpl:​fail '​common-fail:​object-unreachable +
-                           :​description "​Misgrasp happened and retrying didn't help."​))) +
-</​code>​ +
- +
-Next the failure handling for grasping is written since, if it fails the object should be detected one more time and the different arms and grasps should be tried again. All of these is done afterwards, that's why it's defined here before all that. +
- +
-<code lisp> +
- (let ((?​more-precise-perceived-object-desig +
-                    (exe:​perform (desig:an action +
-                                           (type detecting) +
-                                           ​(object ?​object-designator))))) +
-</​code>​ +
- +
-To get a more precise location of the object the object ''?​object-designator''​ will be detected from the robot. If it fails, a new location ''?​pick-up-robot-location''​ will be referenced like mentioned above. +
- +
-<code lisp> +
- (let ((?arm (cut:​lazy-car ?arms))) +
-                ;; if picking up fails, try another arm +
-                (cpl:​with-retry-counters ((arm-retries 1)) +
-                  (cpl:​with-failure-handling +
-                      (((or common-fail:​manipulation-low-level-failure +
-                            common-fail:​object-unreachable +
-                            desig:​designator-error) (e) +
-                         ​(common-fail:​retry-with-list-solutions +
-                             ?​arms +
-                             ​arm-retries +
-                             ​(:​error-object-or-string +
-                              (format NIL "​Manipulation failed: ~a.~%Next."​ e) +
-                              :​warning-namespace (kvr plans) +
-                              :​rethrow-failure '​common-fail:​object-unreachable) +
-                           (setf ?arm (cut:​lazy-car ?​arms))))) +
- +
-                    (let ((?grasp (cut:​lazy-car ?​grasps))) +
-                      ;; if picking up fails, try another grasp orientation +
-                      (cpl:​with-retry-counters ((grasp-retries 4)) +
-                        (cpl:​with-failure-handling +
-                            (((or common-fail:​manipulation-low-level-failure +
-                                  common-fail:​object-unreachable +
-                                  desig:​designator-error) (e) +
-                               ​(common-fail:​retry-with-list-solutions +
-                                   ?​grasps +
-                                   ​grasp-retries +
-                                   ​(:​error-object-or-string +
-                                    (format NIL "​Picking up failed: ~a.~%Next"​ e) +
-                                    :​warning-namespace (kvr plans)) +
-                                 (setf ?grasp (cut:​lazy-car ?​grasps))))) +
-</​code>​ +
- +
-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> +
-(let ((pick-up-action +
-                                  ;; if pick-up-action already exists, +
-                                  ;; use its params for picking up +
-                                  (or (when pick-up-action +
-                                        (let* ((referenced-action-desig +
-                                                 ​(desig:​reference pick-up-action)) +
-                                               ​(?​arm +
-                                                 ​(desig:​desig-prop-value +
-                                                  referenced-action-desig +
-                                                  :arm)) +
-                                               ​(?​grasp +
-                                                 ​(desig:​desig-prop-value +
-                                                  referenced-action-desig +
-                                                  :grasp))) +
-                                          (desig:an action +
-                                                    (type picking-up) +
-                                                    (arm ?arm) +
-                                                    (grasp ?grasp) +
-                                                    (object +
-                                                     ?​more-precise-perceived-object-desig)))) +
-</​code>​ +
- +
-Then we construct the picking-up action with either the action designator ''​pick-up-action'',​ which was an input parameter or by inserting the arm and grasp values. +
- +
-<code lisp> +
-(setf pick-up-action (desig:​current-desig pick-up-action)) +
-(proj-reasoning:​check-picking-up-collisions pick-up-action) +
-(setf pick-up-action (desig:​current-desig pick-up-action)) +
-</​code>​ +
- +
-First we check if this action is in collision and then +
-  +
-<code lisp> +
-(exe:​perform pick-up-action) +
-</​code>​ +
- +
-we try by executing the action. +
- +
-<code lisp> +
- ​(exe:​perform (desig:an action +
-                                                   (type positioning-arm) +
-                                                   ​(left-configuration park) +
-                                                   ​(right-configuration park))) +
-                            (desig:​current-desig ?​object-designator))))))))))))))) +
-</​code>​ +
- +
-Afterwards call an action to park the arms to end this method and return the object ''?​object-designator''​. +
- +
-{{playground:​fetched-obj.png?&​600|}} +
- +
-The robot fetched the object. +
- +
-**deliver** +
- +
-<code lisp> +
-(defun deliver (&key +
-                  ((:object ?​object-designator)) +
-                  ((:arm ?arm)) +
-                  ((:target ?​target-location)) +
-                  ((:​robot-location ?​target-robot-location)) +
-                  place-action +
-                &​allow-other-keys) +
-  (declare (type desig:​object-designator ?​object-designator) +
-           (type (or keyword null) ?arm) +
-           ;; don't pass NULL as ?​target-location or ?​target-robot-location! +
-           ;; 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:​action-designator null) place-action)) +
-  "​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. +
-If a failure happens, try a different `?​target-location'​ or `?​target-robot-location'​."​ +
-</​code>​ +
-The documentation of this method describes the input parameters and in the declare section you see which types the designators have. +
-<code lisp> +
-  ;; Reference the `?​target-location'​ to see if that works at all +
-  ;; If not, delivering is impossible so throw a OBJECT-UNDERLIVERABLE failure +
-  (cpl:​with-failure-handling +
-      ((desig:​designator-error (e) +
-         ​(roslisp:​ros-warn (fd-plans deliver) "​~a~%Propagating up." e) +
-         ​(cpl:​fail '​common-fail:​object-undeliverable +
-                   :​description "Some designator could not be resolved."​))) +
-</​code>​ +
- +
-If the resolving from a designator below fails we throw the above error. +
- +
-<code lisp> +
-    (cpl:​with-retry-counters ((outer-target-location-retries 2)) +
-      (cpl:​with-failure-handling +
-          (((or desig:​designator-error +
-                common-fail:​object-undeliverable) (e) +
-             ​(common-fail:​retry-with-loc-designator-solutions +
-                 ?​target-location +
-                 ​outer-target-location-retries +
-                 ​(:​error-object-or-string +
-                  (format NIL "​Undeliverable. Trying another target location.~%~a"​ e) +
-                  :​warning-namespace (fd-plans deliver) +
-                  :​reset-designators (list ?​target-robot-location) +
-                  :​rethrow-failure '​common-fail:​object-undeliverable)))) +
-</​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''​. +
- +
-<code lisp> +
-;; take a new `?​target-robot-location'​ sample if a failure happens +
-        (cpl:​with-retry-counters ((relocation-for-ik-retries 4)) +
-          (cpl:​with-failure-handling +
-              (((or common-fail:​navigation-goal-in-collision +
-                    common-fail:​object-undeliverable +
-                    common-fail:​manipulation-low-level-failure) (e) +
-                 ​(common-fail:​retry-with-loc-designator-solutions +
-                     ?​target-robot-location +
-                     ​relocation-for-ik-retries +
-                     ​(:​error-object-or-string +
-                      (format NIL "​Object is undeliverable from base location.~%~a"​ e) +
-                      :​warning-namespace (fd-plans deliver) +
-                      :​rethrow-failure '​common-fail:​object-undeliverable)))) +
- +
-            ;; navigate +
-            (exe:​perform (desig:an action +
-                                   (type navigating) +
-                                   ​(location ?​target-robot-location))) +
- +
-</​code>​ +
- +
-In our example the following location is the target: +
- +
-<code lisp> +
-#<A LOCATION +
- (REACHABLE-FOR PR2) +
- (LOCATION #<A LOCATION +
- (ON #<A OBJECT +
- (TYPE COUNTER-TOP) +
- (URDF-NAME KITCHEN-ISLAND-SURFACE) +
- (OWL-NAME "​kitchen_island_counter_top"​) +
- (PART-OF KITCHEN)>​) +
- (CONTEXT TABLE-SETTING) +
- (FOR #<A OBJECT +
- (TYPE BOWL)>​) +
- (OBJECT-COUNT 3) +
- (SIDE BACK) +
- (SIDE RIGHT) +
- (RANGE-INVERT 0.5)>​)>​  +
-</​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. +
-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. +
- +
-{{playground:​delivering.png?​600|}} +
- +
-The robot moved to the target location in the deliver function. +
- +
-<code lisp> +
- ;; take a new `?​target-location'​ sample if a failure happens +
-            (cpl:​with-retry-counters ((target-location-retries 9)) +
-              (cpl:​with-failure-handling +
-                  (((or common-fail:​looking-high-level-failure +
-                        common-fail:​object-unreachable +
-                        common-fail:​high-level-failure) (e) +
-                     ​(common-fail:​retry-with-loc-designator-solutions +
-                         ?​target-location +
-                         ​target-location-retries +
-                         ​(:​error-object-or-string (format NIL "​Placing failed: ~a" e) +
-                          :​warning-namespace (fd-plans deliver) +
-                          :​reset-designators (list ?​target-robot-location) +
-                          :​rethrow-failure '​common-fail:​object-undeliverable) +
-                       ​(roslisp:​ros-warn (fd-plans deliver) +
-                                         "​Retrying with new placing location ...~%"​)))) +
- +
-                ;; look +
-                (exe:​perform (desig:an action +
-                                       (type turning-towards) +
-                                       ​(target ?​target-location))) +
-</​code>​ +
- +
-In the following picture the robot tries to locate the object within nine retires. +
- +
-{{playground:​put-it-down.png?​600|}} +
- +
-After trying to often the method throws an ''​common-fail:​object-undeliverable''​ error, which will be catched in the '​outer-error-handling'​ snippet mentioned above. Therefore, the the method will then try to navigate again the robot to a new ''?​target-robot-location''​. This means ''​get-location-poses''​ will be called again. +
- +
-{{playground:​retry-rob-loc.png?​600|}} +
- +
-<code lisp> +
- ;; place +
-                (let ((place-action +
-                        (or (when place-action +
-                              (let* ((referenced-action-desig +
-                                       ​(desig:​reference place-action)) +
-                                     ​(?​arm +
-                                       ​(desig:​desig-prop-value referenced-action-desig :arm)) +
-                                     ​(?​projected-target-location +
-                                       ​(desig:​desig-prop-value referenced-action-desig :​target))) +
-                                (desig:an action +
-                                          (type placing) +
-                                          (arm ?arm) +
-                                          (object ?​object-designator) +
-                                          (target ?​projected-target-location)))) +
-                            (desig:an action +
-                                      (type placing) +
-                                      (desig:when ?arm +
-                                        (arm ?arm)) +
-                                      (object ?​object-designator) +
-                                      (target ?​target-location))))) +
-</​code>​ +
- +
-With the valid standing point position for the robot (''?​target-robot-location''​) ... +
- +
-{{playground:​found-rob-loc.png?​600|}} +
- +
-... and after turning towards the object (with the coordination ''?​target-location''​),​ we can assemble the placing action... +
- +
-<code lisp> +
-;; test if the placing trajectory is reachable and not colliding +
-(setf place-action (desig:​current-desig place-action)) +
-(proj-reasoning:​check-placing-collisions place-action) +
-(setf place-action (desig:​current-desig place-action)) +
-</​code>​ +
- +
-... to then check if it leads to collisions and ... +
- +
-<code lisp> +
-;; test if the placing pose is a good one -- not falling on the floor +
-;; test function throws a high-level-failure if not good pose +
-(proj-reasoning:​check-placing-pose-stability +
- ?​object-designator ?​target-location) +
-</​code>​  +
- +
-... test if it places the object on a valid surface. +
-The constructed placing action looks in our example like this:  +
- +
-<code lisp> +
-#<A ACTION +
-(TYPE PLACING) +
-(OBJECT #<A OBJECT +
- (LOCATION #<A LOCATION +
- (ON #<A 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)>) +
- (TYPE BOWL) +
- (NAME BOWL-1) +
- (POSE ((:POSE +
-#<​CL-TRANSFORMS-STAMPED:​POSE-STAMPED  +
-FRAME-ID: "​base_footprint",​ STAMP: 1.560941941216607d9 +
-#<​3D-VECTOR (0.5515197167662196d0 3.296925998874656d-8 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005169252184379536d0 -0.003215177926776794d0 0.04518378557908993d0 0.9989601391642928d0)>>​) +
-(:​TRANSFORM +
-#<​CL-TRANSFORMS-STAMPED:​TRANSFORM-STAMPED  +
-FRAME-ID: "​base_footprint",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560941941216607d9 +
-#<​3D-VECTOR (0.5515197167662196d0 3.296925998874656d-8 0.8886420529683431d0)>​ +
-#<​QUATERNION (-0.005169252184379536d0 -0.003215177926776794d0 0.04518378557908993d0 0.9989601391642928d0)>>​) +
-(:​POSE-IN-MAP +
-#<​CL-TRANSFORMS-STAMPED:​POSE-STAMPED  +
-FRAME-ID: "​map",​ STAMP: 1.560941941216607d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.00298106437548995d0 5.163229070603848d-4 0.9999813437461853d0)>>​) +
-(:​TRANSFORM-IN-MAP +
-#<​CL-TRANSFORMS-STAMPED:​TRANSFORM-STAMPED  +
-FRAME-ID: "​map",​ CHILD-FRAME-ID:​ "​bowl_1",​ STAMP: 1.560941941216607d9 +
-#<​3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>​ +
-#<​QUATERNION (-0.005307710729539394d0 -0.00298106437548995d0 5.163229070603848d-4 0.9999813437461853d0)>>​)))>​) +
- (TARGET #<A LOCATION +
- (ON #<A OBJECT +
- (TYPE COUNTER-TOP) +
- (URDF-NAME KITCHEN-ISLAND-SURFACE) +
- (OWL-NAME "​kitchen_island_counter_top"​) +
- (PART-OF KITCHEN)>​) +
- (CONTEXT TABLE-SETTING) +
- (FOR #<A OBJECT +
- (TYPE BOWL)>​) +
- (OBJECT-COUNT 3) +
- (SIDE BACK) +
- (SIDE RIGHT) +
- (RANGE-INVERT 0.5)>​)>​  +
-</​code>​ +
- +
-After all this we can finally execute the place action. +
- +
-<code lisp> +
-(exe:​perform place-action)))))))))) +
-</​code>​ +
- +
-{{playground:​delivered.png?​direct&​600|}}+