Differences

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

Link to this comparison view

Next revision		Previous revision
Last revisionBoth sides next revision
tutorials:demo:fetch_and_place_solutions [2019/09/04 18:53] – created gkazhoyatutorials:demo:fetch_and_place_solutions [2019/09/04 19:39] – [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))))
  
Line 21: Line 21:
     (handle-failure (or object-nowhere-to-be-found     (handle-failure (or object-nowhere-to-be-found
                         object-unreachable)                         object-unreachable)
-      +        
         ((let* ((?perceived-bottle (find-object ?obj-type))         ((let* ((?perceived-bottle (find-object ?obj-type))
-                (?grasping-arm (get-preferred-arm ?perceived-bottle)))+                (?grasping-arm :left)) ; or (get-preferred-arm ?perceived-bottle)
            ;; We update the value of ?grasping-arm according to what the method used            ;; We update the value of ?grasping-arm according to what the method used
            (setf ?grasping-arm (pick-up-object ?perceived-bottle ?grasping-arm))            (setf ?grasping-arm (pick-up-object ?perceived-bottle ?grasping-arm))
Line 33: Line 33:
         (setf ?current-base-pose (first ?possible-base-poses))         (setf ?current-base-pose (first ?possible-base-poses))
         (setf ?possible-base-poses (rest ?possible-base-poses))         (setf ?possible-base-poses (rest ?possible-base-poses))
-        (handle-failure navigation-pose-unreachable +        (perform (an action
-            ((perform (an action+
                      (type going)                      (type going)
                      (target (a location                      (target (a location
-                                (pose ?current-base-pose)))))) +                                (pose ?current-base-pose)))))
-          (when (first ?possible-base-poses) +
-            (setf ?current-base-pose (first ?possible-base-poses)) +
-            (setf ?possible-base-poses (rest ?possible-base-poses)) +
-            (cpl:retry)) +
-          (print "No location remaining where the robot can reach successfully") +
-          (cpl:fail 'navigation-pose-unreachable))+
         (cpl:retry))         (cpl:retry))
       (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 54: 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 109: 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 136: 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 144: 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>