Differences

This shows you the differences between two versions of the page.

Link to this comparison view

tutorials:intermediate:simple_mobile_manipulation_plan [2019/04/05 11:14]
amar
tutorials:intermediate:simple_mobile_manipulation_plan [2019/07/08 20:09] (current)
gkazhoya [Expanding Failure Management Capabilities]
Line 3: Line 3:
 ====== Simple Mobile Manipulation Plan ====== ====== Simple Mobile Manipulation Plan ======
  
-This tutorial demonstrates how to write a simple ​plan for the mobile manipulation ​using the robot, how to write simple ​error handling and recovery ​behavior ​using a lightweight simulation.+This tutorial demonstrates how to write a simple mobile manipulation ​plan and error handling and recovery ​behaviors for it. 
 +In this tutorial we will be using the Bullet world as a lightweight ​simulator. But as plans in CRAM are independent of the hardware platform, the same plan that can be executed in simulation ​can just as well be executed on the real robot, if we stick to motion and action designators defined in CRAM.
  
-This tutorial assumes that you have a basic understanding of plans, and have undergone the [[tutorials:​intermediate:​bullet_world|Bullet World tutorial]], and are now familiar with manipulating various elements in the Bullet world simulation. ​You can find the code of this tutorial on [[https://​github.com/​cram2/​cram/​tree/​master/​cram_tutorials/​cram_bullet_world_tutorial|Github]].+This tutorial assumes that you have a basic understanding of plans, and have undergone the [[tutorials:​intermediate:​bullet_world|Bullet World tutorial]], and are now familiar with manipulating various elements in the Bullet world simulation. 
 +===== Setup =====
  
 ==== Installing the projection project ==== ==== Installing the projection project ====
Line 15: Line 17:
 ==== Environment Setup ==== ==== Environment Setup ====
  
-In order to be able to demonstrate many different features of the belief state representation we will set up the environment ​of the robot similar to how it is done for the real system: we will need an occupancy map for navigation, an IK solver for manipulation,​ a semantic ​map of the environment with the furniture etc. +For this tutorial we need to set up the environment similar to how it is done for the real system: we will need an occupancy map for navigation, an IK solver for manipulation,​ a map of the static parts of the environment with the furniture etc. 
  
 First, let's set up the environment in our terminal by calling the launch file. It already brings up a ''​roscore''​ from within: First, let's set up the environment in our terminal by calling the launch file. It already brings up a ''​roscore''​ from within:
Line 32: Line 34:
 You'll have your bullet world launched with the PR2 in the kitchen. Now you're good to go. You'll have your bullet world launched with the PR2 in the kitchen. Now you're good to go.
  
-==== Some Useful Designators ====+===== Some Useful Designators ====
 Before we proceed further let's look at some of the designators that are supported by the robot in the bullet world (and this works in the real world robot as well). To refresh your memory on designators,​ visit the tutorial [[tutorials:​beginner:​motion_designators|Creating motion designators for the TurtleSim]]. ​ Before we proceed further let's look at some of the designators that are supported by the robot in the bullet world (and this works in the real world robot as well). To refresh your memory on designators,​ visit the tutorial [[tutorials:​beginner:​motion_designators|Creating motion designators for the TurtleSim]]. ​
 Here are some of the supported motion and action designators:​ Here are some of the supported motion and action designators:​
-  * **Motion Designators** - They describe ​low-level ​motion ​that the robot would take+  * **Motion Designators** - They describe low-level ​atomic motions ​that the robot can execute
-      * <code lisp> (desig:a motion (type moving-torso) (joint-angle ?​angle-val))</​code>​ is a motion for moving the torso of the robot and expects a key ''​joint-angle''​ with an value of the required joint angle.+      * <code lisp> (desig:a motion (type moving-torso) (joint-angle ?​angle-val))</​code>​ is a motion for moving the torso of the robot and expects a key ''​joint-angle''​ with value of the required joint angle.
       * <code lisp> (desig:a motion (type going) (target ?​loc-desig)) </​code>​ is a motion that moves the body of the robot and expects a key ''​target'',​ whose value should be of type ''​location-designator''​.       * <code lisp> (desig:a motion (type going) (target ?​loc-desig)) </​code>​ is a motion that moves the body of the robot and expects a key ''​target'',​ whose value should be of type ''​location-designator''​.
       * <code lisp> (desig:a motion (type looking) (target ?​loc-desig))</​code>​ is a motion that moves the head of the robot to look at the specfied location. It expects a key ''​target''​ with value of type ''​location''​ designator. ​       * <code lisp> (desig:a motion (type looking) (target ?​loc-desig))</​code>​ is a motion that moves the head of the robot to look at the specfied location. It expects a key ''​target''​ with value of type ''​location''​ designator. ​
       * <code lisp> (desig:a motion (type detecting) (object ?​obj-desig)) </​code>​ is a a motion that perceives and detects the specified object. It expects a key ''​object''​ with a value of type ''​object-designator''​       * <code lisp> (desig:a motion (type detecting) (object ?​obj-desig)) </​code>​ is a a motion that perceives and detects the specified object. It expects a key ''​object''​ with a value of type ''​object-designator''​
     * **Action Designators** - These describe the high-level actions which may consist of multiple calls to different low-level motion to carry out a small plan.     * **Action Designators** - These describe the high-level actions which may consist of multiple calls to different low-level motion to carry out a small plan.
-        * <code lisp> (desig:an action (type picking-up) (arm ?grasp-arm) (grasp ?​grasp-pose) (object ?​obj-desig)) </​code>​ is an action that picks an object with the specified arm and grasp pose. It expects an ''​object''​ key with value of type ''​object-designators'',​ an ''​arm''​ with the value specifying the keyword for the arm to choose, and a ''​grasp''​ key whose value specifies the keyword of the grasp pose to pick the object ​up from+        * <code lisp> (desig:an action (type picking-up) (arm ?grasp-arm) (grasp ?​grasp-pose) (object ?​obj-desig)) </​code>​ is an action that picks an object with the specified arm and grasp pose. It expects an ''​object''​ key with value of type ''​object-designator'',​ an ''​arm''​ with the value of type keyword for the arm to choose, and a ''​grasp''​ key whose value specifies the keyword of the grasp pose to pick the object ​with
-        * <code lisp> (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ?​loc-desig)) </​code>​ is an action that places the object specified to the target. It expects ​key ''​arm''​ with the keyword of the arm to use, an ''​object'' ​specifying value of type ''​object-designator''​ and a ''​target''​ key with value of type ''​location-designator''​.+        * <code lisp> (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ?​loc-desig)) </​code>​ is an action that places the object specified to the target. It expects ​an optional ​key ''​arm''​ with the keyword of the arm to use, an ''​object''​ of type ''​object-designator''​ and a ''​target''​ key with the value of type ''​location-designator''​.
  
 We will use these designators throughout this tutorial. We will use these designators throughout this tutorial.
  
-==== Constructing plans ====+===== Constructing plans =====
  
-The goal of this chapter is to write a plan for a simple task pick and place an object from one position to another position in the world - something you'll already be familiar with from the previous tutorial. Let's go ahead and dive into the code:+The goal of this chapter is to write a plan for a simple taskpick and place an object from one position to another position in the world. Let's go ahead and dive into the code:
  
 Any time you want a clean environment,​ you can run: Any time you want a clean environment,​ you can run:
Line 61: Line 64:
   (unless (assoc :bottle btr::​*mesh-files*)   (unless (assoc :bottle btr::​*mesh-files*)
     (add-objects-to-mesh-list))     (add-objects-to-mesh-list))
-  ​(prolog:​prolog +  (btr-utils:spawn-object ​'bottle-1 ​:bottle :color '(1 0 0) :pose '((-1.6 -0.9 0.82) (0 0 0 1))) 
-   ​`(and ​(btr:bullet-world ?world) +  (btr:​simulate ​btr:​*current-bullet-world* 10))
-         ​(assert (btr:object ​?world :mesh bottle-1 +
-                             ((-1.6 -0.9 0.860) (0 0 0 1)+
-                             :mass 0.2 :color (1 0 0) :mesh :bottle)) +
-         ​(btr:​simulate ​?world 100))))+
 BTW-TUT> (spawn-bottle) BTW-TUT> (spawn-bottle)
 </​code>​ </​code>​
  
-You should now see a red bottle on the table. To make our lives easier, let's define some parameters. The *final-object-destination* is the place where the pr2 has to place the red bottle in the end. The ''​*base-pose-near-table*''​ is the pose, where pr2 should stand near the table, when he approaches the bottle and ''​*base-pose-near-counter*''​ is the poseto access ​the the counter to place the bottle.+You should now see a red bottle on the table. To make our lives easier, let's define some parameters. The *final-object-destination* is the place where the pr2 has to place the red bottle in the end. The ''​*base-pose-near-table*''​ is the pose, where pr2 should stand near the table, when he approaches the bottle and ''​*base-pose-near-counter*''​ is the pose to access the counter to place the bottle.
  
 <code lisp> <code lisp>
Line 165: Line 164:
 Now run ''​(move-bottle)''​ and you will see that the robot successfully picks the bottle and places it on the counter. Now run ''​(move-bottle)''​ and you will see that the robot successfully picks the bottle and places it on the counter.
  
-// + 
-Note:- Every time you run ''​move-bottle''​ you need to reset the world with ''​(init-projection)''​. You can include the call at the top of the test function, but maybe you want to comment some parts out to see how the plan behaves.// +===== Recovering from Failures ​===== 
-===== Increasing the Effectiveness by Improving the plan ===== + 
-The previous example worked perfectly ​because, we knew 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,​ 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 redefine our ''​spawn-bottle''​+The previous example worked perfectly, ​because ​we knew 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,​ 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 redefine our ''​spawn-bottle''​
  
 <code lisp> <code lisp>
Line 175: Line 174:
   (unless (assoc :bottle btr::​*mesh-files*)   (unless (assoc :bottle btr::​*mesh-files*)
     (add-objects-to-mesh-list))     (add-objects-to-mesh-list))
-  ​(prolog:​prolog +  (btr-utils:spawn-object ​'bottle-1 ​:bottle :color '(1 0 0) :pose '((-2 -0.9 0.860) (0 0 0 1))) 
-   ​`(and ​(btr:bullet-world ?world) +  (btr:​simulate ​btr:​*current-bullet-world* 10))
-         ​(assert (btr:object ​?world :mesh bottle-1 +
-                             ((-2 -0.9 0.860) (0 0 0 1)+
-                             :mass 0.2 :color (1 0 0) :mesh :bottle)) +
-         ​(btr:​simulate ​?world 100))))+
 </​code>​ </​code>​
-Now try running ''​(move-bottle)''​ again. Don't forget to clean up the projection before running.+Now try running ''​(move-bottle)''​ again.
 And the output will look like this. And the output will look like this.
 <code lisp> <code lisp>
Line 193: Line 188:
 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. ​ 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. ​
  
-=== Recovering from Failures === 
 To understand the syntax and get a refresher on failure handling, you can refer to [[tutorials:​beginner:​failure_handling|Failure Handling]]. To understand the syntax and get a refresher on failure handling, you can refer to [[tutorials:​beginner:​failure_handling|Failure Handling]].
 We're going to add some code into fixing 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. We're going to add some code into fixing 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.
Line 310: Line 304:
 </​code>​ </​code>​
  
-Clean up and run ''​(move-bottle)''​ again, and this time, you'll find that the robot succeeds in transporting the bottle.+Run ''​(move-bottle)''​ again, and this time, you'll find that the robot succeeds in transporting the bottle.
 {{:​tutorials:​intermediate:​btw-tut-found-bottle-again.png?​800|}} {{:​tutorials:​intermediate:​btw-tut-found-bottle-again.png?​800|}}
  
-=== Expanding Failure Management Capabilities ===+===== Expanding Failure Management Capabilities ​=====
 Everything is good so far, but 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. 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 an object model, which has a specific front, back and sides according to its orientation with respect to the bullet world. 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. Everything is good so far, but 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. 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 an object model, which has a specific front, back and sides according to its orientation with respect to the bullet world. 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.
  
Line 321: Line 315:
   (unless (assoc :bottle btr::​*mesh-files*)   (unless (assoc :bottle btr::​*mesh-files*)
     (add-objects-to-mesh-list))     (add-objects-to-mesh-list))
-  ​(prolog:​prolog +  (btr-utils:spawn-object ​'bottle-1 ​:bottle :color '(1 0 0) :pose '((-1.1 -0.75 0.860) (0 0 0 1))) 
-   ​`(and ​(btr:bullet-world ?world) +  (btr:​simulate ​btr:​*current-bullet-world* 10))
-         ​(assert (btr:object ​?world :mesh bottle-1 +
-                             ((-1.1 -0.75 0.860) (0 0 0 1)+
-                             :mass 0.2 :color (1 0 0) :mesh :bottle)) +
-         ​(btr:​simulate ​?world 100))))+
 </​code>​ </​code>​
 Now run ''​move-bottle''​ once more, and the output should be something similar to as below: Now run ''​move-bottle''​ once more, and the output should be something similar to as below:
Line 503: Line 493:
 You can now try spawning the bottle in different points on the table and observing how the robot resolves the arm and grasp for it. Subsequently,​ you will notice that our plans will work as long as the bottle is within reach of the robot. You could try implementing the adjustment of the robot'​s position to facilitate the fetch. You can now try spawning the bottle in different points on the table and observing how the robot resolves the arm and grasp for it. Subsequently,​ you will notice that our plans will work as long as the bottle is within reach of the robot. You could try implementing the adjustment of the robot'​s position to facilitate the fetch.
  
-Since this is a simple tutorial in formulating and understanding mobile plans using CRAM, developing advanced plans and recovery behaviorsis left up to you.+Since this is a simple tutorial in formulating and understanding mobile plans using CRAM, developing advanced plans and recovery behaviors is left up to you. 
 + 
 + 
 +<​html><​!-- 
 +THIS IS ONLY IN CRAM v0.8.0 
  
 === Useful Macros === === Useful Macros ===
Line 598: Line 593:
 </​code>​ </​code>​
 This code will behave exactly like before but all the repetitive logic has been moved to this macro. The first two arguments are always the iterating list (in this case, the list containing possible grasp pose) and the number of retries. The keyword ''​error-object-or-string''​ can accept a warning string or just the plain error object and it will be used for logging. ''​warning-namespace''​ will accept the name in which the logging information is classified into. Since our method rethrows ''​common-fail:​object-unreachable''​ we pass that along into ''​rethrow-failure'',​ and if none is passed, it will rethrow the same error it received. This code will behave exactly like before but all the repetitive logic has been moved to this macro. The first two arguments are always the iterating list (in this case, the list containing possible grasp pose) and the number of retries. The keyword ''​error-object-or-string''​ can accept a warning string or just the plain error object and it will be used for logging. ''​warning-namespace''​ will accept the name in which the logging information is classified into. Since our method rethrows ''​common-fail:​object-unreachable''​ we pass that along into ''​rethrow-failure'',​ and if none is passed, it will rethrow the same error it received.
 +
 +
 +--></​html>​