Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
tutorials:demo:fetch_and_place_solutions [2019/09/04 19:17] – [Exercise 2] cleaned up gkazhoya | tutorials:demo:fetch_and_place_solutions [2019/09/04 19:54] (current) – [Exercise 1] gkazhoya | ||
---|---|---|---|
Line 16: | Line 16: | ||
(make-pose " | (make-pose " | ||
- | (defun | + | (defun |
- | (let ((?possible-base-poses `(,*base-pose-near-table-towards-island* , | + | (spawn-object bottle-spawn-pose) |
- | (?current-base-pose | + | (with-simulated-robot |
- | (handle-failure (or object-nowhere-to-be-found | + | (let ((?navigation-goal *base-pose-near-table*)) |
- | object-unreachable) | + | (cpl:par |
- | | + | ;; Moving |
- | ((let* ((? | + | |
- | (? | + | |
- | ;; We update | + | |
- | (setf ? | + | |
- | | + | |
- | | + | |
- | + | ||
- | (when (first ? | + | |
- | (print " | + | |
- | (setf ? | + | |
- | (setf ? | + | |
(perform (an action | (perform (an action | ||
(type going) | (type going) | ||
- | | + | |
- | (pose ? | + | (pose ? |
- | (cpl: | + | (perform (a motion |
- | (print " | + | (type moving-torso) |
- | (cpl:fail ' | + | (joint-angle 0.3))) |
+ | (park-arms))) | ||
+ | |||
+ | (let ((? | ||
+ | |||
+ | (let ((? | ||
+ | , | ||
+ | (? | ||
+ | |||
+ | (handle-failure (or object-nowhere-to-be-found | ||
+ | object-unreachable) | ||
+ | |||
+ | ((find-object :bottle)) | ||
+ | |||
+ | (when (first ? | ||
+ | (print " | ||
+ | (setf ? | ||
+ | (setf ? | ||
+ | (perform (an action | ||
+ | (type going) | ||
+ | | ||
+ | (pose ? | ||
+ | | ||
+ | (print " | ||
+ | (cpl:fail ' | ||
+ | |||
+ | (? | ||
+ | ;; We update the value of ? | ||
+ | (setf ? | ||
+ | (park-arm ? | ||
+ | ;; Moving the robot near the counter. | ||
+ | (let ((?nav-goal *base-pose-near-counter*)) | ||
+ | (perform (an action | ||
+ | (type going) | ||
+ | | ||
+ | (pose ? | ||
+ | ;; Setting the object down on the counter | ||
+ | (let ((? | ||
+ | (perform (an action | ||
+ | (type placing) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | (pose ? | ||
+ | (park-arm ? | ||
</ | </ | ||
Line 50: | Line 83: | ||
| | ||
| | ||
+ | | ||
;; Position of the object relative to the robot | ;; Position of the object relative to the robot | ||
- | | + | |
(x (get-x-of-pose obj-in-robot-frame)) | (x (get-x-of-pose obj-in-robot-frame)) | ||
(y (get-y-of-pose obj-in-robot-frame))) | (y (get-y-of-pose obj-in-robot-frame))) | ||
Line 101: | Line 135: | ||
| | ||
(type ? | (type ? | ||
- | + | ||
;; If the action fails, try the following: | ;; If the action fails, try the following: | ||
;; try different look directions until there is none left. | ;; try different look directions until there is none left. | ||
Line 128: | Line 162: | ||
(setf ? | (setf ? | ||
(setf possible-torso-link-positions (rest possible-torso-link-positions)) | (setf possible-torso-link-positions (rest possible-torso-link-positions)) | ||
+ | (setf possible-look-directions `(, | ||
+ | , | ||
+ | , | ||
+ | (setf ? | ||
(perform (a motion | (perform (a motion | ||
(type moving-torso) | (type moving-torso) | ||
Line 136: | Line 174: | ||
- | (defun move-bottle (bottle-spawn-pose) | ||
- | (spawn-object bottle-spawn-pose) | ||
- | (with-simulated-robot | ||
- | (let ((? | ||
- | (cpl:par | ||
- | ;; Moving the robot near the table. | ||
- | (perform (an action | ||
- | (type going) | ||
- | | ||
- | (pose ? | ||
- | (perform (a motion | ||
- | (type moving-torso) | ||
- | (joint-angle 0.3))) | ||
- | (park-arms))) | ||
- | (let* ((? | ||
- | | ||
- | | ||
- | ;; Moving the robot near the counter. | ||
- | (let ((?nav-goal *base-pose-near-counter*)) | ||
- | (perform (an action | ||
- | (type going) | ||
- | | ||
- | (pose ? | ||
- | ;; Setting the object down on the counter | ||
- | (let ((? | ||
- | (perform (an action | ||
- | (type placing) | ||
- | (arm ? | ||
- | | ||
- | | ||
- | (pose ? | ||
- | (park-arm ? | ||
</ | </ |