Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
tutorials:intermediate:simple_mobile_manipulation_plan [2019/07/08 18:00] – [Installing the projection project] gkazhoya | tutorials:intermediate:simple_mobile_manipulation_plan [2022/04/25 09:20] (current) – schimpf | ||
---|---|---|---|
Line 1: | Line 1: | ||
- | **//Tested with Cram v0.7.0, ROS version: | + | **//Tested with Cram v0.8.0, ROS version: |
====== Simple Mobile Manipulation Plan ====== | ====== Simple Mobile Manipulation Plan ====== | ||
Line 34: | 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 41: | 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 63: | 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 72: | 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 110: | Line 135: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 118: | 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 124: | 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 143: | 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 159: | Line 184: | ||
</ | </ | ||
- | Note that the plan is nested under '' | + | Note that the plan is nested under '' |
Now run '' | Now run '' | ||
Line 173: | Line 198: | ||
(unless (assoc :bottle btr:: | (unless (assoc :bottle btr:: | ||
(add-objects-to-mesh-list)) | (add-objects-to-mesh-list)) | ||
- | (btr-utils: | + | (btr-utils: |
(btr: | (btr: | ||
</ | </ | ||
Line 221: | 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 228: | 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 238: | 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 261: | 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 271: | 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 286: | 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 301: | Line 326: | ||
(pose ? | (pose ? | ||
(pp-plans:: | (pp-plans:: | ||
+ | |||
</ | </ | ||
- | Clean up and run '' | + | Run '' |
{{: | {{: | ||
Line 309: | Line 335: | ||
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 |
+ | [(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 363: | Line 417: | ||
| | ||
</ | </ | ||
- | 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 380: | Line 441: | ||
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 391: | Line 452: | ||
| | ||
| | ||
- | + | ||
;; Retry by changing the grasp | ;; Retry by changing the grasp | ||
(let* ((? | (let* ((? | ||
- | | + | |
| | ||
(cpl: | (cpl: | ||
Line 421: | Line 482: | ||
| | ||
? | ? | ||
+ | |||
</ | </ | ||
With this, the '' | With this, the '' | ||
Line 432: | Line 494: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 440: | Line 502: | ||
(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 451: | Line 513: | ||
;; 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 457: | Line 519: | ||
(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 471: | Line 536: | ||
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 496: | Line 561: | ||
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 591: | Line 688: | ||
</ | </ | ||
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 '' | ||
+ | |||
+ | |||
+ | --> | ||
+ | |||
+ | </ |