The previous example worked perfectly because we knew and provided the exact coordinates to look for the bottle. This is hardly true for the real-life scenario, especially since we are dealing with a kitchen environment, which is far from being precise compared to a factory floor. What would happen if the bottle was moved a little bit to the right of its previous spawn position? For this, let's change the arguments to reflect this on our method, and let's see what happens.

PP-TUT> (move-bottle '((-2 -0.9 0.860) (0 0 0 1)))

Now, the output will look like this.

PP-TUT>
[(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: Could not find object #<A OBJECT (TYPE BOTTLE)>.
[(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: Could not find object #<A OBJECT (TYPE BOTTLE)>.
[(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: Could not find object #<A OBJECT (TYPE BOTTLE)>.
[(PICK-AND-PLACE PERCEIVE) WARN] 1292688669.674: Could not find object #<A OBJECT (TYPE BOTTLE)>.
; Evaluation aborted on #<CRAM-COMMON-FAILURES:PERCEPTION-OBJECT-NOT-FOUND {1013AC5B93}>.

Press “q” to exit the debugger. If that doesn't help, make sure you're in the debugger window, for that just click on the debugger tab to select it.

Clearly, the robot cannot find the object anymore because even though the robot is near the bottle, it is out of the field of vision of the robot due to the predefined base and object pose in our code. Let's try fixing this issue.

We're going to add some code into fixing the perception in our case, so that the robot would still be able to find the bottle. Let's list down a plan of action for this.

1. Tilt the head of the robot downwards
2. Try to detect the bottle
3. If,
   • successful in finding the bottle - continue with the rest of the code.
   • failed to find the bottle - turn the head to a different configuration (e.g., left/right) and try detecting again.
4. When all possible directions fail, error out.

Let's define two additional parameters to aid us. Open pick-and-place.lisp again and add the following code into it.

(defparameter *left-downward-look-coordinate*
  (make-pose "base_footprint" '((0.65335 0.76 0.758) (0 0 0 1))))
 
(defparameter *right-downward-look-coordinate*
  (make-pose "base_footprint" '((0.65335 -0.76 0.758) (0 0 0 1))))

We defined two coordinates, *left-downward-look-coordinate* and *right-downward-look-coordinate*, in the coordinate frame of our robot's base. These will be alternative directions (left and right respectively) to look at in the case when looking downward fails.

Now we define a method find-object which encapsulates our plan with failure handling:

(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)))
    (setf possible-look-directions (rest possible-look-directions))
    ;; Look towards the first direction
    (perform (an action
                 (type looking)
                 (target (a location 
                            (pose ?looking-direction)))))
 
    ;; 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 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 (an action 
                     (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))))

Let's see what this method does. The handle-failure clause here deals with perception-object-not-found, which, if you remember, was the error raised when the robot couldn't find the bottle. So instead of only looking downwards, the code will now iterate between downwards, left and right, and only upon a failure in all these, will an error be bubbled up - thus increasing our effective field of view. You have to note that cpl:retry in the failure handling part will trigger a retry of the main action, so it's advised to put it under some conditionals so that you won't end up in an endless loop.

Let's try to understand the syntax of handle-failure. The first argument is the type of error message you're trying to catch and work around. The second argument is the list of steps you want to protect from the said error message and the last part are the actions that you perform once you encounter the said error - cpl:retry is called within this block. This looks like the following (Elements under the tag <> are to be replaced with real programming elements):

(handle-failure <the error type that needs to be handled>
    (<all the actions to be performed under normal execution>)
  (<actions that need to be performed if there is an error of the declared type. Call retry if necessary>))

We have used the methods first and rest to iterate between our possible looking directions. first always returns the first element in a list. rest returns a list without including the first element.

PP-TUT> (first '(1 2 3 4))
1
PP-TUT> (rest '(1 2 3 4))
(2 3 4)

In our failure handling code, we use (first possible-look-directions) to get the first element in the possible looking directions list and set it as our new direction to look. Then we reduce our possible directions list by setting it to the rest of the list without the first element (setf possible-look-directions (rest possible-look-directions)). This can be applied to failure handling strategies that iterate on elements of a list as possible fixes.

Now we need to redefine our existing move-bottle to use this method, so modify the existing function with the updated one below

(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)))
    ;; Find and detect the bottle on the table. We use the new method here
    (let ((?perceived-bottle (find-object :bottle))
          (?grasping-arm :right))
      (perform (an action
                   (type picking-up)
                   (arm ?grasping-arm)
                   (grasp left-side)
                   (object ?perceived-bottle)))
      (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))))

Save the changes, compile the file again and switch back to the REPL.

Now run:

PP-TUT> (move-bottle '((-2 -0.9 0.860) (0 0 0 1)))

And once again, you'll find that the robot succeeds in transporting the bottle.

If you get an error that says the manipulation goal is unreachable, that's because the Inverse Kinematics solver is not deterministic and sometimes finds a solution and sometimes does not, so simply press “q” to exit the debugger and try again.