Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revisionLast revisionBoth sides next revision | ||
tutorials:intermediate:simple_mobile_manipulation_plan [2019/07/08 18:00] – [Constructing plans] gkazhoya | tutorials:intermediate:simple_mobile_manipulation_plan [2022/04/14 09:22] – [Expanding Failure Management Capabilities] schimpf | ||
---|---|---|---|
Line 42: | 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 ? | ||
Line 64: | 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 73: | 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 111: | Line 135: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 119: | 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 125: | 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 144: | 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 160: | Line 184: | ||
</ | </ | ||
- | Note that the plan is nested under '' | + | Note that the plan is nested under '' |
Now run '' | Now run '' | ||
Line 174: | 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 222: | 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 229: | 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 239: | 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 262: | 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 272: | 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 287: | 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 302: | Line 326: | ||
(pose ? | (pose ? | ||
(pp-plans:: | (pp-plans:: | ||
+ | |||
</ | </ | ||
- | Clean up and run '' | + | Run '' |
{{: | {{: | ||
Line 310: | 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: #< | ||
| | ||
- | #< | + | #< |
- | #< | + | #< |
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 364: | 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 381: | 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 392: | Line 452: | ||
| | ||
| | ||
- | + | ||
;; Retry by changing the grasp | ;; Retry by changing the grasp | ||
(let* ((? | (let* ((? | ||
- | | + | |
| | ||
(cpl: | (cpl: | ||
Line 422: | Line 482: | ||
| | ||
? | ? | ||
+ | |||
</ | </ | ||
With this, the '' | With this, the '' | ||
Line 433: | Line 494: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 441: | 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 452: | 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 458: | 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 472: | 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 497: | 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 592: | 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 '' | ||
+ | |||
+ | |||
+ | --> | ||
+ | |||
+ | </ |