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:07] – [Grasp objects] 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 390: Line 392:
 We can execute the arm movement, navigation and head tilt in parallel, since each of them uses different joints of the robot. That's what ''cpl:par'' is for. The function ''park-arms'' performs a motion on the joints of both arms, which brings them into a specific position, so they don't hang around the field of view. The base navigation is achieved by the motion designator trying to reach our ''?grasp-base-pose''. Looking at our target works the same way. We can execute the arm movement, navigation and head tilt in parallel, since each of them uses different joints of the robot. That's what ''cpl:par'' is for. The function ''park-arms'' performs a motion on the joints of both arms, which brings them into a specific position, so they don't hang around the field of view. The base navigation is achieved by the motion designator trying to reach our ''?grasp-base-pose''. Looking at our target works the same way.
  
-To grasp the bottle we need to have its pose in the room. Therefore, we first get the estimated position of the bottle, then perceive it for a more accurate pose. With that resulting perceived object we perform the picking up action. With the torso so far down we might not be able to reach for the bottle, so we need to push the torso up.+To grasp the bottle we need to have its pose in the room. Therefore, we first perceive it and store the result in ''*perceived-object*'':
 <code lisp> <code lisp>
 BTW-TUT> BTW-TUT>
 +(defvar *perceived-object* nil "Object designator returned from perception")
 +(proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment
 +           (cpl:top-level
 +             (setf *perceived-object*
 +                   (pr2-pp-plans::perceive (desig:an object (type bottle))))))
 +</code>
 +
 +With that resulting perceived object we perform the picking up action. With the torso so far down we might not be able to reach for the bottle, so we need to also push the torso up:
 +
 +<code lisp>
 (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment
   (cpl:top-level   (cpl:top-level
-    (let* ((?bottle-desig (desig:an object (type bottle))) +    (let ((?perceived-bottle-desig *perceived-object*))
-           (?perceived-bottle-desig (pr2-pp-plans::perceive ?bottle-desig)))+
       (exe:perform       (exe:perform
        (desig:a motion (type moving-torso) (joint-angle 0.3)))        (desig:a motion (type moving-torso) (joint-angle 0.3)))
Line 432: Line 443:
 ==== Place objects ==== ==== Place objects ====
  
-To place things held in the gripper we first need an object attached to a gripper. Use the previous chapter ''Grasp objects'' to prepare the robot appropriately. The easiest way to place down an object is by calling this code. It will put the bottle down to its original position.+To place things held in the gripper we first need an object attached to a gripper. Use the previous chapter ''Grasp objects'' to prepare the robot appropriately. The easiest way to place down an object is by calling the code below. It will put the bottle down to its original position.
 <code lisp> <code lisp>
 BTW-TUT>  BTW-TUT> 
 (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment
-             (cpl:top-level +  (let ((?perceived-bottle-desig *perceived-object*)) 
-               (exe:perform (desig:an action +    (cpl:top-level 
-                                      (type placing) +      (exe:perform (desig:an action 
-                                      (arm right)))))+                             (type placing) 
 +                             (object ?perceived-bottle-desig) 
 +                             (arm right))))))
 </code> </code>
  
Line 446: Line 459:
 BTW-TUT> BTW-TUT>
 (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment
-             (cpl:top-level +  (cpl:top-level 
-               (let* ((?bottle-desig (desig:an object +    (let ((?perceived-bottle-desig *perceived-object*)) 
-                                               (type bottle))) +      (exe:perform 
-                      (?perceived-bottle-desig (pr2-pp-plans::perceive ?bottle-desig))) +       (desig:a motion (type moving-torso) (joint-angle 0.3))) 
-                 (pr2-pp-plans::drive-and-pick-up-plan ?perceived-bottle-desig :?arm :right))))+      (exe:perform 
 +       (desig:an action 
 +                 (type picking-up) 
 +                 (object ?perceived-bottle-desig) 
 +                 (arm right))))))
 </code> </code>
-Now move the robot to the other table. We use a motion designator for that, who needs a specific pose.+Now move the robot to the other table. We use a motion designator for that, which needs a specific pose.
 <code lisp> <code lisp>
 BTW-TUT> (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment BTW-TUT> (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment
-             (cpl:top-level +           (cpl:top-level 
-               (exe:perform +             (exe:perform 
-                (let ((?pose (cl-tf:make-pose-stamped +              (let ((?pose (cl-tf:make-pose-stamped 
-                              cram-tf:*robot-base-frame* 0.0 +                            cram-tf:*robot-base-frame* 0.0 
-                              (cl-transforms:make-3d-vector -0.15 2.0 0) +                            (cl-transforms:make-3d-vector -0.15 2.0 0) 
-                              (cl-transforms:make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))) +                            (cl-transforms:make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))) 
-                  (desig:a motion (type going) (target (desig:a location (pose ?pose))))))))+                (desig:a motion (type going) (target (desig:a location (pose ?pose))))))))
 </code> </code>
-This seems like a good spot to place the bottle. Let's put it down to coordinates -0.7 2.0 0.861667.+This seems like a good spot to place the bottle. Let's put it down to coordinates defined in ''?drop-pose'':
 <code lisp> <code lisp>
-BTW-TUT> (proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment +BTW-TUT>  
-             (cpl:top-level +(proj:with-projection-environment pr2-proj::pr2-bullet-projection-environment 
-               (let ((?drop-pose (cl-transforms-stamped:make-pose-stamped  +  (cpl:top-level 
-                                  "map" 0.0  +    (let ((?perceived-object-desig *perceived-object*) 
-                                  (cl-transforms:make-3d-vector -0.7d0 2.0d0 0.861667d0) +          (?drop-pose (cl-transforms-stamped:make-pose-stamped  
-                                  (cl-transforms:make-identity-rotation)))) +                       "map" 0.0  
-                 (exe:perform (desig:an action +                       (cl-transforms:make-3d-vector -0.8d0 2.0d0 0.9d0) 
-                                        (type placing) +                       (cl-transforms:make-identity-rotation)))) 
-                                        (arm right) +      (exe:perform (desig:an action 
-                                        (target (desig:a location (pose ?drop-pose))))))))+                             (type placing) 
 +                             (arm right) 
 +                             (object ?perceived-object-desig) 
 +                             (target (desig:a location (pose ?drop-pose))))))))
 </code> </code>
 With these actions you can pick up objects, move the robot and place them at specific points. The scene should now look like this: With these actions you can pick up objects, move the robot and place them at specific points. The scene should now look like this:
Line 486: Line 506:
 BTW-TUT> (init-projection) BTW-TUT> (init-projection)
 </code> </code>
-If you just started your EMACS or loaded the package don't forget to add our extra meshes from resource, otherwise you won't be able to spawn a bottle.+If you just started your Emacs or loaded the package don't forget to add our extra meshes from resource, otherwise you won't be able to spawn a bottle.
 <code lisp> <code lisp>
 BTW-TUT> (add-objects-to-mesh-list) BTW-TUT> (add-objects-to-mesh-list)
Line 492: Line 512:
 Now we need those two objects in the world. Now we need those two objects in the world.
 <code lisp> <code lisp>
-BTW-TUT> (prolog:prolog '(and (btr:bullet-world ?world) +BTW-TUT>  
-              (assert (btr:object ?world :mesh bottle-1 ((-2 -0.9 0.861667d0) (0 0 0 1)) +(prolog:prolog `(and (btr:bullet-world ?world) 
-                                 :mass 0.2 :color (1 0 0) :mesh :bottle)) +                     (assert (btr:object ?world :mesh bottle-1 ((-2 -0.9 0.861667d0) (0 0 0 1)) 
-              (assert (btr:object ?world :mesh bottle-2 ((-0.65 2 0.955) (0 0 0 1)) +                                         :mass 0.2 :color (1 0 0) :mesh :bottle)) 
-                                 :mass 0.2 :color (0 1 0) :mesh :bottle)) +                     (assert (btr:object ?world :mesh bottle-2 ((-0.65 2 0.955) (0 0 0 1)) 
-              (btr:simulate ?world 100)))+                                         :mass 0.2 :color (0 1 0) :mesh :bottle)) 
 +                     (btr:simulate ?world 100)))
 </code>                       </code>                      
-To make the plan shorter we can define some parameters a prior. The *pose-meal-table* is the pose, where the pr2 should stand, when he approaches the green bottle. The other pose, *pose-meal-counter* is the base pose for the counter table, when we want to place the bottle.+To make the plan shorter we can define some parameters a priori. The *pose-meal-table* is the pose, where the pr2 should stand, when he approaches the green bottle. The other pose, *pose-meal-counter* is the base pose for the counter table, when we want to place the bottle.
 <code lisp> <code lisp>
-BTW-TUT> +BTW-TUT>    
 (defparameter *pose-bottle-1* (defparameter *pose-bottle-1*
   (cl-transforms-stamped:make-pose-stamped    (cl-transforms-stamped:make-pose-stamped 
    "map" 0.0     "map" 0.0 
-   (cl-transforms:make-3d-vector -2 -0.9d0 0.861667d0)+   (cl-transforms:make-3d-vector -2 -0.9d0 0.86d0)
    (cl-transforms:make-identity-rotation)))    (cl-transforms:make-identity-rotation)))
 + 
 (defparameter *pose-bottle-2* (defparameter *pose-bottle-2*
   (cl-transforms-stamped:make-pose-stamped    (cl-transforms-stamped:make-pose-stamped 
    "map" 0.0     "map" 0.0 
-   (cl-transforms:make-3d-vector -0.7 1.8 0.955)+   (cl-transforms:make-3d-vector -0.8 2 0.9)
    (cl-transforms:make-identity-rotation)))    (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 523: 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 531: Line 552:
   (unless (assoc :bottle btr::*mesh-files*)   (unless (assoc :bottle btr::*mesh-files*)
     (add-objects-to-mesh-list))     (add-objects-to-mesh-list))
-  (prolog:prolog '(and (btr:bullet-world ?world) +  (prolog:prolog  
-              (assert (btr:object ?world :mesh bottle-1 ((-2 -0.9 0.861667d0) (0 0 0 1)) +   `(and (btr:bullet-world ?world) 
-                                 :mass 0.2 :color (1 0 0) :mesh :bottle)) +         (assert (btr:object ?world :mesh bottle-1 ((-2 -0.9 0.860) (0 0 0 1)) 
-              (assert (btr:object ?world :mesh bottle-2 ((-0.65 2 0.955) (0 0 0 1)) +                             :mass 0.2 :color (1 0 0) :mesh :bottle)) 
-                                 :mass 0.2 :color (0 1 0) :mesh :bottle)) +         (assert (btr:object ?world :mesh bottle-2 ((-0.8 2 0.9) (0 0 0 1)) 
-              (btr:simulate ?world 100)))) +                             :mass 0.2 :color (0 1 0) :mesh :bottle)) 
- +         (btr:simulate ?world 100))))
-(defun move-arm-out-of-sight (&key (arm '(:left :right)))       +
-  (unless (listp arm) +
-    (setf arm (list arm))) +
-  (exe:perform +
-           (let ((?left-configuration-to-go pr2-pp-plans::*pr2-left-arm-out-of-sight-joint-positions*) +
-                 (?right-configuration-to-go pr2-pp-plans::*pr2-right-arm-out-of-sight-joint-positions*))        +
-             (desig:a motion +
-                      (type moving-joints) +
-                      (left-configuration ?left-configuration-to-go) +
-                      (right-configuration ?right-configuration-to-go)))))+
  
 (defun navigate-to (?navigation-goal) (defun navigate-to (?navigation-goal)
Line 556: Line 567:
 (defun look-at (?point-of-interest) (defun look-at (?point-of-interest)
   (exe:perform (desig:a motion   (exe:perform (desig:a motion
-                              (type looking) +                        (type looking) 
-                              (target (desig:a location (pose ?point-of-interest))))))+                        (target (desig:a location (pose ?point-of-interest))))))
  
 (defun get-perceived-bottle-desig () (defun get-perceived-bottle-desig ()
-  (let* ((?bottle-desig (desig:an object +  (let* ((?bottle-desig (desig:an object (type bottle))) 
-                                      (type bottle))) +         (?perceived-bottle-desig (exe:perform 
-             (?perceived-bottle-desig (exe:perform +                                   (desig:a motion 
-                                         (desig:a motion +                                            (type detecting) 
-                                                  (type detecting) +                                            (object ?bottle-desig)))))
-                                                  (object ?bottle-desig)))))+
     ?perceived-bottle-desig))     ?perceived-bottle-desig))
  
 (defun pick-up (?object-designator &optional (?arm :right)) (defun pick-up (?object-designator &optional (?arm :right))
   (exe:perform (desig:an action   (exe:perform (desig:an action
-                                 (type picking-up) +                         (type picking-up) 
-                                 (arm ?arm) +                         (arm ?arm) 
-                                 (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) 
-                   (target (desig:a location (pose ?pose)))))) +                         (object ?object) 
- +                         (target (desig:a location (pose ?pose))))))
-(defun move-to-reach (?object-designator &optional (arm :right)) +
-  (pr2-pp-plans::drive-towards-object-plan ?object-designator :?arm arm))+
 </code> </code>
 Here we go. Now we can use those functions to write down the top-level plan. Here we go. Now we can use those functions to write down the top-level plan.
Line 591: 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.