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/10 10:03] – 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:
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> </code>
  
Line 288: Line 288:
 <code lisp> <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)))))
 </code> </code>
 To fetch the object the object first has to be searched. For this we create To fetch the object the object first has to be searched. For this we create
Line 313: Line 313:
 <code lisp> <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> </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. 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> <code lisp>
-;; If running on the real robot, execute below task tree in projection +         ;; If running on the real robot, execute below task tree in projection 
-;; N times first, then pick the best parameterization +         ;; N times first, then pick the best parameterization 
-;; and use that parameterization in the real world. +         ;; and use that parameterization in the real world. 
-;; If running in projection, just execute the task tree below as normal. +         ;; If running in projection, just execute the task tree below as normal. 
-(let (?fetch-pick-up-action ?deliver-place-action) +         (let (?fetch-pick-up-action ?deliver-place-action) 
-(proj-reasoning:with-projected-task-tree +           (proj-reasoning:with-projected-task-tree 
-(?fetch-robot-location ?fetch-pick-up-action +               (?fetch-robot-location ?fetch-pick-up-action 
-?deliver-robot-location ?deliver-place-action) +                                      ?deliver-robot-location ?deliver-place-action) 
-3 +               3 
-#'proj-reasoning:pick-best-parameters-by-distance+               #'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> </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. 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.
Line 358: Line 358:
 <code lisp> <code lisp>
 (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> </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''.   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> <code lisp>
-(unless search-location-accessible + (unless search-location-accessible 
-(exe:perform (desig:an action +      (exe:perform (desig:an action 
-(type sealing) +                             (type sealing) 
-(location ?search-location) +                             (location ?search-location) 
-(distance 0.3))))))+                             (distance 0.48))))))
 </code> </code>
  
Line 837: 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 925: 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 934: 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 965: 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 1006: 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 1016: 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 1047: 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|}}