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:demo:fetch_and_place_solutions [2019/09/04 19:26] – [Exercise 2] invert the robot transform gkazhoyatutorials:demo:fetch_and_place_solutions [2019/09/04 19:54] (current) – [Exercise 1] gkazhoya
Line 16: Line 16:
   (make-pose "map" '((0.700000 0.650000 0.00000) (0.00000 0.00000 0 1))))   (make-pose "map" '((0.700000 0.650000 0.00000) (0.00000 0.00000 0 1))))
  
-(defun search-and-pick-up-object (?obj-type) +(defun move-bottle (bottle-spawn-pose) 
-  (let ((?possible-base-poses `(,*base-pose-near-table-towards-island* ,*base-pose-near-sink-surface*)) +  (spawn-object bottle-spawn-pose) 
-        (?current-base-pose *base-pose-near-table*)) +  (with-simulated-robot 
-    (handle-failure (or object-nowhere-to-be-found +    (let ((?navigation-goal *base-pose-near-table*)) 
-                        object-unreachable) +      (cpl:par 
-         +        ;; Moving the robot near the table.
-        ((let* ((?perceived-bottle (find-object ?obj-type)) +
-                (?grasping-arm :left)) ; or (get-preferred-arm ?perceived-bottle) +
-           ;; We update the value of ?grasping-arm according to what the method used +
-           (setf ?grasping-arm (pick-up-object ?perceived-bottle ?grasping-arm)) +
-           (park-arm ?grasping-arm) +
-           `(,?perceived-bottle ,?grasping-arm))) +
-       +
-      (when (first ?possible-base-poses) +
-        (print "Changing the base to a new location to try finding the object") +
-        (setf ?current-base-pose (first ?possible-base-poses)) +
-        (setf ?possible-base-poses (rest ?possible-base-poses))+
         (perform (an action         (perform (an action
                      (type going)                      (type going)
-                     (target (a location +                     (target (a location  
-                                (pose ?current-base-pose))))) +                                (pose ?navigation-goal))))) 
-        (cpl:retry)) +        (perform (a motion 
-      (print "Exhausted all the locations to search. Cannot find the object") +                    (type moving-torso)  
-      (cpl:fail 'object-unreachable))))+                    (joint-angle 0.3))) 
 +        (park-arms))) 
 +  
 +    (let ((?perceived-bottle 
 +             
 +            (let ((?possible-base-poses `(,*base-pose-near-table-towards-island* 
 +                                          ,*base-pose-near-sink-surface*)) 
 +                  (?current-base-pose *base-pose-near-table*)) 
 +            
 +              (handle-failure (or object-nowhere-to-be-found 
 +                                  object-unreachable) 
 +               
 +                  ((find-object :bottle)) 
 + 
 +                (when (first ?possible-base-poses) 
 +                  (print "Changing the base to a new location to try finding the object") 
 +                  (setf ?current-base-pose (first ?possible-base-poses)) 
 +                  (setf ?possible-base-poses (rest ?possible-base-poses)) 
 +                  (perform (an action 
 +                               (type going) 
 +                               (target (a location 
 +                                          (pose ?current-base-pose))))) 
 +                  (cpl:retry)) 
 +                (print "Exhausted all the locations to search. Cannot find the object") 
 +                (cpl:fail 'object-unreachable)))) 
 + 
 +          (?grasping-arm :right)) 
 +      ;; We update the value of ?grasping-arm according to what the method used 
 +      (setf ?grasping-arm (pick-up-object ?perceived-bottle ?grasping-arm)) 
 +      (park-arm ?grasping-arm) 
 +      ;; Moving the robot near the counter. 
 +      (let ((?nav-goal *base-pose-near-counter*)) 
 +        (perform (an action 
 +                     (type going) 
 +                     (target (a location  
 +                                (pose ?nav-goal)))))) 
 +       ;; Setting the object down on the counter 
 +      (let ((?drop-pose *final-object-destination*)) 
 +        (perform (an action 
 +                     (type placing) 
 +                     (arm ?grasping-arm) 
 +                     (object ?perceived-bottle) 
 +                     (target (a location  
 +                                (pose ?drop-pose)))))) 
 +      (park-arm ?grasping-arm))))
 </code> </code>
  
Line 102: Line 135:
                            (object (an object                             (object (an object 
                                        (type ?object-type))))))                                        (type ?object-type))))))
-           + 
            ;; If the action fails, try the following:            ;; If the action fails, try the following:
            ;; try different look directions until there is none left.            ;; try different look directions until there is none left.
Line 129: Line 162:
         (setf ?current-torso-link-position (first possible-torso-link-positions))         (setf ?current-torso-link-position (first possible-torso-link-positions))
         (setf possible-torso-link-positions (rest possible-torso-link-positions))         (setf possible-torso-link-positions (rest possible-torso-link-positions))
 +        (setf possible-look-directions `(,*downward-look-coordinate*
 +                                         ,*left-downward-look-coordinate*
 +                                         ,*right-downward-look-coordinate*))
 +        (setf ?looking-direction (first possible-look-directions))
         (perform (a motion          (perform (a motion 
                     (type moving-torso)                     (type moving-torso)
Line 137: Line 174:
  
  
-(defun move-bottle (bottle-spawn-pose) 
-  (spawn-object bottle-spawn-pose) 
-  (with-simulated-robot 
-    (let ((?navigation-goal *base-pose-near-table*)) 
-      (cpl:par 
-        ;; Moving the robot near the table. 
-        (perform (an action 
-                     (type going) 
-                     (target (a location  
-                                (pose ?navigation-goal))))) 
-        (perform (a motion 
-                    (type moving-torso)  
-                    (joint-angle 0.3))) 
-        (park-arms))) 
-    (let* ((?perceived-bottle-and-grasping-arm (search-and-pick-up-object :bottle)) 
-           (?grasping-arm (second ?perceived-bottle-and-grasping-arm)) 
-           (?perceived-bottle (first ?perceived-bottle-and-grasping-arm))) 
-      ;; Moving the robot near the counter. 
-      (let ((?nav-goal *base-pose-near-counter*)) 
-        (perform (an action 
-                     (type going) 
-                     (target (a location  
-                                (pose ?nav-goal)))))) 
-       ;; Setting the object down on the counter 
-      (let ((?drop-pose *final-object-destination*)) 
-        (perform (an action 
-                     (type placing) 
-                     (arm ?grasping-arm) 
-                     (object ?perceived-bottle) 
-                     (target (a location  
-                                (pose ?drop-pose)))))) 
-      (park-arm ?grasping-arm)))) 
 </code> </code>