Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revisionNext revisionBoth sides next revision | ||
tutorials:intermediate:bullet_world [2019/07/04 12:52] – [Manipulating the kitchen] gkazhoya | tutorials:intermediate:bullet_world [2022/04/25 09:19] – schimpf | ||
---|---|---|---|
Line 1: | Line 1: | ||
- | **//Tested with Cram v0.7.0, ROS version: | + | **//Tested with Cram v0.8.0, ROS version: |
====== Bullet world demonstration ====== | ====== Bullet world demonstration ====== | ||
Line 153: | Line 153: | ||
(cram-robot-interfaces: | (cram-robot-interfaces: | ||
(assert (btr:object ?world :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf , | (assert (btr:object ?world :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf , | ||
- | (cram-robot-interfaces:robot-arms-parking-joint-states ?robot ? | + | (-> (rob-int: |
- | (assert (btr: | + | (assert (btr: |
- | (assert (btr: | + | (true)) |
+ | | ||
+ | | ||
+ | (true))))) | ||
</ | </ | ||
Line 183: | Line 186: | ||
</ | </ | ||
In the visualization window you can now see the kitchen loaded from the URDF with the semantic map information attached to it. | In the visualization window you can now see the kitchen loaded from the URDF with the semantic map information attached to it. | ||
- | |||
- | (If you have KnowRob install, you can visualize the semantic map not as a URDF but as a semantic map object: '' | ||
You can see the environment in rviz as well. Open up a terminal and type ''" | You can see the environment in rviz as well. Open up a terminal and type ''" | ||
Line 196: | Line 197: | ||
To see the URDF of the kitchen in RViz you need its description name ' | To see the URDF of the kitchen in RViz you need its description name ' | ||
- | When you are ready with the robot model, look at the ' | + | When you are ready with the robot model of the kitchen, look at the ' |
As you can see, this environment visualizes all the poses of TF frames of our environment that we can use in our tutorial. Get back to the debug window (Bullet visualization window) to see the actual meshes of the objects. The debug window only shows the collision meshes, not the detailed visuals, since we want to keep the bullet world fast and lightweight and concentrate on collisions not visuals. | As you can see, this environment visualizes all the poses of TF frames of our environment that we can use in our tutorial. Get back to the debug window (Bullet visualization window) to see the actual meshes of the objects. The debug window only shows the collision meshes, not the detailed visuals, since we want to keep the bullet world fast and lightweight and concentrate on collisions not visuals. | ||
+ | |||
+ | Please note that you will not be able to see the robot TF frames with our ' | ||
The environment is now initialized. For convenience, | The environment is now initialized. For convenience, | ||
Line 221: | Line 224: | ||
Let us inspect the kitchen now: | Let us inspect the kitchen now: | ||
- | |||
- | (If you use KnowRob, this part of the tutorial will only work if you have spawned your kitchen from URDF, not the semantic map.) | ||
<code lisp> | <code lisp> | ||
- | BTW-TUT> (btr:object btr: | + | BTW-TUT> (btr:object btr: |
</ | </ | ||
Line 234: | Line 235: | ||
(btr: | (btr: | ||
' | ' | ||
- | | + | |
</ | </ | ||
Line 288: | Line 289: | ||
==== Geometric reasoning ==== | ==== Geometric reasoning ==== | ||
- | Our mug is floating in the air. The belief state world in CRAM is static, in order to apply the physics to it we use the Bullet physics engine to simulate the world for a certain amount of time. | + | Our mug is floating in the air. The world state representation |
<code lisp> | <code lisp> | ||
BTW-TUT> (prolog: | BTW-TUT> (prolog: | ||
Line 295: | Line 296: | ||
This simulates the world for 1 second and the mug falls onto the table. | This simulates the world for 1 second and the mug falls onto the table. | ||
- | We can also ask if the scene is stable | + | We can also ask if the scene is stable: |
<code lisp> | <code lisp> | ||
BTW-TUT> (prolog: | BTW-TUT> (prolog: | ||
Line 307: | Line 308: | ||
<code lisp> | <code lisp> | ||
BTW-TUT> (prolog: | BTW-TUT> (prolog: | ||
- | (cram-robot-interfaces: | + | (assert |
- | | + | |
NIL | NIL | ||
</ | </ | ||
Line 315: | Line 316: | ||
<code lisp> | <code lisp> | ||
BTW-TUT> (prolog: | BTW-TUT> (prolog: | ||
- | (assert (btr: | + | (assert (btr: |
((0.5 1 0) (0 0 1 0)))))) | ((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 331: | Line 332: | ||
BTW-TUT> | BTW-TUT> | ||
(def-fact-group costmap-metadata () | (def-fact-group costmap-metadata () | ||
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
- | + | ||
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
- | (<- (location-costmap: | + | (<- (: |
</ | </ | ||
Now, we create an abstract location description that we call a // | Now, we create an abstract location description that we call a // | ||
Line 359: | Line 360: | ||
We create two symbolic descriptions: | We create two symbolic descriptions: | ||
- | We can also completely abstract away from geometric poses (we used the '' | + | We can also completely abstract away from geometric poses (we used the '' |
<code lisp> | <code lisp> | ||
Line 366: | Line 367: | ||
(on (desig:an object | (on (desig:an object | ||
(type counter-top) | (type counter-top) | ||
- | | + | |
- | | + | |
- | (location ? | + | |
+ | (visible-for pr2) | ||
+ | | ||
(desig: | (desig: | ||
#< | #< | ||
Line 376: | Line 379: | ||
</ | </ | ||
- | 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 | + | 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 closely |
+ | |||
+ | * first an area, which corresponds to the location on the kitchen-island-surface, | ||
+ | * then a sample is being drawn from that distribution, | ||
+ | * then a Gaussian | ||
+ | * and, finally, a sample is taken from the Gaussian visualized again as a red dot. | ||
+ | |||
+ | You can execute the above command multiple times, and each time you will get a different distribution of locations for the robot to stand. Sometimes, you might get an error that the location cannot be referenced. This happens if the sample that the robot | ||
+ | picks on the table is too close to the wall, and there is no location to stand to perceive that point on the table that would not create collisions of the robot with the environment. | ||
+ | |||
+ | Let us move the robot to a location to perceive a mug on the table: | ||
<code lisp> | <code lisp> | ||
Line 392: | Line 405: | ||
==== Attaching objects to the robot ==== | ==== Attaching objects to the robot ==== | ||
- | When manipulating objects in the environment e.g. carrying a cup in the gripper, the simulation needs to be told that such objects should move, when the respective robot part moves. If the PR2 robot grasps an object, it automatically | + | When manipulating objects in the environment, e.g., carrying a cup in the gripper, the simulation needs to be told that such objects should move when the respective robot part moves. If the PR2 robot grasps an object, it is automatically |
+ | In this part of the tutorial, we will place a fork inside a drawer and attach it to the link of the drawer. | ||
Assuming that the cram_bullet_world_tutorial is already loaded, we need to reset the world: | Assuming that the cram_bullet_world_tutorial is already loaded, we need to reset the world: | ||
<code lisp> | <code lisp> | ||
(roslisp-utilities: | (roslisp-utilities: | ||
- | (add-objects-to-mesh-list) ;; load fork mesh | + | (add-objects-to-mesh-list) ;; tell Bullet where the fork mesh is stored on the hard drive |
</ | </ | ||
- | The function '' | + | The function '' |
- | Since a closed drawer would disguise the fork, lets open it: | + | Let us first open the drawer: |
<code lisp> | <code lisp> | ||
(btr: | (btr: | ||
- | ' | + | ' |
- | | + | |
</ | </ | ||
+ | The drawer is called ''" | ||
- | Spawn the fork inside the drawer: | + | {{ : |
+ | |||
+ | Now let us spawn the fork inside the drawer: | ||
<code lisp> | <code lisp> | ||
- | (btr-utils:spawn-object | + | (prolog: |
- | ' | + | (assert (btr:object ? |
- | : | + | :mass 0.2 :color (0.5 0.5 0.5) :mesh : |
- | :pose '((1.0 0.9 0.75) (0 0 0 1))) | + | |
</ | </ | ||
If this throws an error, you probably forgot to use '' | If this throws an error, you probably forgot to use '' | ||
- | {{: | + | {{ : |
- | Try and move the drawer a bit with '' | + | Try and move the drawer a bit with '' |
<code lisp> | <code lisp> | ||
(prolog '(and (btr: | (prolog '(and (btr: | ||
(btr: | (btr: | ||
- | (assert (btr: | + | (assert (btr: |
</ | </ | ||
- | Notice, that the joint name differs from the link name. Now the fork moves when the drawer is moved. Every attachment can be checked with the following predicate: | + | Notice, that the joint name differs from the link name. Now the fork moves when the drawer is moved. |
+ | <code lisp> | ||
+ | (btr: | ||
+ | ' | ||
+ | | ||
+ | </ | ||
+ | Every attachment can be checked with the following predicate: | ||
<code lisp> | <code lisp> | ||
(prolog '(and (btr: | (prolog '(and (btr: | ||
- | (btr: | + | (btr: |
</ | </ | ||
- | This checks if there is any attachments between kitchen and fork. If needed, it is possible to set the name of a link to be specifically checked. Or set the blank to ''? | + | This checks if there is any attachments between kitchen and fork. If needed, it is possible to set the name of a link to be specifically checked. Or set the ''? |
<code lisp> | <code lisp> | ||
(prolog '(and (btr: | (prolog '(and (btr: | ||
- | (assert | + | (btr:%object ?world fork-1 ? |
+ | | ||
</ | </ | ||
- | In the fourth argument | + | This detaches |
+ | If you only want to detach from a specific | ||
Line 443: | Line 467: | ||
+ | |||
+ | |||
+ | < | ||
==== Visualizing coordinate frames of poses ==== | ==== Visualizing coordinate frames of poses ==== | ||
Line 456: | Line 483: | ||
</ | </ | ||
- | {{: | + | {{ : |
- | + | ||
+ | --></ | ||
Line 499: | Line 526: | ||
- | ==== Executing motions | + | ==== Using TF in the Bullet world ==== |
- | From now on we will use the utility | + | Per default, the TF listener is not set up in the REPL, when you are working with the Bullet world. |
- | The package '' | + | To have it running, we need a TF context. It is possible to create it manually, but to not overcomplicate this tutorial, |
- | By pressing '' | + | we will use the environment provided by the '' |
+ | |||
+ | For example, the following does not work (unless you have a real robot running in your ROS ecosystem): | ||
+ | <code lisp> | ||
+ | BTW-TUT> (cl-tf: | ||
+ | </ | ||
+ | and the following does: | ||
+ | <code lisp> | ||
+ | BTW-TUT> (urdf-proj: | ||
+ | | ||
+ | </ | ||
+ | Here, ''" | ||
+ | |||
+ | ==== Moving the robot in the Bullet world ==== | ||
+ | |||
+ | In this part of the tutorial we will look into moving the robot and it's body parts as well as perceiving objects through the Bullet world. We will use functions from the '' | ||
+ | This teleporting is done by directly calling Prolog predicates that move objects in the world (for navigating the robot, simply teleport it to the goal), changing joint angles (to move the arm simply teleport the arm to given joint values) etc. '' | ||
+ | |||
+ | Another | ||
+ | Until now we had to write lengthy Prolog queries to access the world state and assert changes to it. | ||
+ | From now on we will use the utility functions from '' | ||
+ | There are functions such as '' | ||
+ | By pressing '' | ||
We need a clean environment for this tutorial, so let's clean the world: | We need a clean environment for this tutorial, so let's clean the world: | ||
Line 510: | Line 559: | ||
</ | </ | ||
- | Now, let' | + | Let us try to perceive |
+ | 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 519: | Line 569: | ||
BTW-TUT> (btr-utils: | BTW-TUT> (btr-utils: | ||
(cl-transforms: | (cl-transforms: | ||
- | | + | |
| | ||
</ | </ | ||
- | Lastly we simulate the world for 10 seconds to make sure, nothing moves unexpectedly | + | Lastly we simulate the world for 10 seconds to make sure, nothing moves unexpectedly |
<code lisp> | <code lisp> | ||
- | BTW-TUT> (btr: | + | BTW-TUT> (btr: |
</ | </ | ||
- | Before we grasp the bottle, | + | Before we perceive |
<code lisp> | <code lisp> | ||
- | BTW-TUT> (setf ? | + | BTW-TUT> (defparameter |
| | ||
" | " | ||
Line 538: | Line 588: | ||
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> (defparameter |
| | ||
" | " | ||
Line 545: | Line 595: | ||
(cl-transforms: | (cl-transforms: | ||
</ | </ | ||
- | To execute any plan in CRAM, we need a top-level context. Besides that we also use a macro to specify that the demo should be executed in simulation, not on the real robot. Putting your plan under '' | + | |
+ | < | ||
+ | To execute any plan in CRAM, we need a top-level context. Besides that we also use a macro to specify that the demo should be executed in simulation, not on the real robot. Putting your plan under '' | ||
+ | We can execute some movements in parallel, if they use different joints of the robot. That's what '' | ||
+ | We have used a simple call to low level methods to achieve motions like move to the ''? | ||
+ | --></ | ||
+ | |||
+ | Putting all these together we end up with the following: | ||
<code lisp> | <code lisp> | ||
BTW-TUT> | BTW-TUT> | ||
- | (pr2-proj: | + | (urdf-proj: |
- | (cpl:par | + | (urdf-proj:: |
- | (pp-plans:park-arms) | + | -0.26499816732737785d0 |
- | (pr2-proj:: | + | |
- | (pr2-proj:: | + | |
+ | | ||
+ | | ||
+ | | ||
+ | ' | ||
+ | -0.2567290370386635d0 | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | (urdf-proj:: | ||
+ | (urdf-proj:: | ||
+ | </ | ||
+ | As some of the functions in '' | ||
+ | The function '' | ||
+ | <code lisp> | ||
+ | (prolog: | ||
</ | </ | ||
- | We can execute some movements in parallel, if they use different joints of the robot. That's what ''cpl:par'' | + | '' |
- | To grasp the bottle we need to have its pose in the room. Therefore, we first perceive | + | Now, let us finally |
<code lisp> | <code lisp> | ||
BTW-TUT> | BTW-TUT> | ||
(defvar *perceived-object* nil " | (defvar *perceived-object* nil " | ||
- | (pr2-proj: | + | (urdf-proj: |
(setf *perceived-object* | (setf *perceived-object* | ||
- | (pr2-proj:: | + | (urdf-proj:: |
</ | </ | ||
- | 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 that resulting perceived object we could perform the picking up action. With the torso so far down we might not be able to reach the bottle, so we need to push the torso up: |
<code lisp> | <code lisp> | ||
- | (pr2-proj: | + | (urdf-proj: |
- | (let ((? | + | (urdf-proj:: |
- | (pr2-proj:: | + | |
</ | </ | ||
As there is no atomic motion for picking up an object, in fact, picking up is comprised of multiple move-arm motions, | As there is no atomic motion for picking up an object, in fact, picking up is comprised of multiple move-arm motions, | ||
- | so pick up is implemented within a plan and called by performing an action designator. | + | pick up is implemented within a plan and called by performing an action designator. |