Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
tutorials:demo:fetch_and_place_solutions [2019/09/04 19:39] – [Exercise 1] gkazhoyatutorials:demo:fetch_and_place_solutions [2019/09/04 19:54] (current) – [Exercise 1] gkazhoya
Line 15: Line 15:
 (defparameter *base-pose-near-sink-surface*  (defparameter *base-pose-near-sink-surface* 
   (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) 
-  (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) 
-         
-        ((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 
-                     (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)))) 
-       
  
 (defun move-bottle (bottle-spawn-pose) (defun move-bottle (bottle-spawn-pose)
Line 56: Line 30:
                     (joint-angle 0.3)))                     (joint-angle 0.3)))
         (park-arms)))         (park-arms)))
-    (let* ((?perceived-bottle-and-grasping-arm (search-and-pick-up-object :bottle)) +  
-           (?grasping-arm (second ?perceived-bottle-and-grasping-arm)) +    (let ((?perceived-bottle 
-           (?perceived-bottle (first ?perceived-bottle-and-grasping-arm)))+             
 +            (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.       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))       (let ((?nav-goal *base-pose-near-counter*))