Differences

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

--- playground:playground [2019/07/03 14:35] – tlipps
+++ playground:playground [2022/09/03 17:15] (current) – external edit 127.0.0.1
@@ 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)
-;; (arm right)
-(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 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.
-<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)))))))))))
-ablauf:
-</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>