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_3 [2021/11/03 17:05] – created arthurtutorials:demo:fetch_and_place_3 [2021/11/03 17:09] (current) – arthur
Line 1: Line 1:
-===== Tutorial 2: Recovering from Failures =====+====== 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. 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.
Line 166: Line 166:
  
 {{:tutorials:intermediate:btw-tut-found-bottle-again.png?800|}} {{:tutorials:intermediate:btw-tut-found-bottle-again.png?800|}}
 +
 +
 +===== 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) 
 +
 +{{:tutorials:demo:bottle_axes.png?600|}}
 +  * +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:
 +<code lisp>
 +PP-TUT> (spawn-object '((-1.0 -0.75 0.860) (0 0 0 1)))
 +</code>
 +Now run ''move-bottle'' once more, and the output should be something similar to the following:
 +<code lisp>
 +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}>.
 +</code>
 +
 +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.
 +
 +{{:tutorials:intermediate:btw-tut-grasp-config-fail.png?800|}}
 +
 +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:
 +
 +<code lisp>
 +PP-TUT> (list-defined-grasps :bottle)
 +</code>
 +
 +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:
 +<code lisp>
 +(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
 +</code>
 +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:
 +<code lisp>
 +(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))))
 +</code>
 +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.]
 +<code lisp> 
 +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
 +</code>
 +{{:tutorials:intermediate:btw-tut-grasp-again.png?800|}}
 +
 +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.
 +<code lisp>
 +PP-TUT> (spawn-object '((-0.9 -0.75 0.860) (0 0 0 1)))
 +</code>
 +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.]
 +<code lisp> 
 +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
 +</code>
 +{{:tutorials:demo:btw-tut-grasp-again-right.png?800|}}
 +
 +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.