Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
tutorials:intermediate:fetch_deliver_plans [2019/07/10 10:34] – [fetch] tlipps | tutorials: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:: | + | CL-USER> (urdf-proj:: |
- | | + | " |
- | | + | 0.0 |
- | | + | (cl-transforms: |
- | | + | (cl-transforms: |
</ | </ | ||
Line 35: | Line 35: | ||
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj:: | + | CL-USER> (urdf-proj:: |
</ | </ | ||
Line 45: | Line 45: | ||
(cl-tf: | (cl-tf: | ||
(cl-tf: | (cl-tf: | ||
- | CL-USER> (pr2-proj:: | + | CL-USER> (urdf-proj:: |
#<A OBJECT | #<A OBJECT | ||
(TYPE CUP) | (TYPE CUP) | ||
Line 74: | Line 74: | ||
==== Projection process modules ==== | ==== Projection process modules ==== | ||
- | The PMs are implemented in '' | + | The PMs are implemented in '' |
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: | + | CL-USER> (urdf-proj: |
+ | (let ((?pose (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
| | ||
- | 'pr2-proj::pr2-proj-navigation | + | 'urdf-proj::urdf-proj-navigation |
- | | + | (desig:an motion |
- | " | + | (type going) |
- | (cl-transforms: | + | (pose ?pose))))) |
- | (cl-transforms: | + | |
- | | + | |
- | | + | |
- | (target (desig:a location | + | |
- | | + | |
</ | </ | ||
| | ||
Line 96: | Line 95: | ||
To automatically dispatch motion designators to their correct PMs, there are the '' | To automatically dispatch motion designators to their correct PMs, there are the '' | ||
- | To execute a motion with automatic PM dispatching, | + | To execute a motion with automatic PM dispatching, |
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | (cram-executive: | + | |
- | | + | |
- | " | + | |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | (cram-executive: |
- | (desig:a motion | + | |
- | | + | |
- | (target (desig:a location | + | |
- | | + | |
</ | </ | ||
| | ||
- | '' | + | '' |
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, | 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, | ||
Line 125: | Line 122: | ||
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | (cram-executive: | + | (let ((?pose (cl-transforms-stamped: |
- | | + | " |
- | " | + | (cl-transforms: |
- | (cl-transforms: | + | (cl-transforms: |
- | (cl-transforms: | + | |
- | (desig:an action | + | (desig:an action |
- | (type going) | + | |
- | (target (desig:a location | + | |
- | | + | (pose ? |
</ | </ | ||
Line 140: | Line 137: | ||
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | (cram-executive: | + | |
- | | + | |
- | " | + | |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | (cram-executive: |
- | (desig:an action | + | |
- | (type lifting) | + | (type lifting) |
- | (left-poses (? | + | (left-poses (? |
| | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | (cram-executive: | + | |
- | | + | |
- | " | + | |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | (cram-executive: |
- | (desig:an action | + | |
- | (type reaching) | + | (type reaching) |
- | (left-poses (? | + | (left-poses (? |
</ | </ | ||
| | ||
Line 179: | Line 176: | ||
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | | + | |
- | (let ((?pose (cl-transforms-stamped: | + | (let ((?pose (cl-transforms-stamped: |
- | " | + | " |
- | (cl-transforms: | + | (cl-transforms: |
- | (cl-transforms: | + | (cl-transforms: |
- | (desig:an action | + | (desig:an action |
- | (type looking) | + | (type looking) |
- | (target (desig:a location (pose ? | + | (target (desig:a location (pose ? |
</ | </ | ||
Line 193: | Line 190: | ||
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | | + | |
- | (desig:an action | + | (desig:an action |
- | (type detecting) | + | (type detecting) |
- | (object (desig:an object (type cup)))))) | + | (object (desig:an object (type cup)))))) |
</ | </ | ||
| | ||
Line 203: | Line 200: | ||
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | | + | |
- | | + | |
- | (desig:an action | + | (desig:an action |
- | (type picking-up) | + | (type picking-up) |
- | (object ?obj))))) | + | (object ?obj) |
- | </ | + | (arm left) |
- | + | (grasp | |
- | The missing information is inferred automatically. | + | |
- | + | ||
- | But, we can specify the arm explicitly: | + | |
- | + | ||
- | <code lisp> | + | |
- | CL-USER> (pr2-proj: | + | |
- | (let ((?obj *)) | + | |
- | | + | |
- | (desig:an action | + | |
- | (type picking-up) | + | |
- | (object ?obj) | + | |
- | (arm left))))) | + | |
- | </ | + | |
- | + | ||
- | And the grasp type as well: | + | |
- | + | ||
- | <code lisp> | + | |
- | (pr2-proj: | + | |
- | (let ((?obj *)) | + | |
- | (cram-executive: | + | |
- | (desig:an action | + | |
- | (type picking-up) | + | |
- | (object ?obj) | + | |
- | (arm left) | + | |
- | (grasp | + | |
</ | </ | ||
+ | |||
| | ||
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: | + | CL-USER> (urdf-proj: |
- | (cram-executive: | + | |
- | | + | |
- | " | + | |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | (cram-executive: |
- | (desig:an action | + | |
- | (type going) | + | (type going) |
- | (target (desig:a location | + | (target (desig:a location |
- | | + | |
</ | </ | ||
Line 256: | Line 229: | ||
<code lisp> | <code lisp> | ||
- | CL-USER> (pr2-proj: | + | CL-USER> (urdf-proj: |
- | | + | |
- | (desig:an action | + | (desig:an action |
- | (type placing) | + | (type placing) |
- | (arm left)))) | + | (arm left)))) |
</ | </ | ||
| | ||
- | ==== High-level action plans: fetching and delivering and manipulating environment | + | ===== High-level action plans: fetching and delivering and manipulating environment |
Most high-level plans at the moment are implemented in '' | Most high-level plans at the moment are implemented in '' | ||
We do this by loading the packages '' | We do this by loading the packages '' | ||
<code lisp> | <code lisp> | ||
- | (ros-load: | + | (ros-load: |
- | (ros-load: | + | (ros-load: |
</ | </ | ||
... and call the following functions: | ... and call the following functions: | ||
- | < | + | < |
(in-package : | (in-package : | ||
(roslisp-utilities: | (roslisp-utilities: | ||
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: | + | {{ playground: |
Since we passed '' | Since we passed '' | ||
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: | + | (type (or desig: |
" | " | ||
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: | + | {{ 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. | ||
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, | Since we already searched the object we know the pose of the object which is therefore in the location designator. Nevertheless, | ||
- | {{playground: | + | {{ playground: |
Line 966: | Line 939: | ||
Afterwards call an action to park the arms to end this method and return the object ''? | Afterwards call an action to park the arms to end this method and return the object ''? | ||
- | {{playground: | + | {{ playground: |
The robot fetched the object. | The robot fetched the object. | ||
Line 1006: | Line 979: | ||
<code lisp> | <code lisp> | ||
- | | + | (cpl: |
- | (cpl: | + | (cpl: |
- | (((or desig: | + | (((or desig: |
- | common-fail: | + | common-fail: |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (format NIL " | + | (format NIL " |
- | : | + | : |
- | : | + | : |
- | : | + | : |
</ | </ | ||
- | Again this method uses same thinking as [[playground: | + | Again this method uses same thinking as [[tutorials:intermediate: |
<code lisp> | <code lisp> | ||
;; take a new `? | ;; take a new `? | ||
- | | + | (cpl: |
- | (cpl: | + | (cpl: |
- | (((or common-fail: | + | (((or common-fail: |
- | common-fail: | + | common-fail: |
- | common-fail: | + | common-fail: |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (format NIL " | + | (format NIL " |
- | : | + | : |
- | : | + | : |
- | + | ||
- | ;; navigate | + | |
- | (exe: | + | |
- | (type navigating) | + | |
- | | + | |
+ | ;; navigate | ||
+ | (exe: | ||
+ | (type navigating) | ||
+ | | ||
</ | </ | ||
Line 1063: | Line 1035: | ||
(RANGE-INVERT 0.5)> | (RANGE-INVERT 0.5)> | ||
</ | </ | ||
- | As you can see we have again like in [[playground: | + | As you can see we have again like in [[tutorials:intermediate: |
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 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. | If everything goes as intended the robot will stay in front of his target robot location like seen here. | ||
- | {{playground: | + | {{ playground: |
The robot moved to the target location in the deliver function. | The robot moved to the target location in the deliver function. | ||
<code lisp> | <code lisp> | ||
- | ;; take a new `? | + | ;; take a new `? |
- | (cpl: | + | (cpl: |
- | (cpl: | + | (cpl: |
- | (((or common-fail: | + | (((or common-fail: |
- | common-fail: | + | common-fail: |
- | common-fail: | + | common-fail: |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | : | + | : |
- | : | + | : |
- | : | + | : |
- | | + | |
- | | + | |
- | | + | |
- | (exe: | + | (exe: |
- | | + | |
- | | + | |
</ | </ | ||
In the following picture the robot tries to locate the object within nine retires. | In the following picture the robot tries to locate the object within nine retires. | ||
- | {{playground: | + | {{ playground: |
After trying to often the method throws an '' | After trying to often the method throws an '' | ||
- | {{playground: | + | {{ playground: |
<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:an action | + | (desig:an action |
- | (type placing) | + | (type placing) |
- | (arm ?arm) | + | (arm ?arm) |
- | (object ? | + | (object ? |
- | (target ? | + | (target ? |
- | (desig:an action | + | (desig:an action |
- | (type placing) | + | (type placing) |
- | (desig:when ?arm | + | (desig:when ?arm |
- | (arm ?arm)) | + | (arm ?arm)) |
- | (object ? | + | (object ? |
- | (target ? | + | (target ? |
</ | </ | ||
With the valid standing point position for the robot (''? | With the valid standing point position for the robot (''? | ||
- | {{playground: | + | {{ playground: |
... and after turning towards the object (with the coordination ''? | ... and after turning towards the object (with the coordination ''? | ||
Line 1206: | Line 1178: | ||
</ | </ | ||
- | {{playground: | + | {{ playground: |
+ | |||
+ | ==== 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. |