Differences
This shows you the differences between two versions of the page.
Next revision | Previous revision | ||
tutorials:demo:fetch_and_place_3 [2021/11/03 17:05] – created arthur | tutorials: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, | 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, | ||
Line 166: | Line 166: | ||
{{: | {{: | ||
+ | |||
+ | |||
+ | ===== 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' | ||
+ | |||
+ | {{: | ||
+ | * +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' | ||
+ | |||
+ | |||
+ | Let's try to create this issue in our scenario, by spawning the bottle in yet another position: | ||
+ | <code lisp> | ||
+ | PP-TUT> (spawn-object ' | ||
+ | </ | ||
+ | Now run '' | ||
+ | <code lisp> | ||
+ | PP-TUT> (move-bottle ' | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1566442996.350: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1566442996.350: | ||
+ | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1566442996.818: | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | Failing. | ||
+ | [(PP-PLANS PICK-UP) WARN] 1566442996.818: | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | Ignoring. | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1566442996.818: | ||
+ | [(PICK-AND-PLACE GRIP) WARN] 1566442996.857: | ||
+ | Retrying | ||
+ | [(PICK-AND-PLACE GRIP) WARN] 1566442996.868: | ||
+ | ; Evaluation aborted on #< | ||
+ | </ | ||
+ | |||
+ | 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 "'' | ||
+ | |||
+ | So what went wrong? For this, we have to think back to the definition of our ''? | ||
+ | |||
+ | {{: | ||
+ | |||
+ | 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) | ||
+ | </ | ||
+ | |||
+ | 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 '' | ||
+ | <code lisp> | ||
+ | (defun pick-up-object (? | ||
+ | (let* ((? | ||
+ | | ||
+ | | ||
+ | | ||
+ | (cpl: | ||
+ | | ||
+ | ;; Outer handle-failure handling arm change | ||
+ | | ||
+ | ((setf ? | ||
+ | (setf ?grasp (first ? | ||
+ | (setf ? | ||
+ | |||
+ | ;; Inner handle-failure handling grasp change | ||
+ | (handle-failure (or manipulation-pose-unreachable gripper-closed-completely) | ||
+ | ;; Try to perform the pick up | ||
+ | | ||
+ | (type picking-up) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | |||
+ | ;; When pick-up fails this block gets executed | ||
+ | | ||
+ | ;; | ||
+ | | ||
+ | ;; 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 ? | ||
+ | (setf ?grasp (first ? | ||
+ | (setf ? | ||
+ | (format t " | ||
+ | (park-arms) | ||
+ | (cpl: | ||
+ | ;; This will get executed when there are no more elements in the | ||
+ | ;; ? | ||
+ | ;; which will be caught by the outer handle-failure | ||
+ | (print | ||
+ | (cpl:fail ' | ||
+ | |||
+ | ;; This is the failure management of the outer handle-failure call | ||
+ | ;; It changes the arm that is used to grasp | ||
+ | | ||
+ | ;; (print e) ; | ||
+ | ;; Here we use the retry counter we defined. The value is decremented automatically | ||
+ | | ||
+ | ;; if the current grasping arm is right set left, else set right | ||
+ | (setf ? | ||
+ | :left | ||
+ | :right)) | ||
+ | (park-arms) | ||
+ | (cpl: | ||
+ | ;; When all retries are exhausted print the error message. | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | With this, the '' | ||
+ | |||
+ | 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 '' | ||
+ | |||
+ | Next is the '' | ||
+ | |||
+ | So let's see how both of these help with our situation. The inner failure-handling part will take care of '' | ||
+ | |||
+ | 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 '' | ||
+ | <code lisp> | ||
+ | (defun move-bottle (bottle-spawn-pose) | ||
+ | (spawn-object bottle-spawn-pose) | ||
+ | (with-simulated-robot | ||
+ | (let ((? | ||
+ | (cpl:par | ||
+ | ;; Moving the robot near the table. | ||
+ | (perform (an action | ||
+ | (type going) | ||
+ | | ||
+ | (pose ? | ||
+ | (perform (a motion | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
+ | (park-arms))) | ||
+ | | ||
+ | (let ((? | ||
+ | (? | ||
+ | ;; We update the value of ? | ||
+ | (setf ? | ||
+ | (park-arm ? | ||
+ | ;; Moving the robot near the counter. | ||
+ | (let ((?nav-goal *base-pose-near-counter*)) | ||
+ | (perform (an action | ||
+ | (type going) | ||
+ | | ||
+ | (pose ? | ||
+ | ;; Setting the object down on the counter | ||
+ | (let ((? | ||
+ | (perform (an action | ||
+ | (type placing) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | (pose ? | ||
+ | (park-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.] | ||
+ | <code lisp> | ||
+ | PP-TUT> (move-bottle ' | ||
+ | … | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250741.518: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250741.522: | ||
+ | … | ||
+ | Grasping failed with RIGHT arm and LEFT-SIDE grasp | ||
+ | Retrying with RIGHT arm and RIGHT-SIDE grasp | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250742.709: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250742.709: | ||
+ | … | ||
+ | Grasping failed with RIGHT arm and RIGHT-SIDE grasp | ||
+ | Retrying with RIGHT arm and FRONT grasp | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250744.157: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250744.159: | ||
+ | … | ||
+ | Grasping failed with RIGHT arm and FRONT grasp | ||
+ | Retrying with RIGHT arm and BACK grasp | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250745.425: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250745.430: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250746.218: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250746.273: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621250746.277: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621250746.751: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621250746.961: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621250747.064: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621250747.142: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621250747.185: | ||
+ | </ | ||
+ | {{: | ||
+ | |||
+ | 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 ' | ||
+ | </ | ||
+ | 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 ' | ||
+ | … | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251445.989: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251445.993: | ||
+ | … | ||
+ | Grasping failed with RIGHT arm and LEFT-SIDE grasp | ||
+ | Retrying with RIGHT arm and RIGHT-SIDE grasp | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251447.290: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251447.291: | ||
+ | … | ||
+ | Grasping failed with RIGHT arm and RIGHT-SIDE grasp | ||
+ | Retrying with RIGHT arm and FRONT grasp | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251448.443: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251448.454: | ||
+ | … | ||
+ | Grasping failed with RIGHT arm and FRONT grasp | ||
+ | Retrying with RIGHT arm and BACK grasp | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251449.628: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251449.650: | ||
+ | … | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251450.450: | ||
+ | [(PICK-AND-PLACE GRIP) WARN] 1621251450.480: | ||
+ | Retrying | ||
+ | [(PICK-AND-PLACE GRIP) WARN] 1621251450.491: | ||
+ | #< | ||
+ | Grasping failed with RIGHT arm and BACK grasp | ||
+ | "No more grasp retries left : | ||
+ | #< | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251450.606: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251450.608: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251451.151: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251451.234: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1621251451.235: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621251451.709: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621251451.892: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621251451.988: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621251452.027: | ||
+ | [(PICK-PLACE PLACE) INFO] 1621251452.074: | ||
+ | </ | ||
+ | {{: | ||
+ | |||
+ | 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. |