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:17] – [Exercise 2] cleaned up 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 50: Line 84:
          (obj-pose (get-current-pose-of-object obj-name))          (obj-pose (get-current-pose-of-object obj-name))
          (robot-transformation (get-robot-transformation))          (robot-transformation (get-robot-transformation))
 +         (inverse-robot-transform (inverse-transformation robot-transformation))
          ;; Position of the object relative to the robot          ;; Position of the object relative to the robot
-         (obj-in-robot-frame (apply-transformation robot-transformation obj-pose))+         (obj-in-robot-frame (apply-transformation inverse-robot-transform obj-pose))
          (x (get-x-of-pose obj-in-robot-frame))          (x (get-x-of-pose obj-in-robot-frame))
          (y (get-y-of-pose obj-in-robot-frame)))          (y (get-y-of-pose obj-in-robot-frame)))
Line 101: 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 128: 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 136: 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>