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/15 15:57] – [Geometric reasoning] 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 281: Line 281:
 </code> </code>
  
-Let's move the object in the visibility range of PR2:+Let's move PR2 such that the cup is in its visibility range:
 <code lisp> <code lisp>
 BTW-TUT> (prolog:prolog '(and (btr:bullet-world ?world) BTW-TUT> (prolog:prolog '(and (btr:bullet-world ?world)
-                              (assert (btr:object-pose ?world mug-1 ((1.5 0 1.2) (0 0 0 1))))))+                              (assert (btr:object-pose ?world cram-pr2-description:pr2 
 +                                                       ((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 293: Line 294:
 <code lisp> <code lisp>
 BTW-TUT> (let* ((mug-pose (btr:pose (btr:object btr:*current-bullet-world* 'mug-1))) BTW-TUT> (let* ((mug-pose (btr:pose (btr:object btr:*current-bullet-world* 'mug-1)))
-                  (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-pose-designator (desig:make-designator :location `((:pose ,mug-pose-stamped)))) +                (?mug-location-designator (desig:a location (pose ?mug-pose-stamped))) 
-                  (to-see-designator (desig:make-designator :location `((:to :see) +                (to-see-designator (desig:a location (type visible) 
-                                                                        (:location ,mug-pose-designator)))))+                                                     (for pr2) 
 +                                                     (location ?mug-location-designator))))
              (desig:reference to-see-designator))              (desig:reference to-see-designator))
 #<CL-TRANSFORMS-STAMPED:POSE-STAMPED  #<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
Line 310: Line 312:
 <code lisp> <code lisp>
 BTW-TUT>  BTW-TUT> 
-(let* ((on-counter (desig:make-designator :location `((:on "CounterTop") +(let* ((?on-counter (desig:a location (on "CounterTop") 
-                                                (:name "iai_kitchen_kitchen_island_counter_top")))) +                                       (name "iai_kitchen_kitchen_island_counter_top"))) 
-       (the-object (desig:make-designator :object `((:type :mug) +       (?the-object (desig:an object (type mug) 
-                                              (:at ,on-counter)))) +                                     (at ?on-counter))) 
-       (pos-to-see (desig:make-designator :location `((:to :see) +       (location-to-see (desig:a location (type visible) 
-                                                (:object ,the-object))))) +                                          (for pr2) 
-    (desig:reference pos-to-see))+                                          (object ?the-object)))) 
 +    (desig:reference location-to-see))
 #<CL-TRANSFORMS-STAMPED:POSE-STAMPED  #<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
    FRAME-ID: "map", STAMP: 0.0    FRAME-ID: "map", STAMP: 0.0
Line 323: Line 326:
 </code> </code>
  
-Here, as we didn't specifically say which pose we are trying to perceive the robot randomly samples one pose from all the poses on the kitchen island table and generates a distribution to perceive that pose, i.e., it looks in the general direction of the table. If you look at the debug window now you can see a gaussian area near the table. We can use that pose to move the robot there.+Here, as we didn't specifically say which pose we are trying to perceive: the robot randomly samples one pose from all the poses on the kitchen island table and generates a distribution to perceive that pose, i.e., it looks in the general direction of the table. If you look at the debug window now you can see an area that looks like a part of a Gaussian bell near the table. We can use that pose to move the robot there.
  
 <code lisp> <code lisp>
Line 332: Line 335:
 ==== Grasp objects ==== ==== Grasp objects ====
  
-We need a clean environment for this, so restart the demo with+We need a clean environment for this, so let's clean the world:
 <code lisp> <code lisp>
 BTW-TUT> (roslisp-utilities:startup-ros) BTW-TUT> (roslisp-utilities:startup-ros)
 </code> </code>
-Now we need an object to grasp. For that we will use our own mesh of a bottle loaded from resources. +From now on we will use the utility functions from ''cram_bullet_reasoning_utilities'' package, to save time on writing lengthy Prolog queries. 
 + 
 +We need an object to grasp. For that we will use a mesh of a bottle loaded from the ''resources'' subdirectory of the tutorial.
 <code lisp> <code lisp>
 BTW-TUT> (add-objects-to-mesh-list) BTW-TUT> (add-objects-to-mesh-list)
Line 342: Line 347:
 Then we spawn an object of type bottle and move it onto the table. Then we spawn an object of type bottle and move it onto the table.
 <code lisp> <code lisp>
-BTW-TUT> (btr-utils:spawn-object :bottle-1 :bottle) +BTW-TUT> (btr-utils:spawn-object 'bottle-1 :bottle) 
-BTW-TUT> (btr-utils:move-object :bottle-1 (cl-transforms:make-pose +BTW-TUT> (btr-utils:move-object 'bottle-1 
-                                    (cl-transforms:make-3d-vector -2 -0.9 0.861667d0) +                                (cl-transforms:make-pose 
-                                    (cl-transforms:make-identity-rotation)))+                                 (cl-transforms:make-3d-vector -2 -0.9 0.83) 
 +                                 (cl-transforms:make-identity-rotation)))
 </code> </code>
-Lastly we simulate the world for 100 seconds to make sure, nothing moves unexpectedly on runtime.+Lastly we simulate the world for 10 seconds to make sure, nothing moves unexpectedly on runtime.
 <code lisp> <code lisp>
 BTW-TUT> (btr:simulate btr:*current-bullet-world* 100) BTW-TUT> (btr:simulate btr:*current-bullet-world* 100)
 </code> </code>
-From now on it's okay to move the robot. Before we grasp the bottle however we have to prepare the PR2, nothing fancy, just three steps: move the arms out of sight, navigate the base in front of the bottle and look at the bottle. The point we want the base to navigate to can be hard coded and saved temporarily.+Before we grasp the bottle, let's first prepare the PR2, just three steps: move the arms out of sight, navigate the base in front of the bottle and look at the bottle. The point we want the base to navigate to can be hard coded and saved temporarily.
 <code lisp> <code lisp>
 BTW-TUT> (setf ?grasp-base-pose  BTW-TUT> (setf ?grasp-base-pose 
-(cl-transforms-stamped:make-pose-stamped +               (cl-transforms-stamped:make-pose-stamped 
-   "map" +                "map" 
-   0.0 +                0.0 
-   (cl-transforms:make-3d-vector -1.8547d0 -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>
-The same thing can be done with the point, we want to look at.+The same thing can be done with the point we want to look at.
 <code lisp> <code lisp>
 BTW-TUT> (setf ?grasp-look-pose  BTW-TUT> (setf ?grasp-look-pose 
-(cl-transforms-stamped:make-pose-stamped +               (cl-transforms-stamped:make-pose-stamped 
-   "base_footprint" +                "base_footprint" 
-   0.0 +                0.0 
-   (cl-transforms:make-3d-vector 0.65335d0 0.076d0 0.758d0) +                (cl-transforms:make-3d-vector 0.65335d0 0.076d0 0.758d0) 
-   (cl-transforms:make-identity-rotation)))+                (cl-transforms:make-identity-rotation)))
 </code> </code>
-As usual we need a top-level context to start the execution of cram plans. Besides that we also use a macro to determine, that the demo should be executed in simulation, not on the real robot. Putting that together we end up with this plan:+To execute any plan in CRAM, we need a top-level context. Besides that we also use a macro to specify that the demo should be executed in simulation, not on the real robot. Putting that together we end up with this plan:
 <code lisp> <code lisp>
 BTW-TUT> BTW-TUT>
Line 375: Line 382:
   (cpl:top-level    (cpl:top-level 
     (cpl:par     (cpl:par
-      (pr2-pp-plans::move-pr2-arms-out-of-sight)+      (pr2-pp-plans::park-arms)
       (exe:perform (desig:a motion       (exe:perform (desig:a motion
                             (type going)                             (type going)
Line 383: Line 390:
                             (target (desig:a location (pose ?grasp-look-pose))))))))                             (target (desig:a location (pose ?grasp-look-pose))))))))
 </code> </code>
-We actually 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 ''move-pr2-arms-out-of-sight'' performs motions on the joints of both arms, which bring 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 perceive it and store the result in ''*perceived-object*'': 
 +<code lisp> 
 +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:
  
-With the torso so far down we won't be able to reach for the bottle, so we need to push him up. To grasp the bottle we also need 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 pose we start the plan ''drive-and-pick-up-plan''. Remember, that you can jump to any definition by pressing Alt and dot (or ''Alt,.''). Look into those plans at your leisure. 
 <code lisp> <code lisp>
-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))) +
-           (?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)))
-      (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 the PR2 should have the bottle in his right gripper. Now the PR2 should have the bottle in his right gripper.
 {{:tutorials:advanced:bullet_world_bottle_in_gripper.png?800|}} {{:tutorials:advanced:bullet_world_bottle_in_gripper.png?800|}}
  
 +<html><!--
 Let's take a quick look into the ''drive-and-pick-up-plan'' that caused the last movement. Let's take a quick look into the ''drive-and-pick-up-plan'' that caused the last movement.
 <code lisp> <code lisp>
Line 417: Line 436:
                            (object ?object-designator)))))                            (object ?object-designator)))))
 </code> </code>
-Here he first calls a function to get the base in a position optimized for grasping with a specific arm. After that he again looks at the object and finally picks it up. To see which action designator is used for picking up the object we need a good search. Use ''"Alt,x"'', type ''rgrep'' for full-text search, hit enter, then use ''":type :picking-up"'' as search string. The code we are looking for is in ''cram_pr2/cram_pr2_pick_place_plans/src/pick-place-designators.lisp''. Here the action designator is broken down to atomic action designators, that will be resolved depending on the machine that we choose to run on, either the simulation or the real robot. +Here he first calls a function to get the base in a position optimized for grasping with a specific arm. After that he again looks at the object and finally picks it up.  
 +--></html> 
 + 
 +To see which action designator is used for picking up the object we can use a grep search. Use ''"Alt-x"'', type ''rgrep'' for full-text search, hit enter, then use ''":type :picking-up"'' as search string. The code we are looking for is in ''cram_pr2/cram_pr2_pick_place_plans/src/pick-place-designators.lisp''. Here the action designator is broken down to atomic action designators, that will be resolved depending on the hardware that we choose to run on, either the simulation or the real robot. 
  
 ==== 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 435: 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 475: 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 481: 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 512: 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 520: 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 545: 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 580: 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.