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:advanced:bullet_world [2017/09/18 16:28] – [Place objects] gkazhoya | tutorials:advanced:bullet_world [2017/09/22 09:40] – [Abstract entity descriptions] gkazhoya | ||
---|---|---|---|
Line 1: | Line 1: | ||
- | //Tested with Cram branch: Cram4, ROS version: | + | **//Tested with Cram v0.4.0, ROS version: |
====== Bullet world demonstration ====== | ====== Bullet world demonstration ====== | ||
Line 70: | Line 70: | ||
====REPL setup==== | ====REPL setup==== | ||
- | Now, let's load the package in the REPL: | + | Now, let's load the package in the REPL (start the REPL with '' |
<code lisp> | <code lisp> | ||
CL-USER> (ros-load: | CL-USER> (ros-load: | ||
Line 214: | Line 214: | ||
(btr: | (btr: | ||
' | ' | ||
- | | + | |
</ | </ | ||
Line 285: | Line 285: | ||
BTW-TUT> (prolog: | BTW-TUT> (prolog: | ||
(assert (btr: | (assert (btr: | ||
- | ((0.5 1 0) (0 0 1 )))))) | + | ((0.5 1 0) (0 0 1 0)))))) |
</ | </ | ||
and ask the visibility query again: instead of typing, put the cursor where you would type and press '' | and ask the visibility query again: instead of typing, put the cursor where you would type and press '' | ||
Line 296: | Line 296: | ||
(? | (? | ||
(? | (? | ||
- | (to-see-designator (desig:a location (to see) | + | (to-see-designator (desig:a location (type visible) |
+ | (for pr2) | ||
| | ||
| | ||
Line 315: | Line 316: | ||
| | ||
(at ? | (at ? | ||
- | | + | |
+ | (for pr2) | ||
(object ? | (object ? | ||
(desig: | (desig: | ||
Line 504: | Line 506: | ||
BTW-TUT> (init-projection) | BTW-TUT> (init-projection) | ||
</ | </ | ||
- | If you just started your EMACS or loaded the package don't forget to add our extra meshes from resource, otherwise you won't be able to spawn a bottle. | + | If you just started your Emacs or loaded the package don't forget to add our extra meshes from resource, otherwise you won't be able to spawn a bottle. |
<code lisp> | <code lisp> | ||
BTW-TUT> (add-objects-to-mesh-list) | BTW-TUT> (add-objects-to-mesh-list) | ||
Line 510: | Line 512: | ||
Now we need those two objects in the world. | Now we need those two objects in the world. | ||
<code lisp> | <code lisp> | ||
- | BTW-TUT> (prolog: | + | BTW-TUT> |
- | (assert (btr:object ?world :mesh bottle-1 ((-2 -0.9 0.861667d0) (0 0 0 1)) | + | (prolog: |
- | | + | |
- | (assert (btr:object ?world :mesh bottle-2 ((-0.65 2 0.955) (0 0 0 1)) | + | |
- | | + | |
- | (btr: | + | |
+ | | ||
</ | </ | ||
- | To make the plan shorter we can define some parameters a prior. The *pose-meal-table* is the pose, where the pr2 should stand, when he approaches the green bottle. The other pose, *pose-meal-counter* is the base pose for the counter table, when we want to place the bottle. | + | To make the plan shorter we can define some parameters a priori. The *pose-meal-table* is the pose, where the pr2 should stand, when he approaches the green bottle. The other pose, *pose-meal-counter* is the base pose for the counter table, when we want to place the bottle. |
<code lisp> | <code lisp> | ||
- | BTW-TUT> | + | BTW-TUT> |
(defparameter *pose-bottle-1* | (defparameter *pose-bottle-1* | ||
(cl-transforms-stamped: | (cl-transforms-stamped: | ||
" | " | ||
- | | + | |
| | ||
+ | |||
(defparameter *pose-bottle-2* | (defparameter *pose-bottle-2* | ||
(cl-transforms-stamped: | (cl-transforms-stamped: | ||
" | " | ||
- | | + | |
| | ||
- | + | ||
(defparameter *pose-meal-table* | (defparameter *pose-meal-table* | ||
(cl-tf: | (cl-tf: | ||
" | " | ||
- | | + | |
| | ||
Line 541: | Line 544: | ||
(cl-transforms-stamped: | (cl-transforms-stamped: | ||
" | " | ||
- | | + | |
| | ||
</ | </ | ||
Line 549: | Line 552: | ||
(unless (assoc :bottle btr:: | (unless (assoc :bottle btr:: | ||
(add-objects-to-mesh-list)) | (add-objects-to-mesh-list)) | ||
- | (prolog: | + | (prolog: |
- | (assert (btr:object ?world :mesh bottle-1 ((-2 -0.9 0.861667d0) (0 0 0 1)) | + | `(and (btr: |
- | | + | |
- | (assert (btr:object ?world :mesh bottle-2 ((-0.65 2 0.955) (0 0 0 1)) | + | |
- | | + | |
- | (btr: | + | |
- | + | | |
- | (defun move-arm-out-of-sight (&key (arm ' | + | |
- | (unless (listp arm) | + | |
- | (setf arm (list arm))) | + | |
- | (exe: | + | |
- | (let ((? | + | |
- | | + | |
- | | + | |
- | (type moving-joints) | + | |
- | (left-configuration ? | + | |
- | (right-configuration ? | + | |
(defun navigate-to (? | (defun navigate-to (? | ||
Line 574: | Line 567: | ||
(defun look-at (? | (defun look-at (? | ||
(exe: | (exe: | ||
- | | + | |
- | (target (desig:a location (pose ? | + | (target (desig:a location (pose ? |
(defun get-perceived-bottle-desig () | (defun get-perceived-bottle-desig () | ||
- | (let* ((? | + | (let* ((? |
- | | + | |
- | | + | |
- | | + | (type detecting) |
- | (type detecting) | + | (object ? |
- | (object ? | + | |
? | ? | ||
(defun pick-up (? | (defun pick-up (? | ||
(exe: | (exe: | ||
- | (type picking-up) | + | (type picking-up) |
- | | + | |
- | | + | |
- | (defun place-down (?pose ?arm) | + | (defun place-down (? |
(exe: | (exe: | ||
- | (type placing) | + | (type placing) |
- | | + | |
- | | + | (object ?object) |
- | + | (target (desig:a location (pose ?pose)))))) | |
- | (defun move-to-reach (? | + | |
- | (pr2-pp-plans:: | + | |
</ | </ | ||
Here we go. Now we can use those functions to write down the top-level plan. | Here we go. Now we can use those functions to write down the top-level plan. | ||
Line 609: | Line 599: | ||
(cpl: | (cpl: | ||
;; Go to counter top and perceive bottle | ;; Go to counter top and perceive bottle | ||
- | (let ((? | + | (let ((? |
- | (? | + | (? |
+ | (cl-transforms-stamped:make-pose-stamped | ||
+ | " | ||
+ | 0.0 | ||
+ | | ||
+ | (cl-transforms: | ||
(cpl:par | (cpl:par | ||
;; Move torso up | ;; Move torso up | ||
(exe: | (exe: | ||
| | ||
- | (move-arm-out-of-sight) | + | (pr2-pp-plans::park-arms) |
(navigate-to ? | (navigate-to ? | ||
(look-at ?ptu-goal)) | (look-at ?ptu-goal)) | ||
;; Pick up bottle-1 with right arm. | ;; Pick up bottle-1 with right arm. | ||
- | (let* ((? | + | (let ((? |
- | (move-to-reach | + | (pick-up ? |
- | (pick-up ?perceived-bottle-desig | + | (pr2-pp-plans::park-arms :arm :right) |
- | ;; (move-arm-out-of-sight | + | ;; Move to the meal table |
- | ;; Move to the meal table | + | (let ((?pose *pose-meal-table*)) |
- | (let ((?pose *pose-meal-table*)) | + | (navigate-to ?pose)) |
- | (navigate-to ?pose)) | + | ;; Pick up bottle-2 with left arm |
- | ;; Pick up bottle-2 with left arm | + | (let ((? |
- | (let* ((? | + | (pick-up ? |
- | | + | ;; Move left arm out of sight |
- | | + | (pr2-pp-plans::park-arms :arm :left) |
- | ;; Move left arm out of sight | + | ;; Place bottle-1 on second table |
- | (move-arm-out-of-sight | + | (let ((? |
- | ;; Place bottle-1 on second table | + | (place-down ? |
- | (let ((? | + | ;; Move right arm out of sight |
- | (place-down ?drop-pose :right)) | + | (pr2-pp-plans::park-arms :arm :right) |
- | ;; Move right arm out of sight | + | ;; Move to the counter table |
- | (move-arm-out-of-sight | + | (let ((? |
- | ;; Move to the counter table | + | (navigate-to ? |
- | (let ((? | + | ;; Place bottle-2 on the counter |
- | | + | (let ((? |
- | ;; Place bottle-2 on the counter | + | (place-down ? |
- | (let ((? | + | (pr2-pp-plans::park-arms)))))) |
- | (place-down ?drop-pose :left)) | + | |
- | (move-arm-out-of-sight)))) | + | |
</ | </ | ||
Each time you run '' | Each time you run '' |