Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revisionLast revisionBoth sides next revision | ||
playground:playground [2019/07/03 14:35] – tlipps | playground:playground [2019/07/10 11:58] – tlipps | ||
---|---|---|---|
Line 8: | Line 8: | ||
</ | </ | ||
... and call the following functions: | ... and call the following functions: | ||
- | </code> | + | < |
(in-package : | (in-package : | ||
(roslisp-utilities: | (roslisp-utilities: | ||
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: | + | {{ playground: |
Since we passed '' | Since we passed '' | ||
<code lisp> | <code lisp> | ||
(when (eq ? | (when (eq ? | ||
- | (cpl: | + | |
- | ((common-fail: | + | ((common-fail: |
- | (roslisp: | + | (roslisp: |
- | (return))) | + | (return))) |
- | (let ((?loc (cdr (assoc : | + | (let ((?loc (cdr (assoc : |
- | (exe: | + | (exe: |
- | (desig:an action | + | (desig:an action |
- | (type searching) | + | (type searching) |
- | (object (desig:an object (type breakfast-cereal))) | + | (object (desig:an object (type breakfast-cereal))) |
- | (location ?loc)))))) | + | (location ?loc)))))) |
(cpl: | (cpl: | ||
- | ((common-fail: | + | |
- | (roslisp: | + | (roslisp: |
- | (return))) | + | (return))) |
- | (if (eq ? | + | (if (eq ? |
- | (exe: | + | (exe: |
- | (desig:an action | + | |
- | (type transporting) | + | |
- | (object ? | + | |
- | ;; (arm right) | + | |
- | (location ? | + | |
- | (target ? | + | |
</ | </ | ||
Line 52: | Line 51: | ||
<code lisp> | <code lisp> | ||
(defun search-for-object (&key | (defun search-for-object (&key | ||
- | ((:object ? | + | ((:object ? |
- | ((:location ? | + | |
- | ((: | + | |
- | (retries 3) | + | |
- | & | + | & |
(declare (type desig: | (declare (type desig: | ||
;; location desigs can turn NILL in the course of execution | ;; location desigs can turn NILL in the course of execution | ||
Line 70: | Line 69: | ||
<code lisp> | <code lisp> | ||
(cpl: | (cpl: | ||
- | ((desig: | + | |
- | (roslisp: | + | (roslisp: |
- | "Desig ~a could not be resolved: ~a~%Propagating up." | + | "Desig ~a could not be resolved: ~a~%Propagating up." |
- | ? | + | ? |
- | (cpl:fail ' | + | (cpl:fail ' |
- | : | + | : |
</ | </ | ||
- | If the referencing of below designators | + | If the referencing of below designators |
- | The basic idea is the following: First the robot tries to navigate to the location ''? | + | The basic idea is the following: First the robot tries to navigate to the location ''? |
Now if the robot cannot detect the object or cannot navigate to the ''? | Now if the robot cannot detect the object or cannot navigate to the ''? | ||
Line 86: | Line 85: | ||
;; take new `? | ;; take new `? | ||
(cpl: | (cpl: | ||
- | (cpl: | + | |
- | ((common-fail: | + | ((common-fail: |
- | (common-fail: | + | (common-fail: |
- | ? | + | ? |
- | outer-search-location-retries | + | outer-search-location-retries |
- | (: | + | (: |
- | : | + | : |
- | : | + | : |
- | : | + | : |
- | (roslisp: | + | |
- | " | + | |
</ | </ | ||
Line 104: | Line 103: | ||
;; if the going action fails, pick another `? | ;; if the going action fails, pick another `? | ||
(cpl: | (cpl: | ||
- | (cpl: | + | |
- | (((or common-fail: | + | (((or common-fail: |
- | common-fail: | + | common-fail: |
- | common-fail: | + | common-fail: |
- | (common-fail: | + | |
- | ? | + | |
- | robot-location-retries | + | |
- | (: | + | |
- | : | + | : |
- | : | + | : |
- | : | + | : |
- | ;; navigate | + | ;; navigate |
- | (exe: | + | |
- | (type navigating) | + | (type navigating) |
- | (location ? | + | (location ? |
</ | </ | ||
Line 184: | Line 183: | ||
The procedure can be visualized with the btr too: | The procedure can be visualized with the btr too: | ||
- | {{playground: | + | {{playground: |
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: | + | {{playground: |
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: | + | {{playground: |
This is after the execution of the navigation action designator. | This is after the execution of the navigation action designator. | ||
+ | |||
<code lisp> | <code lisp> | ||
;; if perception action fails, try another `? | ;; if perception action fails, try another `? | ||
(cpl: | (cpl: | ||
- | (cpl: | + | |
- | (((or common-fail: | + | (((or common-fail: |
- | common-fail: | + | common-fail: |
- | (common-fail: | + | |
- | ? | + | |
- | search-location-retries | + | |
- | (: | + | |
- | : | + | : |
- | : | + | : |
- | (exe: | + | (exe: |
- | (type turning-towards) | + | (type turning-towards) |
- | (target ? | + | (target ? |
- | (exe: | + | |
- | (type detecting) | + | (type detecting) |
- | (object ? | + | (object ? |
- | ablauf: | + | |
</ | </ | ||
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 '' | 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 '' | ||
The detection action calls the '' | The detection action calls the '' | ||
- | At the end the function '' | + | At the end the function '' |
<code lisp> | <code lisp> | ||
#<A 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 " | + | (OWL-NAME " |
- | (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) | ||
Line 252: | Line 255: | ||
#< | #< | ||
</ | </ | ||
+ | |||
+ | **Transport** | ||
+ | |||
+ | <code lisp> | ||
+ | (defun transport (&key | ||
+ | ((:object ? | ||
+ | ((: | ||
+ | ((: | ||
+ | ((: | ||
+ | ((:arm ?arm)) | ||
+ | ((:grasp ?grasp)) | ||
+ | ((:arms ?arms)) | ||
+ | ((:grasps ?grasps)) | ||
+ | ((: | ||
+ | ((: | ||
+ | search-location-accessible | ||
+ | delivery-location-accessible | ||
+ | & | ||
+ | </ | ||
+ | |||
+ | <code lisp> | ||
+ | (unless search-location-accessible | ||
+ | (exe: | ||
+ | (type accessing) | ||
+ | | ||
+ | | ||
+ | </ | ||
+ | |||
+ | 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 '' | ||
+ | |||
+ | <code lisp> | ||
+ | (unwind-protect | ||
+ | (let ((? | ||
+ | | ||
+ | (type searching) | ||
+ | (object ? | ||
+ | (location ? | ||
+ | (desig:when ? | ||
+ | (robot-location ? | ||
+ | </ | ||
+ | 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) | ||
+ | | ||
+ | | ||
+ | | ||
+ | (side left) | ||
+ | (side front) | ||
+ | | ||
+ | </ | ||
+ | |||
+ | <code lisp> | ||
+ | (unless ? | ||
+ | (setf ? | ||
+ | | ||
+ | (reachable-for ? | ||
+ | (desig:when ?arm | ||
+ | (arm ?arm)) | ||
+ | (object ? | ||
+ | | ||
+ | (setf ? | ||
+ | | ||
+ | (reachable-for ? | ||
+ | (location ? | ||
+ | </ | ||
+ | Moreover, we need the fetch- and deliver-robot-locations. The ? | ||
+ | |||
+ | <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 (? | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | 3 | ||
+ | #' | ||
+ | |||
+ | (let ((? | ||
+ | | ||
+ | (type fetching) | ||
+ | (desig:when ?arm | ||
+ | (arm ?arm)) | ||
+ | (desig:when ?grasp | ||
+ | (grasp ?grasp)) | ||
+ | (desig:when ?arms | ||
+ | (arms ?arms)) | ||
+ | (desig:when ?grasps | ||
+ | (grasps ?grasps)) | ||
+ | (object ? | ||
+ | (robot-location ? | ||
+ | (pick-up-action ? | ||
+ | </ | ||
+ | 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 '' | ||
+ | |||
+ | <code lisp> | ||
+ | (unless delivery-location-accessible | ||
+ | | ||
+ | (type accessing) | ||
+ | (location ? | ||
+ | (distance 0.3)))) | ||
+ | | ||
+ | (exe: | ||
+ | (type delivering) | ||
+ | | ||
+ | (arm ?arm)) | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | (type sealing) | ||
+ | (location ? | ||
+ | (distance 0.3)))))))))) | ||
+ | </ | ||
+ | In the last part of this function we first check if the deliver location is accessible ('' | ||
+ | |||
+ | <code lisp> | ||
+ | | ||
+ | (exe: | ||
+ | (type sealing) | ||
+ | | ||
+ | | ||
+ | </ | ||
+ | |||
+ | At the end we seal the e. g. drawer we got our fork from with a specific distance towards the drawer. | ||
+ | |||
+ | **go-without-collisions** | ||
+ | |||
+ | <code lisp> | ||
+ | (defun go-without-collisions (&key | ||
+ | ((:location ? | ||
+ | & | ||
+ | (declare (type desig: | ||
+ | "Check if navigation goal is in reach, if not propagate failure up, | ||
+ | if yes, perform GOING action while ignoring failures." | ||
+ | |||
+ | (exe: | ||
+ | (type positioning-arm) | ||
+ | | ||
+ | | ||
+ | </ | ||
+ | |||
+ | First we move the arms in the park position, so they do not hit anything on their way. | ||
+ | |||
+ | <code lisp> | ||
+ | (proj-reasoning: | ||
+ | </ | ||
+ | |||
+ | Then we check if the requested location is in collision with the bullet world. | ||
+ | |||
+ | <code lisp> | ||
+ | (setf ? | ||
+ | |||
+ | (cpl: | ||
+ | ((common-fail: | ||
+ | | ||
+ | " | ||
+ | | ||
+ | (exe: | ||
+ | (type going) | ||
+ | | ||
+ | </ | ||
+ | |||
+ | Then we try to go with the action designator to the desired location, if it fails, because it was an '' | ||
+ | |||
+ | |||
+ | **turn-towards** | ||
+ | <code lisp> | ||
+ | (defun turn-towards (&key | ||
+ | | ||
+ | | ||
+ | & | ||
+ | (declare (type desig: | ||
+ | " | ||
+ | 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. | ||
+ | |||
+ | <code lisp> | ||
+ | | ||
+ | ((desig: | ||
+ | | ||
+ | " | ||
+ | ? | ||
+ | | ||
+ | |||
+ | | ||
+ | | ||
+ | " | ||
+ | Cannot look." | ||
+ | e) | ||
+ | | ||
+ | </ | ||
+ | |||
+ | If the referencing of the designators below with type looking or navigation fail '' | ||
+ | |||
+ | <code lisp> | ||
+ | | ||
+ | (cpl: | ||
+ | ((common-fail: | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | (type navigating) | ||
+ | (location ? | ||
+ | | ||
+ | (type looking) | ||
+ | (direction forward)))) | ||
+ | | ||
+ | | ||
+ | | ||
+ | </ | ||
+ | |||
+ | Now the robot tries with | ||
+ | <code lisp> | ||
+ | | ||
+ | (type looking) | ||
+ | | ||
+ | </ | ||
+ | to look at a location. If this fails with an '' | ||
+ | |||
+ | **fetch** | ||
+ | |||
+ | <code lisp> | ||
+ | (defun fetch (&key | ||
+ | ((:object ? | ||
+ | ((:arms ?arms)) | ||
+ | ((:grasps ?grasps)) | ||
+ | ((: | ||
+ | pick-up-action | ||
+ | & | ||
+ | (declare (type desig: | ||
+ | (type list ?arms ?grasps) | ||
+ | ;; ? | ||
+ | ;; but can become NULL during execution of the plan | ||
+ | (type (or desig: | ||
+ | (type (or desig: | ||
+ | " | ||
+ | one of arms in the `? | ||
+ | while standing at `? | ||
+ | and using the grasp and arm specified in `pick-up-action' | ||
+ | </ | ||
+ | |||
+ | The documentation of this method describes the input parameters and in the declare section you see which types the designators have. | ||
+ | |||
+ | <code lisp> | ||
+ | (cpl: | ||
+ | ((desig: | ||
+ | | ||
+ | | ||
+ | : | ||
+ | : | ||
+ | </ | ||
+ | |||
+ | As always if any of the designators below cannot resolve the error '' | ||
+ | |||
+ | <code lisp> | ||
+ | ;; take a new `? | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or common-fail: | ||
+ | common-fail: | ||
+ | common-fail: | ||
+ | common-fail: | ||
+ | common-fail: | ||
+ | | ||
+ | ? | ||
+ | | ||
+ | | ||
+ | (format NIL " | ||
+ | (desig: | ||
+ | : | ||
+ | : | ||
+ | </ | ||
+ | |||
+ | 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 '' | ||
+ | |||
+ | <code lisp> | ||
+ | ;; navigate, look, detect and pick-up | ||
+ | (exe: | ||
+ | (type navigating) | ||
+ | | ||
+ | </ | ||
+ | |||
+ | The first action should move the robot without any collisions to the location ''? | ||
+ | |||
+ | <code lisp> | ||
+ | #<A ACTION | ||
+ | (TYPE NAVIGATING) | ||
+ | (LOCATION #<A LOCATION | ||
+ | (REACHABLE-FOR PR2) | ||
+ | (OBJECT #<A OBJECT | ||
+ | (LOCATION #<A LOCATION | ||
+ | (ON #<A OBJECT | ||
+ | (TYPE COUNTER-TOP) | ||
+ | (URDF-NAME SINK-AREA-SURFACE) | ||
+ | (OWL-NAME kitchen_sink_block_counter_top) | ||
+ | (PART-OF KITCHEN)> | ||
+ | (SIDE LEFT) | ||
+ | (SIDE FRONT) | ||
+ | (RANGE-INVERT 0.5)>) | ||
+ | (TYPE BOWL) | ||
+ | (NAME BOWL-1) | ||
+ | (POSE ((POSE | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (TRANSFORM | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (POSE-IN-MAP | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (TRANSFORM-IN-MAP | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | </ | ||
+ | |||
+ | Since we already searched the object we now the pose of the object which is therefore in the location designator. Nevertheless, | ||
+ | |||
+ | {{playground: | ||
+ | |||
+ | |||
+ | <code lisp> | ||
+ | (exe: | ||
+ | (type turning-towards) | ||
+ | | ||
+ | |||
+ | </ | ||
+ | |||
+ | The second actions turn the robot towards the location of the object ''? | ||
+ | |||
+ | <code lisp> | ||
+ | #<A ACTION | ||
+ | (TYPE TURNING-TOWARDS) | ||
+ | (TARGET #<A LOCATION | ||
+ | (OF #<A OBJECT | ||
+ | (LOCATION #<A LOCATION | ||
+ | (ON #<A OBJECT | ||
+ | (TYPE COUNTER-TOP) | ||
+ | (URDF-NAME SINK-AREA-SURFACE) | ||
+ | (OWL-NAME kitchen_sink_block_counter_top) | ||
+ | (PART-OF KITCHEN)> | ||
+ | (SIDE LEFT) | ||
+ | (SIDE FRONT) | ||
+ | (RANGE-INVERT 0.5)>) | ||
+ | (TYPE BOWL) | ||
+ | (NAME BOWL-1) | ||
+ | (POSE ((POSE | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (TRANSFORM | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (POSE-IN-MAP | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (TRANSFORM-IN-MAP | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | </ | ||
+ | |||
+ | And therefore '' | ||
+ | |||
+ | <code lisp> | ||
+ | #<A LOCATION | ||
+ | (OF #<A OBJECT | ||
+ | (LOCATION #<A LOCATION | ||
+ | (ON #<A OBJECT | ||
+ | (TYPE COUNTER-TOP) | ||
+ | (URDF-NAME SINK-AREA-SURFACE) | ||
+ | (OWL-NAME " | ||
+ | (PART-OF KITCHEN)> | ||
+ | (SIDE LEFT) | ||
+ | (SIDE FRONT) | ||
+ | (RANGE-INVERT 0.5)>) | ||
+ | (TYPE BOWL) | ||
+ | (NAME BOWL-1) | ||
+ | (POSE ((:POSE | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (:TRANSFORM | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (: | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (: | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | </ | ||
+ | |||
+ | and returns a lazy list of pose-stamped with which then a looking action can be executed. | ||
+ | |||
+ | <code lisp> | ||
+ | #<A ACTION | ||
+ | (TYPE LOOKING) | ||
+ | (TARGET #<A LOCATION | ||
+ | (OF #<A OBJECT | ||
+ | (LOCATION #<A LOCATION | ||
+ | (ON #<A OBJECT | ||
+ | (TYPE COUNTER-TOP) | ||
+ | (URDF-NAME SINK-AREA-SURFACE) | ||
+ | (OWL-NAME kitchen_sink_block_counter_top) | ||
+ | (PART-OF KITCHEN)> | ||
+ | (SIDE LEFT) | ||
+ | (SIDE FRONT) | ||
+ | (RANGE-INVERT 0.5)>) | ||
+ | (TYPE BOWL) | ||
+ | (NAME BOWL-1) | ||
+ | (POSE ((POSE | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (TRANSFORM | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (POSE-IN-MAP | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (TRANSFORM-IN-MAP | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | </ | ||
+ | |||
+ | <code lisp> | ||
+ | | ||
+ | (cpl: | ||
+ | ((common-fail: | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | (type positioning-arm) | ||
+ | (left-configuration park) | ||
+ | (right-configuration park))) | ||
+ | | ||
+ | | ||
+ | | ||
+ | : | ||
+ | </ | ||
+ | |||
+ | Next the failure handling for grasping is written since, if it fails the object should be detected one more time and the different arms and grasps should be tried again. All of these is done afterwards, that's why it's defined here before all that. | ||
+ | |||
+ | <code lisp> | ||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type detecting) | ||
+ | | ||
+ | </ | ||
+ | |||
+ | To get a more precise location of the object the object ''? | ||
+ | |||
+ | <code lisp> | ||
+ | (let ((?arm (cut: | ||
+ | ;; if picking up fails, try another arm | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or common-fail: | ||
+ | common-fail: | ||
+ | desig: | ||
+ | | ||
+ | ?arms | ||
+ | | ||
+ | | ||
+ | (format NIL " | ||
+ | : | ||
+ | : | ||
+ | (setf ?arm (cut: | ||
+ | |||
+ | (let ((?grasp (cut: | ||
+ | ;; if picking up fails, try another grasp orientation | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or common-fail: | ||
+ | common-fail: | ||
+ | desig: | ||
+ | | ||
+ | ? | ||
+ | | ||
+ | | ||
+ | (format NIL " | ||
+ | : | ||
+ | (setf ?grasp (cut: | ||
+ | </ | ||
+ | |||
+ | To allow the robot to access the object with both arms and all possible grasps, the lists '' | ||
+ | |||
+ | <code lisp> | ||
+ | (let ((pick-up-action | ||
+ | ;; if pick-up-action already exists, | ||
+ | ;; use its params for picking up | ||
+ | (or (when pick-up-action | ||
+ | (let* ((referenced-action-desig | ||
+ | | ||
+ | (?arm | ||
+ | | ||
+ | referenced-action-desig | ||
+ | :arm)) | ||
+ | | ||
+ | | ||
+ | referenced-action-desig | ||
+ | :grasp))) | ||
+ | (desig:an action | ||
+ | (type picking-up) | ||
+ | (arm ?arm) | ||
+ | (grasp ?grasp) | ||
+ | (object | ||
+ | ? | ||
+ | </ | ||
+ | |||
+ | Then we construct the picking-up action with either the action designator '' | ||
+ | |||
+ | <code lisp> | ||
+ | (setf pick-up-action (desig: | ||
+ | (proj-reasoning: | ||
+ | (setf pick-up-action (desig: | ||
+ | </ | ||
+ | |||
+ | First we check if this action is in collision and then | ||
+ | |||
+ | <code lisp> | ||
+ | (exe: | ||
+ | </ | ||
+ | |||
+ | we try by executing the action. | ||
+ | |||
+ | <code lisp> | ||
+ | | ||
+ | (type positioning-arm) | ||
+ | | ||
+ | | ||
+ | (desig: | ||
+ | </ | ||
+ | |||
+ | Afterwards call an action to park the arms to end this method and return the object ''? | ||
+ | |||
+ | {{playground: | ||
+ | |||
+ | The robot fetched the object. | ||
+ | |||
+ | **deliver** | ||
+ | |||
+ | <code lisp> | ||
+ | (defun deliver (&key | ||
+ | ((:object ? | ||
+ | ((:arm ?arm)) | ||
+ | ((:target ? | ||
+ | ((: | ||
+ | place-action | ||
+ | & | ||
+ | (declare (type desig: | ||
+ | (type (or keyword null) ?arm) | ||
+ | ;; don't pass NULL as ? | ||
+ | ;; they can turn NULL during execution but not at the beginning | ||
+ | (type (or desig: | ||
+ | (type (or desig: | ||
+ | " | ||
+ | and the robot should stand at `? | ||
+ | If a failure happens, try a different `? | ||
+ | </ | ||
+ | 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 `? | ||
+ | ;; If not, delivering is impossible so throw a OBJECT-UNDERLIVERABLE failure | ||
+ | (cpl: | ||
+ | ((desig: | ||
+ | | ||
+ | | ||
+ | : | ||
+ | </ | ||
+ | |||
+ | If the resolving from a designator below fails we throw the above error. | ||
+ | |||
+ | <code lisp> | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or desig: | ||
+ | common-fail: | ||
+ | | ||
+ | ? | ||
+ | | ||
+ | | ||
+ | (format NIL " | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | </ | ||
+ | |||
+ | Again this method uses same thinking as [[playground: | ||
+ | |||
+ | <code lisp> | ||
+ | ;; take a new `? | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or common-fail: | ||
+ | common-fail: | ||
+ | common-fail: | ||
+ | | ||
+ | ? | ||
+ | | ||
+ | | ||
+ | (format NIL " | ||
+ | : | ||
+ | : | ||
+ | |||
+ | ;; navigate | ||
+ | (exe: | ||
+ | (type navigating) | ||
+ | | ||
+ | |||
+ | </ | ||
+ | |||
+ | 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 " | ||
+ | (PART-OF KITCHEN)> | ||
+ | (CONTEXT TABLE-SETTING) | ||
+ | (FOR #<A OBJECT | ||
+ | (TYPE BOWL)>) | ||
+ | (OBJECT-COUNT 3) | ||
+ | (SIDE BACK) | ||
+ | (SIDE RIGHT) | ||
+ | (RANGE-INVERT 0.5)> | ||
+ | </ | ||
+ | As you can see we have again like in [[playground: | ||
+ | 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 '' | ||
+ | If everything goes as intended the robot will stay in front of his target robot location like seen here. | ||
+ | |||
+ | {{playground: | ||
+ | |||
+ | The robot moved to the target location in the deliver function. | ||
+ | |||
+ | <code lisp> | ||
+ | ;; take a new `? | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or common-fail: | ||
+ | common-fail: | ||
+ | common-fail: | ||
+ | | ||
+ | ? | ||
+ | | ||
+ | | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | | ||
+ | " | ||
+ | |||
+ | ;; look | ||
+ | (exe: | ||
+ | (type turning-towards) | ||
+ | | ||
+ | </ | ||
+ | |||
+ | In the following picture the robot tries to locate the object within nine retires. | ||
+ | |||
+ | {{playground: | ||
+ | |||
+ | After trying to often the method throws an '' | ||
+ | |||
+ | {{playground: | ||
+ | |||
+ | <code lisp> | ||
+ | ;; place | ||
+ | (let ((place-action | ||
+ | (or (when place-action | ||
+ | (let* ((referenced-action-desig | ||
+ | | ||
+ | (?arm | ||
+ | | ||
+ | | ||
+ | | ||
+ | (desig:an action | ||
+ | (type placing) | ||
+ | (arm ?arm) | ||
+ | (object ? | ||
+ | (target ? | ||
+ | (desig:an action | ||
+ | (type placing) | ||
+ | (desig:when ?arm | ||
+ | (arm ?arm)) | ||
+ | (object ? | ||
+ | (target ? | ||
+ | </ | ||
+ | |||
+ | With the valid standing point position for the robot (''? | ||
+ | |||
+ | {{playground: | ||
+ | |||
+ | ... and after turning towards the object (with the coordination ''? | ||
+ | |||
+ | <code lisp> | ||
+ | ;; test if the placing trajectory is reachable and not colliding | ||
+ | (setf place-action (desig: | ||
+ | (proj-reasoning: | ||
+ | (setf place-action (desig: | ||
+ | </ | ||
+ | |||
+ | ... 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: | ||
+ | ? | ||
+ | </ | ||
+ | |||
+ | ... 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 " | ||
+ | (PART-OF KITCHEN)> | ||
+ | (SIDE LEFT) | ||
+ | (SIDE FRONT) | ||
+ | (RANGE-INVERT 0.5)>) | ||
+ | (TYPE BOWL) | ||
+ | (NAME BOWL-1) | ||
+ | (POSE ((:POSE | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (:TRANSFORM | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (: | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (: | ||
+ | #< | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
+ | (TARGET #<A LOCATION | ||
+ | (ON #<A OBJECT | ||
+ | (TYPE COUNTER-TOP) | ||
+ | (URDF-NAME KITCHEN-ISLAND-SURFACE) | ||
+ | (OWL-NAME " | ||
+ | (PART-OF KITCHEN)> | ||
+ | (CONTEXT TABLE-SETTING) | ||
+ | (FOR #<A OBJECT | ||
+ | (TYPE BOWL)>) | ||
+ | (OBJECT-COUNT 3) | ||
+ | (SIDE BACK) | ||
+ | (SIDE RIGHT) | ||
+ | (RANGE-INVERT 0.5)> | ||
+ | </ | ||
+ | |||
+ | After all this we can finally execute the place action. | ||
+ | |||
+ | <code lisp> | ||
+ | (exe: | ||
+ | </ | ||
+ | |||
+ | {{playground: | ||
+ |