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: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:39] – [Exercise 1] gkazhoya
Line 40: Line 40:
       (print "Exhausted all the locations to search. Cannot find the object")       (print "Exhausted all the locations to search. Cannot find the object")
       (cpl:fail 'object-unreachable))))       (cpl:fail 'object-unreachable))))
 +      
 +
 +(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>
  
Line 102: Line 136:
                            (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 163:
         (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 175:
  
  
-(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>