Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revisionNext revisionBoth sides next revision | ||
tutorials:intermediate:simple_mobile_manipulation_plan [2019/07/08 17:30] – [Environment Setup] gkazhoya | tutorials:intermediate:simple_mobile_manipulation_plan [2022/04/14 09:22] – [Expanding Failure Management Capabilities] schimpf | ||
---|---|---|---|
Line 6: | Line 6: | ||
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. | 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: | + | This tutorial assumes that you have a basic understanding of plans, and have undergone the [[tutorials: |
+ | ===== Setup ===== | ||
==== Installing the projection project ==== | ==== Installing the projection project ==== | ||
Line 33: | 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, | 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, | ||
Here are some of the supported motion and action designators: | Here are some of the supported motion and action designators: | ||
- | * **Motion Designators** - They describe | + | * **Motion Designators** - They describe low-level |
- | * <code lisp> (desig:a motion (type moving-torso) (joint-angle ? | + | * <code lisp> (desig:a motion (type moving-torso) (joint-angle ? |
* <code lisp> (desig:a motion (type going) (target ? | * <code lisp> (desig:a motion (type going) (target ? | ||
* <code lisp> (desig:a motion (type looking) (target ? | * <code lisp> (desig:a motion (type looking) (target ? | ||
- | * <code lisp> (desig:a motion (type detecting) (object ? | + | * <code lisp> (desig:a motion (type detecting) (object ? |
+ | (desig:a motion (type detecting) (desig:an object (type bowl))) </ | ||
+ | #<A OBJECT | ||
+ | (TYPE BOWL) | ||
+ | (NAME BOWL-1) | ||
+ | (POSE ((:POSE | ||
+ | #< | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | | ||
+ | #< | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | | ||
+ | #< | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | | ||
+ | #< | ||
+ | | ||
+ | #< | ||
+ | #< | ||
* **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 ? | + | * <code lisp> (desig:an action (type picking-up) (arm ?grasp-arm) (grasp ?grasp-pose-identifier) (object ? |
- | * <code lisp> (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ? | + | * <code lisp> (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ? |
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 task: pick 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, | Any time you want a clean environment, | ||
Line 62: | Line 88: | ||
(unless (assoc :bottle btr:: | (unless (assoc :bottle btr:: | ||
(add-objects-to-mesh-list)) | (add-objects-to-mesh-list)) | ||
- | | + | (btr-utils:spawn-object |
- | | + | (btr: |
- | | + | |
- | ((-1.6 -0.9 0.860) (0 0 0 1)) | + | |
- | :mass 0.2 :color (1 0 0) :mesh :bottle)) | + | |
- | | + | |
BTW-TUT> (spawn-bottle) | BTW-TUT> (spawn-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 '' | + | 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 '' |
<code lisp> | <code lisp> | ||
BTW-TUT> | BTW-TUT> | ||
- | (defparameter *final-object-destination* | + | (defparameter *final-object-destination* |
(cl-transforms-stamped: | (cl-transforms-stamped: | ||
" | " | ||
| | ||
- | | + | |
(defparameter *base-pose-near-table* | (defparameter *base-pose-near-table* | ||
Line 113: | Line 135: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 121: | Line 143: | ||
(pp-plans:: | (pp-plans:: | ||
;; Moving the robot near the table. | ;; Moving the robot near the table. | ||
- | (exe: | + | (exe: |
(type going) | (type going) | ||
(target (desig:a location | (target (desig:a location | ||
Line 127: | Line 149: | ||
;; Looking towards the bottle before perceiving. | ;; Looking towards the bottle before perceiving. | ||
(let ((? | (let ((? | ||
- | (exe: | + | (exe: |
(type looking) | (type looking) | ||
(target (desig:a location | (target (desig:a location | ||
Line 146: | Line 168: | ||
;; Moving the robot near the counter. | ;; Moving the robot near the counter. | ||
(let ((?nav-goal *base-pose-near-counter*)) | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (exe: | + | (exe: |
(type going) | (type going) | ||
(target (desig:a location | (target (desig:a location | ||
Line 162: | Line 184: | ||
</ | </ | ||
- | Note that the plan is nested under '' | + | Note that the plan is nested under '' |
Now run '' | Now run '' | ||
- | // | + | |
- | Note:- Every time you run '' | + | ===== Recovering from Failures |
- | ===== Increasing the Effectiveness by Improving the plan ===== | + | |
- | The previous example worked perfectly | + | The previous example worked perfectly, |
<code lisp> | <code lisp> | ||
Line 176: | Line 198: | ||
(unless (assoc :bottle btr:: | (unless (assoc :bottle btr:: | ||
(add-objects-to-mesh-list)) | (add-objects-to-mesh-list)) | ||
- | | + | (btr-utils:spawn-object |
- | | + | (btr: |
- | | + | |
- | ((-2 -0.9 0.860) (0 0 0 1)) | + | |
- | :mass 0.2 :color (1 0 0) :mesh :bottle)) | + | |
- | | + | |
</ | </ | ||
- | Now try running '' | + | Now try running '' |
And the output will look like this. | And the output will look like this. | ||
<code lisp> | <code lisp> | ||
Line 194: | Line 212: | ||
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: | To understand the syntax and get a refresher on failure handling, you can refer to [[tutorials: | ||
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 229: | Line 246: | ||
(setf preferred-arm :LEFT)) | (setf preferred-arm :LEFT)) | ||
preferred-arm)) | preferred-arm)) | ||
- | | + | |
(defun find-object (? | (defun find-object (? | ||
(let* ((possible-look-directions `(, | (let* ((possible-look-directions `(, | ||
Line 236: | Line 253: | ||
| | ||
(setf possible-look-directions (cdr possible-look-directions)) | (setf possible-look-directions (cdr possible-look-directions)) | ||
- | (exe: | + | (exe: |
(type looking) | (type looking) | ||
(target (desig:a location | (target (desig:a location | ||
Line 246: | Line 263: | ||
(when possible-look-directions | (when possible-look-directions | ||
| | ||
- | | + | |
(type looking) | (type looking) | ||
| | ||
(setf ? | (setf ? | ||
(setf possible-look-directions (cdr possible-look-directions)) | (setf possible-look-directions (cdr possible-look-directions)) | ||
- | | + | |
(type looking) | (type looking) | ||
| | ||
Line 269: | Line 286: | ||
Let us also update our '' | Let us also update our '' | ||
<code lisp> | <code lisp> | ||
- | (defun move-bottle () | + | (defun move-bottle () |
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 279: | Line 296: | ||
(pp-plans:: | (pp-plans:: | ||
;; Moving the robot near the table. | ;; Moving the robot near the table. | ||
- | (exe: | + | (exe: |
(type going) | (type going) | ||
(target (desig:a location | (target (desig:a location | ||
Line 294: | Line 311: | ||
;; Moving the robot near the counter. | ;; Moving the robot near the counter. | ||
(let ((?nav-goal *base-pose-near-counter*)) | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (exe: | + | (exe: |
(type going) | (type going) | ||
(target (desig:a location | (target (desig:a location | ||
(pose ? | (pose ? | ||
+ | |||
(coe: | (coe: | ||
;; Setting the object down on the counter | ;; Setting the object down on the counter | ||
Line 309: | Line 326: | ||
(pose ? | (pose ? | ||
(pp-plans:: | (pp-plans:: | ||
+ | |||
</ | </ | ||
- | Clean up and run '' | + | Run '' |
{{: | {{: | ||
- | === 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' | 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' | ||
- | Let's try to visualize this issue, by spawning the bottle in yet another position: | + | Let's try to visualize this issue, by spawning the bottle in yet another position |
<code lisp> | <code lisp> | ||
- | (defun spawn-bottle () | + | (defparameter *base-pose-near-table* |
+ | (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
+ | |||
+ | (defun spawn-bottle () | ||
(unless (assoc :bottle btr:: | (unless (assoc :bottle btr:: | ||
(add-objects-to-mesh-list)) | (add-objects-to-mesh-list)) | ||
- | | + | (btr-utils:spawn-object |
- | | + | (btr: |
- | | + | |
- | ((-1.1 | + | |
- | :mass 0.2 :color (1 0 0) :mesh :bottle)) | + | |
- | | + | |
</ | </ | ||
- | Now run '' | + | Now run '' |
<code lisp> | <code lisp> | ||
BTW-TUT> (move-bottle) | BTW-TUT> (move-bottle) | ||
- | [(PICK-PLACE PICK-UP) INFO] 1550502686.470: Opening gripper | + | |
- | [(PICK-PLACE PICK-UP) INFO] 1550502686.470: Reaching | + | [(PICK-PLACE PICK-UP) INFO] 1649925566.714: Looking |
- | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1550502686.797: #< | + | [(PICK-PLACE PICK-UP) INFO] 1649925566.758: Opening gripper and reaching |
+ | [(PERFORM ACTION) INFO] 1649925566.760: | ||
+ | [(URDF-PROJ MOVE-TCP) ERROR] 1649925567.115: #< | ||
| | ||
- | #< | + | #< |
- | #< | + | #< |
+ | Failing. | ||
+ | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1649925567.119: | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
Ignoring. | Ignoring. | ||
- | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] | + | [(PERFORM MOTION) INFO] 1649925567.125: |
+ | #<A MOTION | ||
+ | (TYPE MOVING-TCP) | ||
+ | (RIGHT-POSE #< | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | (COLLISION-MODE ALLOW-ALL) | ||
+ | (MOVE-BASE T)> | ||
+ | [(URDF-PROJ MOVE-TCP) ERROR] | ||
| | ||
- | #< | + | #< |
- | #< | + | #< |
Failing. | Failing. | ||
- | [(PP-PLANS | + | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1649925567.484: #< |
| | ||
- | #< | + | #< |
- | #< | + | #< |
+ | Failing. | ||
+ | [(PP-PLANS PICK-UP) WARN] 1649925567.484: | ||
+ | FRAME-ID: " | ||
+ | #< | ||
+ | #< | ||
Ignoring. | Ignoring. | ||
- | ; Evaluation aborted on #< | + | [(PICK-PLACE PICK-UP) INFO] 1649925567.484: |
+ | [(PICK-PLACE PICK-UP) INFO] 1649925567.845: | ||
+ | [(PICK-AND-PLACE GRIP) WARN] 1649925567.875: | ||
+ | Retrying | ||
+ | [(PICK-AND-PLACE GRIP) WARN] 1649925567.888: | ||
+ | ; Evaluation aborted on #< | ||
</ | </ | ||
Line 371: | Line 418: | ||
| | ||
</ | </ | ||
- | We see that the robot defaults the right arm when the object is in the center and will always try to grasp the left side of the bottle, even when the object is slightly favoring the left side under it (Refer the figure below). | + | We see that the robot defaults the right arm when the object is in the center and will always try to grasp the left side of the bottle, even when the object is slightly favoring the left side under it (Refer the figure below). |
+ | |||
+ | Note: '' | ||
+ | |||
+ | '' | ||
+ | '' | ||
+ | |||
+ | '' | ||
{{: | {{: | ||
Line 388: | Line 442: | ||
Let's encapsulate all this in a method called '' | Let's encapsulate all this in a method called '' | ||
<code lisp> | <code lisp> | ||
- | (defun pick-up-object (? | + | (defun pick-up-object (? |
(let ((? | (let ((? | ||
;;Retry by changing the arm | ;;Retry by changing the arm | ||
Line 399: | Line 453: | ||
| | ||
| | ||
- | + | ||
;; Retry by changing the grasp | ;; Retry by changing the grasp | ||
(let* ((? | (let* ((? | ||
- | | + | |
| | ||
(cpl: | (cpl: | ||
Line 429: | Line 483: | ||
| | ||
? | ? | ||
+ | |||
</ | </ | ||
With this, the '' | With this, the '' | ||
Line 440: | Line 495: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 448: | Line 503: | ||
(pp-plans:: | (pp-plans:: | ||
;; Moving the robot near the table. | ;; Moving the robot near the table. | ||
- | (exe: | + | (exe: |
(type going) | (type going) | ||
(target (desig:a location | (target (desig:a location | ||
(pose ? | (pose ? | ||
- | | + | |
(multiple-value-bind (? | (multiple-value-bind (? | ||
(find-object :bottle) | (find-object :bottle) | ||
Line 459: | Line 514: | ||
;; Moving the robot near the counter. | ;; Moving the robot near the counter. | ||
(let ((?nav-goal *base-pose-near-counter*)) | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (exe: | + | (exe: |
(type going) | (type going) | ||
(target (desig:a location | (target (desig:a location | ||
Line 465: | Line 520: | ||
(coe: | (coe: | ||
+ | (exe: | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
;; Setting the object down on the counter | ;; Setting the object down on the counter | ||
(let ((? | (let ((? | ||
Line 479: | Line 537: | ||
BTW-TUT> (init-projection) | BTW-TUT> (init-projection) | ||
BTW-TUT> (move-bottle) | BTW-TUT> (move-bottle) | ||
- | [(PICK-PLACE PICK-UP) INFO] 1550504321.279: Opening gripper | + | [(PERFORM ACTION) INFO] 1649927731.920: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved. |
- | [(PICK-PLACE PICK-UP) INFO] 1550504321.279: Reaching | + | [(PICK-PLACE PICK-UP) INFO] 1649927732.109: Looking |
- | [(GRASP-FAILURE) WARN] Failed to grasp from LEFT-SIDE using RIGHT arm | + | [(PICK-PLACE PICK-UP) INFO] 1649927732.148: Opening gripper and reaching |
- | [(TRYING-NEW-GRASP) INFO] 1550504800.749: Trying to grasp from RIGHT-SIDE using RIGHT arm | + | [(PICK-PLACE PICK-UP) INFO] 1649927732.789: Grasping |
- | [(PICK-PLACE PICK-UP) INFO] 1550504800.789: Opening gripper | + | [(PICK-PLACE PICK-UP) INFO] 1649927733.055: Gripping |
- | [(PICK-PLACE PICK-UP) INFO] 1550504800.789: Reaching | + | [(PICK-PLACE GRIP) INFO] 1649927733.089: Assert |
- | [(GRASP-FAILURE) WARN] Failed to grasp from RIGHT-SIDE using RIGHT arm | + | [(PICK-PLACE PICK-UP) INFO] 1649927733.097: Lifting |
- | [(TRYING-NEW-GRASP) INFO] 1550504801.577: Trying to grasp from BACK using RIGHT arm | + | [(PICK-PLACE |
- | [(PICK-PLACE PICK-UP) INFO] 1550504801.601: Opening gripper | + | [(PERFORM ACTION) INFO] 1649927733.834: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved. |
- | [(PICK-PLACE | + | [(PICK-PLACE |
- | [(PICK-PLACE PICK-UP) INFO] 1550504801.939: Gripping | + | [(PICK-PLACE PLACE) INFO] 1649927734.003: Reaching |
- | [(PICK-PLACE PICK-UP) INFO] 1550504801.973: Assert grasp into knowledge base | + | [(PICK-PLACE PLACE) INFO] 1649927734.430: Putting |
- | [(PICK-PLACE | + | [(PICK-PLACE PLACE) INFO] 1649927734.677: Opening gripper |
- | [(PICK-PLACE PLACE) INFO] 1550504802.356: Reaching | + | [(PICK-PLACE PLACE) INFO] 1649927734.718: Retract grasp in knowledge base |
- | [(PICK-PLACE PLACE) INFO] 1550504802.508: Putting | + | [(PICK-PLACE PLACE) INFO] 1649927734.807: Updating object location in knowledge base |
- | [(PICK-PLACE PLACE) INFO] 1550504802.619: Opening gripper | + | [(PICK-PLACE PLACE) INFO] 1649927734.807: Retracting |
- | [(PICK-PLACE PLACE) INFO] 1550504802.655: Retract grasp in knowledge base | + | [(PICK-PLACE PLACE) INFO] 1649927735.214: |
- | [(PICK-PLACE PLACE) INFO] 1550504802.660: Retracting | + | [(PERFORM ACTION) INFO] 1649927735.398: |
</ | </ | ||
{{: | {{: | ||
Line 504: | Line 562: | ||
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 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, | ||
- | 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. | + | Instead of a hardcoded pose for the base of the robot, you now would now need to obtain a list of poses where the robot will be able to grasp the bottle from. This is exactly where [[tutorials: |
+ | <code lisp> | ||
+ | (a location | ||
+ | | ||
+ | | ||
+ | (type bottle)))) | ||
+ | |||
+ | </ | ||
+ | Referencing this will give you locations where our robot PR2 can successfully reach an object of type bottle. Note that the object designator used here is pretty generic and will apply to all the bottles in the robot' | ||
+ | To make it more specific we can provide the name and the location designator where the bottle is expected to be in. | ||
+ | Eg: | ||
+ | <code lisp> | ||
+ | ;; This is the location designator used to reference locations where the pr2 should be to reach an object | ||
+ | ;; named bottle-1 of type bottle which is situated on the counter-top named sink-area-surface | ||
+ | (a location | ||
+ | | ||
+ | | ||
+ | (type bottle) | ||
+ | (name bottle-1) | ||
+ | | ||
+ | (on (an object | ||
+ | (type counter-top) | ||
+ | (urdf-name sink-area-surface) | ||
+ | (part-of kitchen)))))))) | ||
+ | </ | ||
+ | You can see that the designator is not that intimidating as it looks, but provides a structured way to reference objects and locations. You can use these to write plans without having to worry about the actual coordinates. | ||
+ | |||
+ | 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. | ||
+ | |||
+ | |||
+ | < | ||
+ | THIS IS ONLY IN CRAM v0.8.0 | ||
=== Useful Macros === | === Useful Macros === | ||
Line 599: | Line 689: | ||
</ | </ | ||
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 '' | 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 '' | ||
+ | |||
+ | |||
+ | --> | ||
+ | |||
+ | </ |