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:55] – [Constructing plans] gkazhoya | tutorials:intermediate:simple_mobile_manipulation_plan [2022/04/14 09:22] – [Expanding Failure Management Capabilities] schimpf | ||
---|---|---|---|
Line 7: | Line 7: | ||
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 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, | 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: | ||
Line 39: | Line 42: | ||
* <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. 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: | ||
Line 61: | Line 88: | ||
(unless (assoc :bottle btr:: | (unless (assoc :bottle btr:: | ||
(add-objects-to-mesh-list)) | (add-objects-to-mesh-list)) | ||
- | (btr-utils: | + | (btr-utils: |
(btr: | (btr: | ||
BTW-TUT> (spawn-bottle) | BTW-TUT> (spawn-bottle) | ||
Line 70: | Line 97: | ||
<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 108: | Line 135: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 116: | 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 122: | 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 141: | 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 157: | Line 184: | ||
</ | </ | ||
- | Note that the plan is nested under '' | + | Note that the plan is nested under '' |
Now run '' | Now run '' | ||
- | ===== Increasing the Effectiveness by Improving the plan ===== | + | ===== Recovering from Failures |
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, | 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, | ||
Line 171: | 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 189: | 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 224: | 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 231: | 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 241: | 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 264: | 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 274: | 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 289: | 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 304: | 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: #< | ||
| | ||
- | #< | + | #< |
- | #< | + | #< |
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 366: | 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 383: | 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 394: | Line 453: | ||
| | ||
| | ||
- | + | ||
;; Retry by changing the grasp | ;; Retry by changing the grasp | ||
(let* ((? | (let* ((? | ||
- | | + | |
| | ||
(cpl: | (cpl: | ||
Line 424: | Line 483: | ||
| | ||
? | ? | ||
+ | |||
</ | </ | ||
With this, the '' | With this, the '' | ||
Line 435: | Line 495: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 443: | 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 454: | 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 460: | 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 474: | 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 499: | 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 594: | 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 '' | ||
+ | |||
+ | |||
+ | --> | ||
+ | |||
+ | </ |