This is an old revision of the document!


Learning about the transport function with an example

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

(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)

… 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.

Since we passed (demo-random) the object of type bowl the actions searching and transporting will be executed:

(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)
;; (arm right)
(location ?fetching-location)
(target ?delivering-location)))

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

(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."

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.

(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.")))

If the referencing of below designators fail the error object-nowhere-to-be-found will be thrown.

The basic idea is the following: First the robot tries to navigate to the location ?robot-location. If this is succeeded within a number of retries the robot tries now to turn towards the location ?search-location and detect the object within in the number of specific retries too. 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.

;; 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~%"))))

So here we set a new ?search-location since we could not move to robot or/and find detect the object.

;; 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)))

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:

#<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)>)>)>

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:

#<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)>)> 

This designator is a subset of the action designator above and …

#<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)>

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:

(#<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}>)) 

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:

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 the nested location designator with get-location-poses.

This is after the execution of the navigation action designator.

;; 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)))))))))))

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.:

#<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)>>)))> 

Transport

(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)

text…

(unless search-location-accessible
(exe:perform (desig:an action
(type accessing)
(location ?search-location)
(distance 0.3))))
 
(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)))))
(?robot-name
(cut:var-value '?robot-name
(car (prolog:prolog '(rob-int:robot ?robot-name))))))
(roslisp:ros-info (pp-plans transport)
"Found object of type ~a."
(desig:desig-prop-value ?perceived-object-designator :type))
 
(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))))
 
;; 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)))))
 
(roslisp:ros-info (pp-plans transport) "Fetched the object.")
(cpl:with-failure-handling
((common-fail:object-undeliverable (e)
(declare (ignore e))
(drop-at-sink)
;; (return)
))
(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))))))))))
 
(unless search-location-accessible
(exe:perform (desig:an action
(type sealing)
(location ?search-location)
(distance 0.3))))))

more text…

go-without-collisions

(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."
\end{lstlisting}
The only input parameter is the location going to which has to be a location designator.
\begin{lstlisting}
(exe:perform (desig:an action
(type positioning-arm)
(left-configuration park)
(right-configuration park)))

First we move the arms in the park position, so the don't hit anything on their way.

(proj-reasoning:check-navigating-collisions ?navigation-location)

Then we check if the requested location is in collision with the bullet world.

(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)))))

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

(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."

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.

(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)))

If the referencing below of the designators with type looking or navigation fail designator-error or if especially the navigation fails with a navigation-high-level-failure error, although this is not implemented since errors will be ignored (see go-without-collisions), the error looking-high-level-failure will be thrown.

(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)))
(exe:perform (desig:an action
(type looking)
(target ?look-target)))))))

Now the robot tries with

(exe:perform (desig:an action
(type looking)
(target ?look-target))

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

(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)."

The documentation of this method describes the input parameters and in the declare section you see which types the designators have.

(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.")))

As always if any of the designators below cannot resolve the error designator-error will be thrown.

;; 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 ;; from navigation action
common-fail:looking-high-level-failure ;; from turning-towards action
common-fail:perception-low-level-failure
common-fail:object-unreachable ;; from picking-up
common-fail:manipulation-low-level-failure) (e) ;; and this too
(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))))

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.

;; navigate, look, detect and pick-up
(exe:perform (desig:an action
(type navigating)
(location ?pick-up-robot-location)))

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:

#<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)>>)))>)>)>

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.

(exe:perform (desig:an action
(type turning-towards)
(target (desig:a location (of ?object-designator)))))

The second actions turn the robot towards the location of the object ?object-designator. For this the following example actions will be created:

#<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)>>)))>)>)>

And therefore get-location-poses will be called again with:

#<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)>>)))>)> 

and returns a lazy list of pose-stamped with which then a looking action can be executed.

#<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)>>)))>)>)>
(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.")))

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.

(let ((?more-precise-perceived-object-desig
(exe:perform (desig:an action
(type detecting)
(object ?object-designator)))))

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.

(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)))))

To allow the robotor to access the object with both arms and all possible grasps, the lists arms and grasps will be traversed like explained here.

(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))))
(desig:an action
(type picking-up)
(desig:when ?arm
(arm ?arm))
(desig:when ?grasp
(grasp ?grasp))
(object
?more-precise-perceived-object-desig)))))

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.

(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))

First we check if this action is in collision and then

(exe:perform pick-up-action)

we try by executing the action.

(exe:perform (desig:an action
(type positioning-arm)
(left-configuration park)
(right-configuration park)))
(desig:current-desig ?object-designator)))))))))))))))

Afterwards call an action to park the arms to end this method and return the object ?object-designator.

The robot fetched the object.