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/15 16:05] – [Abstract entity descriptions] 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 324: | Line 326: | ||
</ | </ | ||
- | Here, as we didn't specifically say which pose we are trying to perceive the robot randomly samples one pose from all the poses on the kitchen island table and generates a distribution to perceive that pose, i.e., it looks in the general direction of the table. If you look at the debug window now you can see a gaussian | + | Here, as we didn't specifically say which pose we are trying to perceive: the robot randomly samples one pose from all the poses on the kitchen island table and generates a distribution to perceive that pose, i.e., it looks in the general direction of the table. If you look at the debug window now you can see an area that looks like a part of a Gaussian bell near the table. We can use that pose to move the robot there. |
<code lisp> | <code lisp> | ||
Line 333: | Line 335: | ||
==== Grasp objects ==== | ==== Grasp objects ==== | ||
- | We need a clean environment for this, so restart | + | We need a clean environment for this, so let's clean the world: |
<code lisp> | <code lisp> | ||
BTW-TUT> (roslisp-utilities: | BTW-TUT> (roslisp-utilities: | ||
</ | </ | ||
- | Now we need an object to grasp. For that we will use our own mesh of a bottle loaded from resources. | + | From now on we will use the utility functions from '' |
+ | |||
+ | We need an object to grasp. For that we will use a mesh of a bottle loaded from the '' | ||
<code lisp> | <code lisp> | ||
BTW-TUT> (add-objects-to-mesh-list) | BTW-TUT> (add-objects-to-mesh-list) | ||
Line 343: | Line 347: | ||
Then we spawn an object of type bottle and move it onto the table. | Then we spawn an object of type bottle and move it onto the table. | ||
<code lisp> | <code lisp> | ||
- | BTW-TUT> (btr-utils: | + | BTW-TUT> (btr-utils: |
- | BTW-TUT> (btr-utils: | + | BTW-TUT> (btr-utils: |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | |
+ | | ||
</ | </ | ||
- | Lastly we simulate the world for 100 seconds to make sure, nothing moves unexpectedly on runtime. | + | Lastly we simulate the world for 10 seconds to make sure, nothing moves unexpectedly on runtime. |
<code lisp> | <code lisp> | ||
BTW-TUT> (btr: | BTW-TUT> (btr: | ||
</ | </ | ||
- | From now on it's okay to move the robot. | + | Before we grasp the bottle, let's first prepare the PR2, just three steps: move the arms out of sight, navigate the base in front of the bottle and look at the bottle. The point we want the base to navigate to can be hard coded and saved temporarily. |
<code lisp> | <code lisp> | ||
BTW-TUT> (setf ? | BTW-TUT> (setf ? | ||
- | (cl-transforms-stamped: | + | (cl-transforms-stamped: |
- | | + | " |
- | | + | 0.0 |
- | | + | (cl-transforms: |
- | | + | (cl-transforms: |
+ | | ||
</ | </ | ||
- | The same thing can be done with the point, we want to look at. | + | The same thing can be done with the point we want to look at. |
<code lisp> | <code lisp> | ||
BTW-TUT> (setf ? | BTW-TUT> (setf ? | ||
- | (cl-transforms-stamped: | + | (cl-transforms-stamped: |
- | | + | " |
- | | + | 0.0 |
- | | + | (cl-transforms: |
- | | + | (cl-transforms: |
</ | </ | ||
- | As usual we need a top-level context | + | To execute any plan in CRAM, we need a top-level context. Besides that we also use a macro to specify |
<code lisp> | <code lisp> | ||
BTW-TUT> | BTW-TUT> | ||
Line 376: | Line 382: | ||
(cpl: | (cpl: | ||
(cpl:par | (cpl:par | ||
- | (pr2-pp-plans:: | + | (pr2-pp-plans:: |
(exe: | (exe: | ||
(type going) | (type going) | ||
Line 384: | Line 390: | ||
(target (desig:a location (pose ? | (target (desig:a location (pose ? | ||
</ | </ | ||
- | We actually | + | We can execute the arm movement, navigation and head tilt in parallel, since each of them uses different joints of the robot. That's what '' |
+ | |||
+ | To grasp the bottle we need to have its pose in the room. Therefore, we first perceive it and store the result in '' | ||
+ | <code lisp> | ||
+ | BTW-TUT> | ||
+ | (defvar *perceived-object* nil " | ||
+ | (proj: | ||
+ | | ||
+ | (setf *perceived-object* | ||
+ | | ||
+ | </ | ||
+ | |||
+ | With that resulting perceived object we perform the picking up action. With the torso so far down we might not be able to reach for the bottle, so we need to also push the torso up: | ||
- | With the torso so far down we won't be able to reach for the bottle, so we need to push him up. To grasp the bottle we also need its pose in the room. Therefore we first get the estimated position of the bottle, then perceive it for a more accurate pose. With that pose we start the plan '' | ||
<code lisp> | <code lisp> | ||
- | BTW-TUT> | ||
(proj: | (proj: | ||
(cpl: | (cpl: | ||
- | (let* ((? | + | (let ((? |
- | (type bottle))) | + | |
- | (? | + | |
(exe: | (exe: | ||
| | ||
- | (pr2-pp-plans::drive-and-pick-up-plan ? | + | (exe:perform |
+ | | ||
+ | (type picking-up) | ||
+ | | ||
+ | (arm right)))))) | ||
</ | </ | ||
- | | ||
Now the PR2 should have the bottle in his right gripper. | Now the PR2 should have the bottle in his right gripper. | ||
{{: | {{: | ||
+ | < | ||
Let's take a quick look into the '' | Let's take a quick look into the '' | ||
<code lisp> | <code lisp> | ||
Line 418: | Line 436: | ||
| | ||
</ | </ | ||
- | Here he first calls a function to get the base in a position optimized for grasping with a specific arm. After that he again looks at the object and finally picks it up. To see which action designator is used for picking up the object we need a good search. Use ''" | + | Here he first calls a function to get the base in a position optimized for grasping with a specific arm. After that he again looks at the object and finally picks it up. |
+ | --></ | ||
+ | |||
+ | To see which action designator is used for picking up the object we can use a grep search. Use ''" | ||
==== Place objects ==== | ==== Place objects ==== | ||
- | To place things held in the gripper we first need an object attached to a gripper. Use the previous chapter '' | + | To place things held in the gripper we first need an object attached to a gripper. Use the previous chapter '' |
<code lisp> | <code lisp> | ||
BTW-TUT> | BTW-TUT> | ||
(proj: | (proj: | ||
- | (cpl: | + | (let ((? |
- | | + | |
- | (type placing) | + | (exe: |
- | (arm right))))) | + | |
+ | (object ? | ||
+ | (arm right)))))) | ||
</ | </ | ||
Line 436: | Line 459: | ||
BTW-TUT> | BTW-TUT> | ||
(proj: | (proj: | ||
- | (cpl: | + | |
- | | + | (let ((?perceived-bottle-desig |
- | (type bottle))) | + | |
- | (? | + | (desig:a motion |
- | | + | (exe:perform |
+ | | ||
+ | (type picking-up) | ||
+ | | ||
+ | (arm right)))))) | ||
</ | </ | ||
- | Now move the robot to the other table. We use a motion designator for that, who needs a specific pose. | + | Now move the robot to the other table. We use a motion designator for that, which needs a specific pose. |
<code lisp> | <code lisp> | ||
BTW-TUT> (proj: | BTW-TUT> (proj: | ||
- | (cpl: | + | (cpl: |
- | | + | |
- | (let ((?pose (cl-tf: | + | (let ((?pose (cl-tf: |
- | cram-tf: | + | cram-tf: |
- | (cl-transforms: | + | (cl-transforms: |
- | (cl-transforms: | + | (cl-transforms: |
- | (desig:a motion (type going) (target (desig:a location (pose ? | + | (desig:a motion (type going) (target (desig:a location (pose ? |
</ | </ | ||
- | This seems like a good spot to place the bottle. Let's put it down to coordinates -0.7 2.0 0.861667. | + | This seems like a good spot to place the bottle. Let's put it down to coordinates |
<code lisp> | <code lisp> | ||
- | BTW-TUT> (proj: | + | BTW-TUT> |
- | | + | (proj: |
- | | + | (cpl: |
- | " | + | (let ((? |
- | (cl-transforms: | + | |
- | (cl-transforms: | + | |
- | | + | |
- | (type placing) | + | |
- | (arm right) | + | (exe: |
- | (target (desig:a location (pose ? | + | |
+ | | ||
+ | (object ? | ||
+ | (target (desig:a location (pose ? | ||
</ | </ | ||
With these actions you can pick up objects, move the robot and place them at specific points. The scene should now look like this: | With these actions you can pick up objects, move the robot and place them at specific points. The scene should now look like this: | ||
Line 476: | 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 482: | 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 513: | Line 544: | ||
(cl-transforms-stamped: | (cl-transforms-stamped: | ||
" | " | ||
- | | + | |
| | ||
</ | </ | ||
Line 521: | 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 546: | 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 581: | 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 '' |