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
Last revisionBoth sides next revision
tutorials:advanced:bullet_world [2017/09/18 16:49] – [Construct plans] gkazhoyatutorials:advanced:bullet_world [2017/09/22 09:40] – [Abstract entity descriptions] gkazhoya
Line 1: Line 1:
-//Tested with Cram branch: Cram4, ROS version: Indigo, Ubuntu 14.04//+**//Tested with Cram v0.4.0, ROS version: Kinetic, Ubuntu 16.04//**
  
 ====== Bullet world demonstration ====== ====== Bullet world demonstration ======
Line 70: Line 70:
 ====REPL setup==== ====REPL setup====
  
-Now, let's load the package in the REPL:+Now, let's load the package in the REPL (start the REPL with ''$ roslisp_repl''):
 <code lisp> <code lisp>
 CL-USER> (ros-load:load-system "cram_bullet_world_tutorial" :cram-bullet-world-tutorial) CL-USER> (ros-load:load-system "cram_bullet_world_tutorial" :cram-bullet-world-tutorial)
Line 214: Line 214:
 (btr:set-robot-state-from-joints (btr:set-robot-state-from-joints
  '(("iai_fridge_door_joint"  0.3d0))  '(("iai_fridge_door_joint"  0.3d0))
- (btr:object btr:*current-bullet-world* 'my-kitchen))+ (btr:object btr:*current-bullet-world* :kitchen))
 </code> </code>
  
Line 285: Line 285:
 BTW-TUT> (prolog:prolog '(and (btr:bullet-world ?world) BTW-TUT> (prolog:prolog '(and (btr:bullet-world ?world)
                               (assert (btr:object-pose ?world cram-pr2-description:pr2                               (assert (btr:object-pose ?world cram-pr2-description:pr2
-                                                       ((0.5 1 0) (0 0 1 ))))))+                                                       ((0.5 1 0) (0 0 1 0))))))
 </code> </code>
 and ask the visibility query again: instead of typing, put the cursor where you would type and press ''Ctrl-Up''. This will find previously executed commands. and ask the visibility query again: instead of typing, put the cursor where you would type and press ''Ctrl-Up''. This will find previously executed commands.
Line 296: Line 296:
                 (?mug-pose-stamped (cl-transforms-stamped:pose->pose-stamped "map" 0 mug-pose))                 (?mug-pose-stamped (cl-transforms-stamped:pose->pose-stamped "map" 0 mug-pose))
                 (?mug-location-designator (desig:a location (pose ?mug-pose-stamped)))                 (?mug-location-designator (desig:a location (pose ?mug-pose-stamped)))
-                (to-see-designator (desig:a location (to see)+                (to-see-designator (desig:a location (type visible) 
 +                                                     (for pr2)
                                                      (location ?mug-location-designator))))                                                      (location ?mug-location-designator))))
              (desig:reference to-see-designator))              (desig:reference to-see-designator))
Line 315: Line 316:
        (?the-object (desig:an object (type mug)        (?the-object (desig:an object (type mug)
                                      (at ?on-counter)))                                      (at ?on-counter)))
-       (location-to-see (desig:a location (to see)+       (location-to-see (desig:a location (type visible) 
 +                                          (for pr2)
                                           (object ?the-object))))                                           (object ?the-object))))
     (desig:reference location-to-see))     (desig:reference location-to-see))
Line 521: Line 523:
 <code lisp> <code lisp>
 BTW-TUT>     BTW-TUT>    
 +(defparameter *pose-bottle-1*
 +  (cl-transforms-stamped:make-pose-stamped 
 +   "map" 0.0 
 +   (cl-transforms:make-3d-vector -2 -0.9d0 0.86d0)
 +   (cl-transforms:make-identity-rotation)))
 + 
 +(defparameter *pose-bottle-2*
 +  (cl-transforms-stamped:make-pose-stamped 
 +   "map" 0.0 
 +   (cl-transforms:make-3d-vector -0.8 2 0.9)
 +   (cl-transforms:make-identity-rotation)))
 +
 (defparameter *pose-meal-table* (defparameter *pose-meal-table*
   (cl-tf:make-pose-stamped   (cl-tf:make-pose-stamped
    "map" 0.0    "map" 0.0
-   (cl-tf:make-3d-vector -0.15 1.8 0)+   (cl-tf:make-3d-vector -0.15 2.0 0)
    (cl-tf:make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))    (cl-tf:make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))
  
Line 530: Line 544:
   (cl-transforms-stamped:make-pose-stamped   (cl-transforms-stamped:make-pose-stamped
    "map" 0.0    "map" 0.0
-   (cl-transforms:make-3d-vector -2.1547d0 -0.381d0 0.0d0)+   (cl-transforms:make-3d-vector -1.8547d0 -0.381d0 0.0d0)
    (cl-transforms:axis-angle->quaternion (cl-transforms:make-3d-vector 0 0 1) (/ pi -2))))    (cl-transforms:axis-angle->quaternion (cl-transforms:make-3d-vector 0 0 1) (/ pi -2))))
 </code> </code>
Line 570: Line 584:
                          (object ?object-designator))))                          (object ?object-designator))))
  
-(defun place-down (?pose ?arm)+(defun place-down (?pose ?object ?arm)
   (exe:perform (desig:an action   (exe:perform (desig:an action
                          (type placing)                          (type placing)
                          (arm ?arm)                          (arm ?arm)
 +                         (object ?object)
                          (target (desig:a location (pose ?pose))))))                          (target (desig:a location (pose ?pose))))))
 </code> </code>
Line 584: Line 599:
     (cpl:top-level     (cpl:top-level
       ;; Go to counter top and perceive bottle       ;; Go to counter top and perceive bottle
-      (let ((?navigation-goal pr2-pp-plans::*meal-table-right-base-pose*) +      (let ((?navigation-goal *pose-counter*) 
-            (?ptu-goal pr2-pp-plans::*meal-table-right-base-look-pose*))+            (?ptu-goal  
 +              (cl-transforms-stamped:make-pose-stamped 
 +               "base_footprint" 
 +               0.0 
 +               (cl-transforms:make-3d-vector 0.65335d0 0.076d0 0.758d0) 
 +               (cl-transforms:make-identity-rotation))))
         (cpl:par         (cpl:par
           ;; Move torso up           ;; Move torso up
           (exe:perform           (exe:perform
            (desig:a motion (type moving-torso) (joint-angle 0.3)))            (desig:a motion (type moving-torso) (joint-angle 0.3)))
-          (move-arm-out-of-sight)+          (pr2-pp-plans::park-arms)
           (navigate-to ?navigation-goal))           (navigate-to ?navigation-goal))
         (look-at ?ptu-goal))         (look-at ?ptu-goal))
       ;; Pick up bottle-1 with right arm.       ;; Pick up bottle-1 with right arm.
-      (let* ((?perceived-bottle-desig (get-perceived-bottle-desig))) +      (let ((?perceived-bottle-1 (get-perceived-bottle-desig))) 
-        (move-to-reach ?perceived-bottle-desig :right) +        (pick-up ?perceived-bottle-1 :right) 
-        (pick-up ?perceived-bottle-desig :right)) +        (pr2-pp-plans::park-arms :arm :right) 
-      ;; (move-arm-out-of-sight :arm :right) +        ;; Move to the meal table 
-      ;; Move to the meal table +        (let ((?pose *pose-meal-table*)) 
-      (let ((?pose *pose-meal-table*)) +          (navigate-to ?pose)) 
-        (navigate-to ?pose)) +        ;; Pick up bottle-2 with left arm 
-      ;; Pick up bottle-2 with left arm +        (let ((?perceived-bottle-2 (get-perceived-bottle-desig))) 
-      (let* ((?perceived-bottle-desig (get-perceived-bottle-desig))) +          (pick-up ?perceived-bottle-2 :left) 
-        (move-to-reach ?perceived-bottle-desig :left) +          ;; Move left arm out of sight 
-        (pick-up ?perceived-bottle-desig :left)) +          (pr2-pp-plans::park-arms :arm :left) 
-      ;; Move left arm out of sight +          ;; Place bottle-1 on second table 
-      (move-arm-out-of-sight :arm :left) +          (let ((?drop-pose *pose-bottle-2*)) 
-      ;; Place bottle-1 on second table +            (place-down ?drop-pose ?perceived-bottle-1 :right)) 
-      (let ((?drop-pose *pose-bottle-2*)) +          ;; Move right arm out of sight 
-        (place-down ?drop-pose :right)) +          (pr2-pp-plans::park-arms :arm :right) 
-      ;; Move right arm out of sight +          ;; Move to the counter table  
-      (move-arm-out-of-sight :arm :right) +          (let ((?navigation-goal *pose-counter*)) 
-      ;; Move to the counter table  +            (navigate-to ?navigation-goal)) 
-      (let ((?navigation-goal *pose-counter*)) +          ;; Place bottle-2 on the counter 
-         (navigate-to ?navigation-goal)) +          (let ((?drop-pose *pose-bottle-1*)) 
-      ;; Place bottle-2 on the counter +            (place-down ?drop-pose ?perceived-bottle-2 :left)) 
-      (let ((?drop-pose *pose-bottle-1*)) +          (pr2-pp-plans::park-arms))))))
-        (place-down ?drop-pose :left)) +
-      (move-arm-out-of-sight))))+
 </code> </code>
 Each time you run ''test-switch-two-bottles'' you first need to reset the world with ''(init-projection)''. You can include the call at the top of the test function, but maybe you want to comment some parts out to see how the plan behaves. Each time you run ''test-switch-two-bottles'' you first need to reset the world with ''(init-projection)''. You can include the call at the top of the test function, but maybe you want to comment some parts out to see how the plan behaves.