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
tutorials:intermediate:fetch_deliver_plans [2019/07/10 10:18] – [fetch] tlippstutorials:intermediate:fetch_deliver_plans [2022/07/04 12:31] (current) – [search-for-object] khoshnam
Line 25: Line 25:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj::drive (cl-transforms-stamped:make-pose-stamped +CL-USER> (urdf-proj::drive (cl-transforms-stamped:make-pose-stamped 
-                           "map" +                            "map" 
-                           0.0 +                            0.0 
-                           (cl-transforms:make-3d-vector 0.5 0 0) +                            (cl-transforms:make-3d-vector 0.5 0 0) 
-                           (cl-transforms:make-identity-rotation)))+                            (cl-transforms:make-identity-rotation)))
 </code> </code>
                                                        
Line 35: Line 35:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj::move-torso 0.3)+CL-USER> (urdf-proj::move-torso 0.3)
 </code> </code>
  
Line 45: Line 45:
                                         (cl-tf:make-3d-vector 1.5 0.0 1.3)                                         (cl-tf:make-3d-vector 1.5 0.0 1.3)
                                         (cl-tf:make-identity-rotation)))                                         (cl-tf:make-identity-rotation)))
-CL-USER> (pr2-proj::detect (desig:an object (type cup)))+CL-USER> (urdf-proj::detect (desig:an object (type cup)))
 #<A OBJECT #<A OBJECT
     (TYPE CUP)     (TYPE CUP)
Line 74: Line 74:
 ==== Projection process modules ==== ==== Projection process modules ====
  
-The PMs are implemented in ''cram_pr2_projection/src/process-modules.lisp''+The PMs are implemented in ''cram_urdf_projection/src/process-modules.lisp''
  
 To directly call a PM, use PM-EXECUTE function: To directly call a PM, use PM-EXECUTE function:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot+CL-USER> (urdf-proj:with-projected-robot 
 +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
 +                         "map" 0.0 
 +                         (cl-transforms:make-3d-vector 0.75 0 0) 
 +                         (cl-transforms:make-identity-rotation))))
              (cram-process-modules:pm-execute              (cram-process-modules:pm-execute
-              'pr2-proj::pr2-proj-navigation +              'urdf-proj::urdf-proj-navigation 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +              (desig:an motion 
-                            "map" 0.0 +                        (type going) 
-                            (cl-transforms:make-3d-vector 0.75 0 0) +                        (pose ?pose)))))
-                            (cl-transforms:make-identity-rotation)))) +
-                (desig:a motion +
-                         (type going) +
-                         (target (desig:a location +
-                                          (pose ?pose)))))))+
 </code> </code>
                                                                                      
Line 96: Line 95:
 To automatically dispatch motion designators to their correct PMs, there are the ''matching-process-module'' Prolog predicates (implementation is in the same ''process-modules.lisp'' file). To automatically dispatch motion designators to their correct PMs, there are the ''matching-process-module'' Prolog predicates (implementation is in the same ''process-modules.lisp'' file).
  
-To execute a motion with automatic PM dispatching, use ''exe:perform'':+To execute a motion with automatic PM dispatching, use ''exe:perform'' or ''cram-executive:perform'':
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "map" 0.0 
-                            "map" 0.0 +                         (cl-transforms:make-3d-vector 0.75 0 0) 
-                            (cl-transforms:make-3d-vector 0 0 0) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform (desig:an motion 
-                (desig:a motion +                                               (type going) 
-                         (type going) +                                               (pose ?pose)))))
-                         (target (desig:a location +
-                                          (pose ?pose)))))))+
 </code> </code>
                                                                                      
-''pr2-proj:with-projected-robot'' is implemented in ''cram_pr2_projection/src/projection-environment.lisp''+''urdf-proj:with-projected-robot'' is implemented in ''cram_urdf_projection/src/projection-environment.lisp''
  
 It activates all the PMs defined in PR2's projection and sets up some environment variables to distinguish between real robot and projected robot, for example the TF publishers and subscribers, in order not to mess up the real robot's TF while it is imagining things in projection. It activates all the PMs defined in PR2's projection and sets up some environment variables to distinguish between real robot and projected robot, for example the TF publishers and subscribers, in order not to mess up the real robot's TF while it is imagining things in projection.
Line 125: Line 122:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +            (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                          "map" 0.0 
-                            "map" 0.0 +                          (cl-transforms:make-3d-vector 0 0 0) 
-                            (cl-transforms:make-3d-vector 0 0 0) +                          (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +              (cram-executive:perform 
-                (desig:an action +               (desig:an action 
-                          (type going) +                         (type going) 
-                          (target (desig:a location +                         (target (desig:a location 
-                                           (pose ?pose)))))))+                                          (pose ?pose)))))))
 </code> </code>
                                                                                        
Line 140: Line 137:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "base_footprint" 0.0 
-                            "base_footprint" 0.0 +                         (cl-transforms:make-3d-vector 0.7 0.5 0.8) 
-                            (cl-transforms:make-3d-vector 0.7 0.5 0.8) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform  
-                (desig:an action +              (desig:an action 
-                          (type lifting) +                        (type lifting) 
-                          (left-poses (?pose))))))+                        (left-poses (?pose))))))
                                                      
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "base_footprint" 0.0 
-                            "base_footprint" 0.0 +                         (cl-transforms:make-3d-vector 0.7 0.5 0.8) 
-                            (cl-transforms:make-3d-vector 0.7 0.5 0.8) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform  
-                (desig:an action +              (desig:an action 
-                          (type reaching) +                        (type reaching) 
-                          (left-poses (?pose))))))+                        (left-poses (?pose))))))
 </code> </code>
                                                      
Line 179: Line 176:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (cram-executive:perform 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +            (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-                            "base_footprint" 0.0 +                          "base_footprint" 0.0 
-                            (cl-transforms:make-3d-vector 0.7 0.5 0.8) +                          (cl-transforms:make-3d-vector 0.7 0.5 0.8) 
-                            (cl-transforms:make-identity-rotation)))) +                          (cl-transforms:make-identity-rotation)))) 
-                (desig:an action +              (desig:an action 
-                          (type looking) +                        (type looking) 
-                          (target (desig:a location (pose ?pose)))))))+                        (target (desig:a location (pose ?pose)))))))
 </code> </code>
  
Line 193: Line 190:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (cram-executive:perform 
-              (desig:an action +            (desig:an action 
-                        (type detecting) +                      (type detecting) 
-                        (object (desig:an object (type cup))))))+                      (object (desig:an object (type cup))))))
 </code> </code>
                                                  
Line 203: Line 200:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (let ((?obj *)) +           (let ((?obj *)) 
-               (cram-executive:perform +             (cram-executive:perform 
-                (desig:an action +              (desig:an action 
-                          (type picking-up) +                        (type picking-up) 
-                          (object ?obj))))) +                        (object ?obj) 
-</code> +                        (arm left) 
-                           +                        (grasp top)))))
-The missing information is inferred automatically. +
- +
-But, we can specify the arm explicitly: +
- +
-<code lisp> +
-CL-USER> (pr2-proj:with-projected-robot +
-             (let ((?obj *)) +
-               (cram-executive:perform +
-                (desig:an action +
-                          (type picking-up) +
-                          (object ?obj) +
-                          (arm left))))) +
-</code> +
- +
-And the grasp type as well: +
- +
-<code lisp> +
-(pr2-proj:with-projected-robot +
-             (let ((?obj *)) +
-               (cram-executive:perform +
-                (desig:an action +
-                          (type picking-up) +
-                          (object ?obj) +
-                          (arm left) +
-                          (grasp side)))))+
 </code> </code>
 +                         
                                                      
 Object got attached through an event, so now it will always follow the robot: Object got attached through an event, so now it will always follow the robot:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "map" 0.0 
-                            "map" 0.0 +                         (cl-transforms:make-3d-vector -0.5 0 0) 
-                            (cl-transforms:make-3d-vector -0.5 0 0) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform  
-                (desig:an action +              (desig:an action 
-                          (type going) +                        (type going) 
-                          (target (desig:a location +                        (target (desig:a location 
-                                           (pose ?pose)))))))+                                         (pose ?pose)))))))
 </code> </code>
                                                                                        
Line 256: Line 229:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (cram-executive:perform 
-              (desig:an action +            (desig:an action 
-                        (type placing) +                      (type placing) 
-                        (arm left))))+                      (arm left))))
 </code> </code>
                                                  
  
-==== High-level action plans: fetching and delivering and manipulating environment and ... ====+===== High-level action plans: fetching and delivering and manipulating environment =====
  
 Most high-level plans at the moment are implemented in ''cram_pr2_fetch_deliver_plans''. 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 are going to follow the execution of the other high level functions in the deliver function. Most high-level plans at the moment are implemented in ''cram_pr2_fetch_deliver_plans''. 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 are going to 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'' ... We do this by loading the packages ''cram_pr2_pick_and_place'' and ''cram_bullet_world_tutorial'' ...
 <code lisp> <code lisp>
-(ros-load:load-system "cram_pr2_pick_and_place" :cram_pr2_pick_and_place) +(ros-load:load-system "cram_pr2_pick_place_demo" :cram-pr2-pick-place-demo) 
-(ros-load:load-system "cram_bullet_world_tutorial" :cram_bullet_world_tutorial)+(ros-load:load-system "cram_bullet_world_tutorial" :cram-bullet-world-tutorial)
 </code> </code>
 ... and call the following functions: ... and call the following functions:
-<code>+<code lisp>
 (in-package :cram-bullet-world-tutorial) (in-package :cram-bullet-world-tutorial)
 (roslisp-utilities:startup-ros) (roslisp-utilities:startup-ros)
Line 281: Line 254:
 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?600 |}}
  
 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:
Line 325: Line 298:
 ;; location desigs can turn NILL in the course of execution ;; location desigs can turn NILL in the course of execution
 ;; but should not be passed as NILL to start with. ;; but should not be passed as NILL to start with.
-(type (or desig:location-designator null) ?search-location ?robot-location))+(type (or desig:location-designator null) ?search-location ?robot-location)))
 "Searches for `?object-designator' in its likely location `?search-location'. "Searches for `?object-designator' in its likely location `?search-location'.
 If the object is not there or navigation location is unreachable, If the object is not there or navigation location is unreachable,
Line 449: Line 422:
 The procedure can be visualized with the Bullet World too: The procedure can be visualized with the Bullet World too:
  
-{{playground:search-for-obj_robot_loc_object_location_resolved.png?600|}}+{{ 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. 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|}}+{{ 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. This is the costmap after we called the nested location designator with get-location-poses.
  
-{{playground:search-for-robot-moved.png?600|}}+{{ playground:search-for-robot-moved.png?600 |}}
  
 This is after the execution of the navigation action designator. This is after the execution of the navigation action designator.
Line 726: Line 699:
 Since we already searched the object we know the pose of the object which is therefore in the location designator. Nevertheless, we need a pose where 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. Since we already searched the object we know the pose of the object which is therefore in the location designator. Nevertheless, we need a pose where 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|}}+{{ playground:fetch-after-navigating.png?600 |}}
  
  
Line 966: Line 939:
 Afterwards call an action to park the arms to end this method and return the object ''?object-designator''. Afterwards call an action to park the arms to end this method and return the object ''?object-designator''.
  
-{{playground:fetched-obj.png?&600|}}+{{ playground:fetched-obj.png?&600 |}}
  
 The robot fetched the object. 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 [[tutorials:intermediate:fetch_deliver_plans#search-for-object|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 [[tutorials:intermediate:fetch_deliver_plans#search-for-object|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 |}}
 +
 +==== 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>
 +
 +<code lisp>
 +(unless search-location-accessible
 +    (exe:perform (desig:an action
 +                           (type accessing)
 +                           (location ?search-location)
 +                           (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
 +       (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)
 +               3
 +               #'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.48))))))
 +</code>
 +
 +At the end we seal the e. g. drawer we got our fork from with a specific distance towards the drawer.