Tutorial 2: Recovering from Failures
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.
- Tilt the head of the robot downwards
- Try to detect the bottle
- 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.
- 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.
Tutorial 3: More Failure handling
Everything is good so far, even though by design, let's call this a lucky coincidence. For the robot, knowing which arm to use to pick up the bottle is not always enough. There are many positions with which we can grasp objects - from the object's front, back, left, right, etc. So what decides the “side” (left, right, top, bottom) of the object? As you might have guessed, these are defined for objects according to the coordinate reference frame of each object. Every object spawned in the bullet world has its own coordinate axes - even rotationally symmetric objects like the bottle here. (Red:+ve x, Green: +ve y, Blue: +ve z axes)
- +ve x-axis → front, -ve x-axis → back
- +ve y-axis → left, -ve y-axis → right
- +ve z-axis → top, -ve z-axis → bottom
And a grasp means that the gripper of the robot approaches from that side and grasps it. So, a left-side grasp means the robot gripper approaches from the +ve y-axis of the bottle and grasps it.
One might think that since the bottle is a rotationally symmetric object, it doesn't matter which side you approach from. But consider the bottle as a generic object, which does not necessarily have to be symmetric. Each object is represented as a 3D model, which has a specific front, back, and sides according to the coordinate frame that model is defined in. In which case, the side with which the arm approaches the bottle greatly matters, accounting for the configuration of the joints the robot arm will make while trying to grasp - some poses may be unachievable, while others may result in the collision of the arm with the table.
Let's try to create this issue in our scenario, by spawning the bottle in yet another position:
PP-TUT> (spawn-object '((-1.0 -0.75 0.860) (0 0 0 1)))
Now run move-bottle once more, and the output should be something similar to the following:
PP-TUT> (move-bottle '((-1.0 -0.75 0.860) (0 0 0 1))) [(PICK-PLACE PICK-UP) INFO] 1566442996.350: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1566442996.350: Reaching [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1566442996.818: #<POSE-STAMPED FRAME-ID: "torso_lift_link", STAMP: 0.0 #<3D-VECTOR (0.4865934453898557d0 0.39450056642817605d0 -0.2601495772666721d0)> #<QUATERNION (-0.001459550578147173d0 0.006997527088969946d0 0.11667527258396149d0 0.9931443929672241d0)>> is unreachable for EE. Failing. [(PP-PLANS PICK-UP) WARN] 1566442996.818: Manipulation messed up: #<POSE-STAMPED FRAME-ID: "torso_lift_link", STAMP: 0.0 #<3D-VECTOR (0.4865934453898557d0 0.39450056642817605d0 -0.2601495772666721d0)> #<QUATERNION (-0.001459550578147173d0 0.006997527088969946d0 0.11667527258396149d0 0.9931443929672241d0)>> is unreachable for EE. Ignoring. [(PICK-PLACE PICK-UP) INFO] 1566442996.818: Gripping [(PICK-AND-PLACE GRIP) WARN] 1566442996.857: There was no object to grip Retrying [(PICK-AND-PLACE GRIP) WARN] 1566442996.868: No retries left. Propagating up. ; Evaluation aborted on #<GRIPPER-CLOSED-COMPLETELY {100C2C57A3}>.
The robot has failed to grasp again, even though the bottle is well within perception and grasping range.
Note: You might also encounter an error of a different type called “MANIPULATION-POSE-UNREACHABLE”. This is perfectly fine and is also a possible error.
So what went wrong? For this, we have to think back to the definition of our ?grasping-arm. Notice that we always use the right arm to grasp the object, with the hardcoded grasp from the left side of the object. But if you look at the position of the bottle now (refer to the figure below), hardcoding these might not be the best idea at all. After all, we have a perfectly functioning left arm and a bunch of grasp configurations to try.
Not all objects have the same possible grasps. How do we know what grasps are defined in our system for specific objects? Try this function:
PP-TUT> (list-defined-grasps :bottle)
To figure out a good failure handling strategy, let's once again come up with a plan, formulated in natural language:
- Choose the favored arm.
- Get all possible grasp poses for the given type of the object and the arm.
- Choose one grasp from the possible grasp list.
- Try to grasp the object.
- If,
- Grasp succeeds - Continue with the rest of the code
- Grasp Fails - Choose a different grasp pose from the list.
- When no more possible grasp poses, try changing the arm used to grasp Once and try resuming from Step (2)
- When attempted with all arms and grasps, error out.
Let's encapsulate all this in a method called pick-up-object. Open up pick-and-place.lisp once more and add the following code to the file:
(defun pick-up-object (?perceived-object ?grasping-arm) (let* ((?possible-grasps '(:left-side :right-side :front :back)) ;define all possible grasps (?remaining-grasps (copy-list ?possible-grasps)) ;make a copy to work though when trying each grasp (?grasp (first ?remaining-grasps))) ;this is the first one to try (cpl:with-retry-counters ((arm-change-retry 1)) ;there is one alternative arm if the first one fails ;; Outer handle-failure handling arm change (handle-failure object-unreachable ((setf ?remaining-grasps (copy-list ?possible-grasps)) ;make sure to try all possible grasps for each arm (setf ?grasp (first ?remaining-grasps)) ;get the first grasp to try (setf ?remaining-grasps (rest ?remaining-grasps)) ;update the remaining grasps to try ;; Inner handle-failure handling grasp change (handle-failure (or manipulation-pose-unreachable gripper-closed-completely) ;; Try to perform the pick up ((perform (an action (type picking-up) (arm ?grasping-arm) (grasp ?grasp) (object ?perceived-object)))) ;; When pick-up fails this block gets executed (format t "~%Grasping failed with ~a arm and ~a grasp~%" ?grasping-arm ?grasp) ;;(format t "~%Error: ~a~%" e) ;uncomment to see the error message ;; Check if we have any remaining grasps left. ;; If yes, then the block nested to it gets executed, which will ;; set the grasp that is used to the new value and trigger retry (when (first ?remaining-grasps) ;if there is a grasp remaining (setf ?grasp (first ?remaining-grasps)) ;get it (setf ?remaining-grasps (rest ?remaining-grasps)) ;update the remaining grasps to try (format t "Retrying with ~a arm and ~a grasp~%" ?grasping-arm ?grasp) (park-arms) (cpl:retry)) ;; This will get executed when there are no more elements in the ;; ?possible-grasps list. We print the error message and throw a new error ;; which will be caught by the outer handle-failure (print "No more grasp retries left :(") (cpl:fail 'object-unreachable))) ;; This is the failure management of the outer handle-failure call ;; It changes the arm that is used to grasp (format t "Manipulation failed with the ~a arm"?grasping-arm) ;; (print e) ;uncomment if you want to see the error ;; Here we use the retry counter we defined. The value is decremented automatically (cpl:do-retry arm-change-retry ;; if the current grasping arm is right set left, else set right (setf ?grasping-arm (if (eq ?grasping-arm :right) :left :right)) (park-arms) (cpl:retry)) ;; When all retries are exhausted print the error message. (print "No more arm change retries left :(")))) ?grasping-arm) ; function value is the arm that was used to grasp the object
With this, the pick-up-object can now iterate through all possible grasp configurations stored in ?possible-grasps.
Even though this code block looks big, we're mostly doing things that we learned in the previous failure handling scenario. But there are two new things that we do here: First, we have used two handle-failure nested, meaning one handle-failure is called within the other, with the main action nested inside the innermost handle-failure, allowing us to layer our issues. When the program is executed, whenever a failure is encountered, it checks within the inner failure handling clause and retries according to it's conditions, and only upon failure of all its scenarios will it bubble up the error to the outer failure handling clause.
Next is the cpl:with-retry-counters. We discussed earlier that we need to have some conditions to not let the retry be called an infinite number of times and get stuck in a loop. cpl:with-retry-counters is one method of doing this, by allowing the user to specify a number to limit the retries, by allowing the user to initialize a counter with a specified number of retries. This counter is used track of how many times the retry is being called. And whenever you see call to cpl:do-retry with the name of the counter, this means that the retry is done only when the counter has a non-zero value and after a retry is done the counter values is decremented. You have to note that the initial try is not counted towards a retry.
So let's see how both of these help with our situation. The inner failure-handling part will take care of manipulation-pose-unreachable or gripper-closed-completely - both of which are errors which can occur during a failed grasp. The recovery method will iterate through all possible grasp configurations for a given arm. Upon failure of this, the outer failure handling clause will take care of the object-unreachable error thrown by the inner part and it'll change the arm it's trying to grasp the object with and then retry the entire procedure again. The arm change is only required once and thus we use a retry counter called arm-change-retry to do it.
And only upon the failure of all these will the error be bubbled up. The method also returns the grasping arm which it used to pick the object up so that the rest of the code is aware of the arm in which the bottle rests. We have also written appropriate warning statements to be informed about the actions the robot is taking.
Also let's redefine move-bottle again to include these changes, so replace the existing method with the code as shown 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))) (let ((?perceived-bottle (find-object :bottle)) (?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))))
Save the file, compile and switch back to the REPL.
You should see a result that looks like the one below. [Some messages have been suppressed here for readability.]
PP-TUT> (move-bottle '((-1.0 -0.75 0.860) (0 0 0 1))) … [(PICK-PLACE PICK-UP) INFO] 1621250741.518: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621250741.522: Reaching … Grasping failed with RIGHT arm and LEFT-SIDE grasp Retrying with RIGHT arm and RIGHT-SIDE grasp [(PICK-PLACE PICK-UP) INFO] 1621250742.709: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621250742.709: Reaching … Grasping failed with RIGHT arm and RIGHT-SIDE grasp Retrying with RIGHT arm and FRONT grasp [(PICK-PLACE PICK-UP) INFO] 1621250744.157: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621250744.159: Reaching … Grasping failed with RIGHT arm and FRONT grasp Retrying with RIGHT arm and BACK grasp [(PICK-PLACE PICK-UP) INFO] 1621250745.425: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621250745.430: Reaching [(PICK-PLACE PICK-UP) INFO] 1621250746.218: Gripping [(PICK-PLACE PICK-UP) INFO] 1621250746.273: Assert grasp into knowledge base [(PICK-PLACE PICK-UP) INFO] 1621250746.277: Lifting [(PICK-PLACE PLACE) INFO] 1621250746.751: Reaching [(PICK-PLACE PLACE) INFO] 1621250746.961: Putting [(PICK-PLACE PLACE) INFO] 1621250747.064: Opening gripper [(PICK-PLACE PLACE) INFO] 1621250747.142: Retract grasp in knowledge base [(PICK-PLACE PLACE) INFO] 1621250747.185: Retracting
The robot has once again succeeded in grasping the object.
Now let's move the bottle even further away from the robot so that it is out of reach of the right arm. This time, after trying all of the grasp poses with the right arm, it should switch to the left arm.
PP-TUT> (spawn-object '((-0.9 -0.75 0.860) (0 0 0 1)))
You should see a result that looks like the one below, succeeding in grasping the bottle with the first grasp (LEFT-SIDE) using the LEFT arm. [Again, some messages have been suppressed here for readability.]
PP-TUT> (move-bottle '((-0.9 -0.75 0.860) (0 0 0 1))) … [(PICK-PLACE PICK-UP) INFO] 1621251445.989: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621251445.993: Reaching … Grasping failed with RIGHT arm and LEFT-SIDE grasp Retrying with RIGHT arm and RIGHT-SIDE grasp [(PICK-PLACE PICK-UP) INFO] 1621251447.290: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621251447.291: Reaching … Grasping failed with RIGHT arm and RIGHT-SIDE grasp Retrying with RIGHT arm and FRONT grasp [(PICK-PLACE PICK-UP) INFO] 1621251448.443: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621251448.454: Reaching … Grasping failed with RIGHT arm and FRONT grasp Retrying with RIGHT arm and BACK grasp [(PICK-PLACE PICK-UP) INFO] 1621251449.628: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621251449.650: Reaching … [(PICK-PLACE PICK-UP) INFO] 1621251450.450: Gripping [(PICK-AND-PLACE GRIP) WARN] 1621251450.480: There was no object to grip Retrying [(PICK-AND-PLACE GRIP) WARN] 1621251450.491: No retries left. Propagating up. #<CRAM-COMMON-FAILURES:GRIPPER-CLOSED-COMPLETELY {100CD890B3}> Grasping failed with RIGHT arm and BACK grasp "No more grasp retries left :(" #<CRAM-COMMON-FAILURES:OBJECT-UNREACHABLE {100B7A2583}> Manipulation failed with the RIGHT arm [(PICK-PLACE PICK-UP) INFO] 1621251450.606: Opening gripper [(PICK-PLACE PICK-UP) INFO] 1621251450.608: Reaching [(PICK-PLACE PICK-UP) INFO] 1621251451.151: Gripping [(PICK-PLACE PICK-UP) INFO] 1621251451.234: Assert grasp into knowledge base [(PICK-PLACE PICK-UP) INFO] 1621251451.235: Lifting [(PICK-PLACE PLACE) INFO] 1621251451.709: Reaching [(PICK-PLACE PLACE) INFO] 1621251451.892: Putting [(PICK-PLACE PLACE) INFO] 1621251451.988: Opening gripper [(PICK-PLACE PLACE) INFO] 1621251452.027: Retract grasp in knowledge base [(PICK-PLACE PLACE) INFO] 1621251452.074: Retracting
The robot has once again succeeded in grasping the object.
One trend you'll notice in both of the failure handling methods that we wrote is: for one line of execution of robot action, we had to write 10+ lines of code to take care of possible failures. And this is how the programs for robots are written, in which failure handling routines dominate a huge part of it.
