Table of Contents

Solutions to Exercises from Zero prerequisites demo tutorial: Simple fetch and place

Exercise 1

;; Simple solution involves adding some hardcoded base poses to search the bottle
;; Then nesting the find object with another failure handling that iterates different hardcoded
;; base poses to find the object.
 
;; Solution to exercise 1 with static poses
 
(defparameter *base-pose-near-table-towards-island*
  (make-pose "map" '((-1.447d0 0.150d0 0.0d0) (0.0d0 0.0d0 0.7071d0 0.7071d0))))
 
(defparameter *base-pose-near-sink-surface* 
  (make-pose "map" '((0.700000 0.650000 0.00000) (0.00000 0.00000 0 1))))
 
(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
 
            (let ((?possible-base-poses `(,*base-pose-near-table-towards-island*
                                          ,*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))))

Exercise 2

;; Solution for choosing the arm based on the position of the robot
(defun get-preferred-arm (?perceived-object)
  (let* ((obj-name (get-obj-name ?perceived-object))
         (obj-pose (get-current-pose-of-object obj-name))
         (robot-transformation (get-robot-transformation))
         (inverse-robot-transform (inverse-transformation robot-transformation))
         ;; Position of the object relative to the robot
         (obj-in-robot-frame (apply-transformation inverse-robot-transform obj-pose))
         (x (get-x-of-pose obj-in-robot-frame))
         (y (get-y-of-pose obj-in-robot-frame)))
         ;; If the object is in front of the robot
    (if (> x 0)
        ;; if the object is on positive y-axis in front of the robot
        ;; then left, else right
        (if (> y 0)
            :left
            :right)
        ;; If the object is on positive y-axis while behind the robot
        ;; then right, else left
        (if (> y 0)
            :right
            :left))))

Exercise 3

;; Modified find-object which changes torso link during perception failure
 
(defun find-object (?object-type)
  (let* ((possible-look-directions `(,*downward-look-coordinate*
                                     ,*left-downward-look-coordinate*
                                     ,*right-downward-look-coordinate*))
         (?looking-direction (first possible-look-directions))
         (possible-torso-link-positions '(0.1 0.2))
         (?current-torso-link-position 0.3))
    (setf possible-look-directions (rest possible-look-directions))
    ;; Look towards the first direction
    (perform (an action
                 (type looking)
                 (target (a location 
                            (pose ?looking-direction)))))
    ;; Set the first torso link angle
    (perform (a motion
                (type moving-torso)
                (joint-angle ?current-torso-link-position)))
    ;; perception-object-not-found is the error that we get when the robot cannot find the object.
    ;; Now we're wrapping it in a failure handling clause to handle it
    (handle-failure object-nowhere-to-be-found
        ((handle-failure perception-object-not-found
             ;; Try the action
             ((perform (an action
                           (type detecting)
                           (object (an object 
                                       (type ?object-type))))))
 
           ;; If the action fails, try the following:
           ;; try different look directions until there is none left.
           (when possible-look-directions
             (print "Perception error happened! Turning head.")
             ;; Resetting the head to look forward before turning again
             (perform (an action
                          (type looking) 
                          (direction forward)))
             (setf ?looking-direction (first possible-look-directions))
             (setf possible-look-directions (rest possible-look-directions))
             (perform (a motion 
                         (type looking)
                         (target (a location
                                    (pose ?looking-direction)))))
             ;; This statement retries the action again
             (cpl:retry))
           ;; If everything else fails, error out
           ;; Reset the neck before erroring out
           (perform (an action
                        (type looking)
                        (direction forward)))      
           (cpl:fail 'object-nowhere-to-be-found)))
      (when possible-torso-link-positions
        (print "Perception error happened! Changing torso link position")
        (setf ?current-torso-link-position (first 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 
                    (type moving-torso)
                    (joint-angle ?current-torso-link-position)))
        (cpl:retry))
      (print "All Torso link positions exhausted :(")
      (cpl:fail 'object-nowhere-to-be-found))))