Differences

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

Link to this comparison view

tutorials:demo:fetch_and_place_solutions [2019/09/04 21:39]
gkazhoya [Exercise 1]
tutorials:demo:fetch_and_place_solutions [2019/09/04 21:54] (current)
gkazhoya [Exercise 1]
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*))