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:10] – cleaned up exercise 1 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 47: Line 81:
 ;; Solution for choosing the arm based on the position of the robot ;; Solution for choosing the arm based on the position of the robot
 (defun get-preferred-arm (?perceived-object) (defun get-preferred-arm (?perceived-object)
-  (let* ((?obj-name (get-obj-name ?perceived-object)) +  (let* ((obj-name (get-obj-name ?perceived-object)) 
-         (?obj-pose (get-current-pose-of-object ?obj-name)) +         (obj-pose (get-current-pose-of-object obj-name)) 
-         (?robot-transformation (get-robot-transformation-matrix))+         (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 (cl-transforms-stamped:transform  +         (obj-in-robot-frame (apply-transformation inverse-robot-transform obj-pose)) 
-                               ?robot-transformation ?obj-pose)) +         (x (get-x-of-pose obj-in-robot-frame)) 
-         (?x (cl-tf:x (cl-tf:origin ?obj-in-robot-frame))) +         (y (get-y-of-pose obj-in-robot-frame)))
-         (?y (cl-tf:y (cl-tf:origin ?obj-in-robot-frame))))+
          ;; If the object is in front of the robot          ;; If the object is in front of the robot
-    (if (> ?x 0)+    (if (> x 0)
         ;; if the object is on positive y-axis in front of the robot         ;; if the object is on positive y-axis in front of the robot
         ;; then left, else right         ;; then left, else right
-        (if (> ?y 0)+        (if (> y 0)
             :left             :left
             :right)             :right)
         ;; If the object is on positive y-axis while behind the robot         ;; If the object is on positive y-axis while behind the robot
         ;; then right, else left         ;; then right, else left
-        (if (> ?y 0)+        (if (> y 0)
             :right             :right
             :left))))             :left))))
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>