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/08/02 14:05] – [Expanding Failure Management Capabilities] Added information about grasps amar | 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 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 ? | ||
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:: | ||
+ | |||
</ | </ | ||
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: | + | (btr-utils: |
(btr: | (btr: | ||
</ | </ | ||
- | 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 384: | 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 395: | Line 452: | ||
| | ||
| | ||
- | + | ||
;; Retry by changing the grasp | ;; Retry by changing the grasp | ||
(let* ((? | (let* ((? | ||
- | | + | |
| | ||
(cpl: | (cpl: | ||
Line 425: | Line 482: | ||
| | ||
? | ? | ||
+ | |||
</ | </ | ||
With this, the '' | With this, the '' | ||
Line 436: | Line 494: | ||
(defun move-bottle () | (defun move-bottle () | ||
(spawn-bottle) | (spawn-bottle) | ||
- | (pr2-proj: | + | (urdf-proj: |
(let ((? | (let ((? | ||
(cpl:par | (cpl:par | ||
Line 444: | 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 455: | 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 461: | 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 475: | 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: |
</ | </ | ||
{{: | {{: |