Differences
This shows you the differences between two versions of the page.
Next revision | Previous revision | ||
tutorials:demo:fetch_and_place_solutions [2019/09/04 18:53] – created gkazhoya | tutorials:demo:fetch_and_place_solutions [2019/09/04 19:54] (current) – [Exercise 1] gkazhoya | ||
---|---|---|---|
Line 11: | Line 11: | ||
(defparameter *base-pose-near-table-towards-island* | (defparameter *base-pose-near-table-towards-island* | ||
- | (make-pose " | + | (make-pose " |
- | (setf *base-pose-near-sink-surface* | + | (defparameter |
(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 |
- | | + | (perform (an action |
- | (? | + | |
- | ;; We update | + | |
- | (setf ? | + | |
- | | + | |
- | | + | |
- | + | ||
- | (when (first ? | + | |
- | | + | |
- | (setf ? | + | |
- | (setf ? | + | |
- | (handle-failure navigation-pose-unreachable | + | |
- | ((perform (an action | + | |
(type going) | (type going) | ||
- | | + | |
- | (pose ?current-base-pose)))))) | + | (pose ?navigation-goal))))) |
- | (when (first ? | + | (perform (a motion |
- | (setf ? | + | (type moving-torso) |
- | (setf ? | + | (joint-angle 0.3))) |
- | (cpl:retry)) | + | (park-arms))) |
- | (print " | + | |
- | (cpl:fail ' | + | (let ((? |
- | (cpl: | + | |
- | (print " | + | (let ((?possible-base-poses `(,*base-pose-near-table-towards-island* |
- | (cpl:fail ' | + | , |
+ | (? | ||
+ | |||
+ | (handle-failure (or object-nowhere-to-be-found | ||
+ | object-unreachable) | ||
+ | |||
+ | ((find-object :bottle)) | ||
+ | |||
+ | | ||
+ | | ||
+ | | ||
+ | (setf ? | ||
+ | (perform (an action | ||
+ | (type going) | ||
+ | | ||
+ | (pose ? | ||
+ | (cpl: | ||
+ | (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 54: | Line 80: | ||
;; Solution for choosing the arm based on the position of the robot | ;; Solution for choosing the arm based on the position of the robot | ||
(defun get-preferred-arm (? | (defun get-preferred-arm (? | ||
- | (let* ((?obj-name (get-obj-name ? | + | (let* ((obj-name (get-obj-name ? |
- | (?obj-pose (get-current-pose-of-object | + | |
- | (?robot-transformation (get-robot-transformation-matrix)) | + | |
+ | | ||
;; Position of the object relative to the robot | ;; Position of the object relative to the robot | ||
- | (?obj-in-robot-frame (cl-transforms-stamped: | + | |
- | ?robot-transformation ?obj-pose)) | + | (x (get-x-of-pose |
- | (?x (cl-tf:x (cl-tf:origin ?obj-in-robot-frame))) | + | (y (get-y-of-pose |
- | (?y (cl-tf:y (cl-tf:origin ?obj-in-robot-frame)))) | + | |
;; If the object is in front of the robot | ;; If the object is in front of the robot | ||
- | (if (> ?x 0) | + | (if (> x 0) |
;; if the object is on positive y-axis in front of the robot | ;; if the object is on positive y-axis in front of the robot | ||
;; then left, else right | ;; then left, else right | ||
- | (if (> ?y 0) | + | (if (> y 0) |
:left | :left | ||
:right) | :right) | ||
;; If the object is on positive y-axis while behind the robot | ;; If the object is on positive y-axis while behind the robot | ||
;; then right, else left | ;; then right, else left | ||
- | (if (> ?y 0) | + | (if (> y 0) |
:right | :right | ||
:left)))) | :left)))) | ||
Line 109: | 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 136: | 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 144: | 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 ? | ||
</ | </ |