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
tutorials:intermediate:fetch_deliver_plans [2019/07/10 10:49] – tlippstutorials: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::drive (cl-transforms-stamped:make-pose-stamped +CL-USER> (urdf-proj::drive (cl-transforms-stamped:make-pose-stamped 
-                           "map" +                            "map" 
-                           0.0 +                            0.0 
-                           (cl-transforms:make-3d-vector 0.5 0 0) +                            (cl-transforms:make-3d-vector 0.5 0 0) 
-                           (cl-transforms:make-identity-rotation)))+                            (cl-transforms:make-identity-rotation)))
 </code> </code>
                                                        
Line 35: Line 35:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj::move-torso 0.3)+CL-USER> (urdf-proj::move-torso 0.3)
 </code> </code>
  
Line 45: Line 45:
                                         (cl-tf:make-3d-vector 1.5 0.0 1.3)                                         (cl-tf:make-3d-vector 1.5 0.0 1.3)
                                         (cl-tf:make-identity-rotation)))                                         (cl-tf:make-identity-rotation)))
-CL-USER> (pr2-proj::detect (desig:an object (type cup)))+CL-USER> (urdf-proj::detect (desig:an object (type cup)))
 #<A OBJECT #<A OBJECT
     (TYPE CUP)     (TYPE CUP)
Line 74: Line 74:
 ==== Projection process modules ==== ==== Projection process modules ====
  
-The PMs are implemented in ''cram_pr2_projection/src/process-modules.lisp''+The PMs are implemented in ''cram_urdf_projection/src/process-modules.lisp''
  
 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:with-projected-robot+CL-USER> (urdf-proj:with-projected-robot 
 +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
 +                         "map" 0.0 
 +                         (cl-transforms:make-3d-vector 0.75 0 0) 
 +                         (cl-transforms:make-identity-rotation))))
              (cram-process-modules:pm-execute              (cram-process-modules:pm-execute
-              'pr2-proj::pr2-proj-navigation +              'urdf-proj::urdf-proj-navigation 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +              (desig:an motion 
-                            "map" 0.0 +                        (type going) 
-                            (cl-transforms:make-3d-vector 0.75 0 0) +                        (pose ?pose)))))
-                            (cl-transforms:make-identity-rotation)))) +
-                (desig:a motion +
-                         (type going) +
-                         (target (desig:a location +
-                                          (pose ?pose)))))))+
 </code> </code>
                                                                                      
Line 96: Line 95:
 To automatically dispatch motion designators to their correct PMs, there are the ''matching-process-module'' Prolog predicates (implementation is in the same ''process-modules.lisp'' file). To automatically dispatch motion designators to their correct PMs, there are the ''matching-process-module'' Prolog predicates (implementation is in the same ''process-modules.lisp'' file).
  
-To execute a motion with automatic PM dispatching, use ''exe:perform'':+To execute a motion with automatic PM dispatching, use ''exe:perform'' or ''cram-executive:perform'':
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "map" 0.0 
-                            "map" 0.0 +                         (cl-transforms:make-3d-vector 0.75 0 0) 
-                            (cl-transforms:make-3d-vector 0 0 0) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform (desig:an motion 
-                (desig:a motion +                                               (type going) 
-                         (type going) +                                               (pose ?pose)))))
-                         (target (desig:a location +
-                                          (pose ?pose)))))))+
 </code> </code>
                                                                                      
-''pr2-proj:with-projected-robot'' is implemented in ''cram_pr2_projection/src/projection-environment.lisp''+''urdf-proj:with-projected-robot'' is implemented in ''cram_urdf_projection/src/projection-environment.lisp''
  
 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, in order not to mess up the real robot's TF while it is imagining things in projection. 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, in order not to mess up the real robot's TF while it is imagining things in projection.
Line 125: Line 122:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +            (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                          "map" 0.0 
-                            "map" 0.0 +                          (cl-transforms:make-3d-vector 0 0 0) 
-                            (cl-transforms:make-3d-vector 0 0 0) +                          (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +              (cram-executive:perform 
-                (desig:an action +               (desig:an action 
-                          (type going) +                         (type going) 
-                          (target (desig:a location +                         (target (desig:a location 
-                                           (pose ?pose)))))))+                                          (pose ?pose)))))))
 </code> </code>
                                                                                        
Line 140: Line 137:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "base_footprint" 0.0 
-                            "base_footprint" 0.0 +                         (cl-transforms:make-3d-vector 0.7 0.5 0.8) 
-                            (cl-transforms:make-3d-vector 0.7 0.5 0.8) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform  
-                (desig:an action +              (desig:an action 
-                          (type lifting) +                        (type lifting) 
-                          (left-poses (?pose))))))+                        (left-poses (?pose))))))
                                                      
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "base_footprint" 0.0 
-                            "base_footprint" 0.0 +                         (cl-transforms:make-3d-vector 0.7 0.5 0.8) 
-                            (cl-transforms:make-3d-vector 0.7 0.5 0.8) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform  
-                (desig:an action +              (desig:an action 
-                          (type reaching) +                        (type reaching) 
-                          (left-poses (?pose))))))+                        (left-poses (?pose))))))
 </code> </code>
                                                      
Line 179: Line 176:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (cram-executive:perform 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +            (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-                            "base_footprint" 0.0 +                          "base_footprint" 0.0 
-                            (cl-transforms:make-3d-vector 0.7 0.5 0.8) +                          (cl-transforms:make-3d-vector 0.7 0.5 0.8) 
-                            (cl-transforms:make-identity-rotation)))) +                          (cl-transforms:make-identity-rotation)))) 
-                (desig:an action +              (desig:an action 
-                          (type looking) +                        (type looking) 
-                          (target (desig:a location (pose ?pose)))))))+                        (target (desig:a location (pose ?pose)))))))
 </code> </code>
  
Line 193: Line 190:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (cram-executive:perform 
-              (desig:an action +            (desig:an action 
-                        (type detecting) +                      (type detecting) 
-                        (object (desig:an object (type cup))))))+                      (object (desig:an object (type cup))))))
 </code> </code>
                                                  
Line 203: Line 200:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (let ((?obj *)) +           (let ((?obj *)) 
-               (cram-executive:perform +             (cram-executive:perform 
-                (desig:an action +              (desig:an action 
-                          (type picking-up) +                        (type picking-up) 
-                          (object ?obj))))) +                        (object ?obj) 
-</code> +                        (arm left) 
-                           +                        (grasp top)))))
-The missing information is inferred automatically. +
- +
-But, we can specify the arm explicitly: +
- +
-<code lisp> +
-CL-USER> (pr2-proj:with-projected-robot +
-             (let ((?obj *)) +
-               (cram-executive:perform +
-                (desig:an action +
-                          (type picking-up) +
-                          (object ?obj) +
-                          (arm left))))) +
-</code> +
- +
-And the grasp type as well: +
- +
-<code lisp> +
-(pr2-proj:with-projected-robot +
-             (let ((?obj *)) +
-               (cram-executive:perform +
-                (desig:an action +
-                          (type picking-up) +
-                          (object ?obj) +
-                          (arm left) +
-                          (grasp side)))))+
 </code> </code>
 +                         
                                                      
 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:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (let ((?pose (cl-transforms-stamped:make-pose-stamped 
-              (let ((?pose (cl-transforms-stamped:make-pose-stamped +                         "map" 0.0 
-                            "map" 0.0 +                         (cl-transforms:make-3d-vector -0.5 0 0) 
-                            (cl-transforms:make-3d-vector -0.5 0 0) +                         (cl-transforms:make-identity-rotation)))) 
-                            (cl-transforms:make-identity-rotation)))) +             (cram-executive:perform  
-                (desig:an action +              (desig:an action 
-                          (type going) +                        (type going) 
-                          (target (desig:a location +                        (target (desig:a location 
-                                           (pose ?pose)))))))+                                         (pose ?pose)))))))
 </code> </code>
                                                                                        
Line 256: Line 229:
  
 <code lisp> <code lisp>
-CL-USER> (pr2-proj:with-projected-robot +CL-USER> (urdf-proj:with-projected-robot 
-             (cram-executive:perform +           (cram-executive:perform 
-              (desig:an action +            (desig:an action 
-                        (type placing) +                      (type placing) 
-                        (arm left))))+                      (arm left))))
 </code> </code>
                                                  
  
-==== High-level action plans: fetching and delivering and manipulating environment and ... ====+===== High-level action plans: fetching and delivering and manipulating environment =====
  
 Most high-level plans at the moment are implemented in ''cram_pr2_fetch_deliver_plans''. 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 are going to follow the execution of the other high level functions in the deliver function. Most high-level plans at the moment are implemented in ''cram_pr2_fetch_deliver_plans''. 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 are going to 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'' ... We do this by loading the packages ''cram_pr2_pick_and_place'' and ''cram_bullet_world_tutorial'' ...
 <code lisp> <code lisp>
-(ros-load:load-system "cram_pr2_pick_and_place" :cram_pr2_pick_and_place) +(ros-load:load-system "cram_pr2_pick_place_demo" :cram-pr2-pick-place-demo) 
-(ros-load:load-system "cram_bullet_world_tutorial" :cram_bullet_world_tutorial)+(ros-load:load-system "cram_bullet_world_tutorial" :cram-bullet-world-tutorial)
 </code> </code>
 ... and call the following functions: ... and call the following functions:
-<code>+<code lisp>
 (in-package :cram-bullet-world-tutorial) (in-package :cram-bullet-world-tutorial)
 (roslisp-utilities:startup-ros) (roslisp-utilities:startup-ros)
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:start.png?500|}}+{{ playground:start.png?600 |}}
  
 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 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:location-designator null) ?search-location ?robot-location))+(type (or desig:location-designator null) ?search-location ?robot-location)))
 "Searches for `?object-designator' in its likely location `?search-location'. "Searches for `?object-designator' in its likely location `?search-location'.
 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:search-for-obj_robot_loc_object_location_resolved.png?600|}}+{{ 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. 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|}}+{{ 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. This is the costmap after we called the nested location designator with get-location-poses.
  
-{{playground:search-for-robot-moved.png?600|}}+{{ playground:search-for-robot-moved.png?600 |}}
  
 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, we need a pose where 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. Since we already searched the object we know the pose of the object which is therefore in the location designator. Nevertheless, we need a pose where 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.
  
-{{playground:fetch-after-navigating.png?600|}}+{{ playground:fetch-after-navigating.png?600 |}}
  
  
Line 966: Line 939:
 Afterwards call an action to park the arms to end this method and return the object ''?object-designator''. Afterwards call an action to park the arms to end this method and return the object ''?object-designator''.
  
-{{playground:fetched-obj.png?&600|}}+{{ playground:fetched-obj.png?&600 |}}
  
 The robot fetched the object. The robot fetched the object.
Line 1066: Line 1039:
 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:delivering.png?600|}}+{{ playground:delivering.png?600 |}}
  
 The robot moved to the target location in the deliver function. The robot moved to the target location in the deliver function.
Line 1095: Line 1068:
 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:put-it-down.png?600|}}+{{ playground:put-it-down.png?600 |}}
  
 After trying to often the method throws an ''common-fail:object-undeliverable'' error, which will be catched in the 'outer-error-handling' snippet mentioned above. Therefore, the the method will then try to navigate again the robot to a new ''?target-robot-location''. This means ''get-location-poses'' will be called again. After trying to often the method throws an ''common-fail:object-undeliverable'' error, which will be catched in the 'outer-error-handling' snippet mentioned above. Therefore, the the method will then try to navigate again the robot to a new ''?target-robot-location''. This means ''get-location-poses'' will be called again.
  
-{{playground:retry-rob-loc.png?600|}}+{{ playground:retry-rob-loc.png?600 |}}
  
 <code lisp> <code lisp>
Line 1126: Line 1099:
 With the valid standing point position for the robot (''?target-robot-location'') ... With the valid standing point position for the robot (''?target-robot-location'') ...
  
-{{playground:found-rob-loc.png?600|}}+{{ playground:found-rob-loc.png?600 |}}
  
 ... and after turning towards the object (with the coordination ''?target-location''), we can assemble the placing action... ... and after turning towards the object (with the coordination ''?target-location''), we can assemble the placing action...
Line 1205: Line 1178:
 </code> </code>
  
-{{playground:delivered.png?direct&600|}}+{{ playground:delivered.png?direct&600 |}}
  
 ==== transport ==== ==== transport ====