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/16 09:30] – [Low-level projection functionality] tlippstutorials:intermediate:fetch_deliver_plans [2022/07/04 12:31] (current) – [search-for-object] khoshnam
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>
                                                  
Line 269: Line 242:
 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 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,