Differences

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

Link to this comparison view

Next revision		Previous revision
tutorials:demo:fetch_and_place_solutions [2019/09/04 18:53] – created gkazhoyatutorials:demo:fetch_and_place_solutions [2019/09/04 19:54] (current) – [Exercise 1] gkazhoya
Line 11: Line 11:
  
 (defparameter *base-pose-near-table-towards-island* (defparameter *base-pose-near-table-towards-island*
-  (make-pose "map" '((-1.447d0 -0.150d0 0.0d0) (0.0d0 0.0d0 0.7071d0 0.7071d0))))+  (make-pose "map" '((-1.447d0 0.150d0 0.0d0) (0.0d0 0.0d0 0.7071d0 0.7071d0))))
  
-(setf *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) +(defun move-bottle (bottle-spawn-pose) 
-  (let ((?possible-base-poses `(,*base-pose-near-table-towards-island* ,*base-pose-near-sink-surface*)) +  (spawn-object bottle-spawn-pose) 
-        (?current-base-pose *base-pose-near-table*)) +  (with-simulated-robot 
-    (handle-failure (or object-nowhere-to-be-found +    (let ((?navigation-goal *base-pose-near-table*)) 
-                        object-unreachable) +      (cpl:par 
-       +        ;; Moving the robot near the table. 
-        ((let* ((?perceived-bottle (find-object ?obj-type)) +        (perform (an action
-                (?grasping-arm (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)) +
-        (handle-failure navigation-pose-unreachable +
-            ((perform (an action+
                      (type going)                      (type going)
-                     (target (a location +                     (target (a location  
-                                (pose ?current-base-pose)))))) +                                (pose ?navigation-goal))))) 
-          (when (first ?possible-base-poses) +        (perform (a motion 
-            (setf ?current-base-pose (first ?possible-base-poses)) +                    (type moving-torso)  
-            (setf ?possible-base-poses (rest ?possible-base-poses)) +                    (joint-angle 0.3))) 
-            (cpl:retry)) +        (park-arms))) 
-          (print "No location remaining where the robot can reach successfully") +  
-          (cpl:fail 'navigation-pose-unreachable)) +    (let ((?perceived-bottle 
-        (cpl:retry)) +             
-      (print "Exhausted all the locations to search. Cannot find the object") +            (let ((?possible-base-poses `(,*base-pose-near-table-towards-island* 
-      (cpl:fail 'object-unreachable))))+                                          ,*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. 
 +      (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 54: Line 80:
 ;; 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 109: Line 135:
                            (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 136: Line 162:
         (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 144: Line 174:
  
  
-(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>