Differences

This shows you the differences between two versions of the page.

Link to this comparison view

tutorials:intermediate:bullet_world [2019/03/07 17:00]
gkazhoya [Abstract entity descriptions]
tutorials:intermediate:bullet_world [2019/03/19 21:52] (current)
gkazhoya
Line 201: Line 201:
 </​code>​ </​code>​
 You should find all of the functions that we manually called above and one additional, the ''​(init-projection)''​ function, which initializes the Bullet world with the robot and the kitchen environment. You should find all of the functions that we manually called above and one additional, the ''​(init-projection)''​ function, which initializes the Bullet world with the robot and the kitchen environment.
 +
 +
 +
 +
  
  
Line 220: Line 224:
 </​code>​ </​code>​
  
-If you did everything right, the fridge door should now be slightly open. Check the other joints and change them as you wish. Watch their limits though. ​+If you did everything right, the fridge door should now be slightly open. Check the other joints and change them as you wish. Watch their limits though. 
 + 
 + 
 + 
 + 
  
 ==== Spawn and change objects in the world ==== ==== Spawn and change objects in the world ====
Line 257: Line 266:
    #<​QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>​    #<​QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>​
 </​code>​ </​code>​
 +
 +
 +
 +
 +
 +
  
 ==== Geometric reasoning ==== ==== Geometric reasoning ====
Line 291: Line 306:
 </​code>​ </​code>​
 and ask the visibility query again: instead of typing, put the cursor where you would type and press ''​Ctrl-Up''​. This will find previously executed commands. and ask the visibility query again: instead of typing, put the cursor where you would type and press ''​Ctrl-Up''​. This will find previously executed commands.
 +
 +
 +
 +
 +
  
 ==== Abstract entity descriptions ==== ==== Abstract entity descriptions ====
Line 354: Line 374:
  
  
-<​html> ​ 
-<!-- 
- ==== Grasp objects ==== 
  
-We need a clean environment ​for this, so let's clean the world:+ 
 + 
 +==== 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 attaches the object to the gripper, more specific, to the link of the rigid body of the gripper. Via prolog it is possible to attach objects to robots parts manually. This comes in handy when cutlery is stored in a drawer and should move, when the drawer opens. Following we will place a fork inside a drawer and attach it to the link of the drawer. This requires the kitchen to be loaded as URDF, not semantic map. See the chapter about how to load the kitchen, if in doubt. If you work on the latest version of this tutorials repositoryyou should be fine. 
 + 
 +Assuming that the cram_bullet_world_tutorial is already loaded, we need to reset the world:
 <code lisp> <code lisp>
-BTW-TUT> ​(roslisp-utilities:​startup-ros)+(roslisp-utilities:​startup-ros) ​;; restart world 
 +(add-objects-to-mesh-list) ;; load fork mesh
 </​code>​ </​code>​
 +
 +The function ''​set-robot-state-from-joints''​ can change the state of joints. A joint can be the hinge of a door, the rails of a drawer, the elbow of an arm of the PR2, basically everything that joins two parts and can be moved in some kind of way. The state of such a joint indicates its current position, e.g. how hard the elbow is bent or how far the drawer is pulled. The function takes two arguments, a list of joints with their desired positions, and the corresponding robot, in this case the kitchen.
 +
 +Since a closed drawer would disguise the fork, lets open it:
 +<code lisp>
 +(btr:​set-robot-state-from-joints
 + '​(("​sink_area_left_upper_drawer_main_joint" ​ 0.5))
 + ​(btr:​object btr:​*current-bullet-world* :kitchen))
 +</​code>​
 +
 +Spawn the fork inside the drawer:
 +<code lisp>
 +(btr-utils:​spawn-object
 + '​fork-1
 + :​fork-plastic
 + :pose '((1.0 0.9 0.75) (0 0 0 1)))
 +</​code>​
 +
 +If this throws an error, you probably forgot to use ''​(add-objects-to-mesh-list)''​ previously, since we need the fork mesh. The whole scene should now look like this:
 +
 +{{:​tutorials:​intermediate:​btw-tut-opendrawer.png?​800|}} {{:​tutorials:​intermediate:​btw-tut-drawerwfork.png?​800|}}
 +
 +Try and move the drawer a bit with ''​set-robot-state-from-joints''​. You'll see, that the fork stays in place, even if the scene is simulated with ''​(btr:​simulate btr:​*current-bullet-world* 10)''​. To solve this, the fork needs to be attached to this specific drawer. All parts and links of the kitchen can be inspected with ''​(btr:​object btr:​*current-bullet-world* :​kitchen)'',​ but to save some time the following example already contains the correct link.
 +<code lisp>
 +(prolog '(and (btr:​bullet-world ?world)
 +              (btr:​%object ?world fork-1 ?fork)
 +              (assert (btr:​attached ?world :kitchen "​sink_area_left_upper_drawer_main"​ ?fork))))
 +</​code>​
 +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:
 +<code lisp>
 +(prolog '(and (btr:​bullet-world ?world)
 +              (btr:​attached ?world :kitchen ?_ fork-1)))
 +</​code>​
 +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 ''?​link'',​ to get the list of links the object is attached to. To detach an object, the ''​retract''​ predicate does the job.
 +<code lisp>
 +(prolog '(and (btr:​bullet-world ?world)
 +              (assert (btr:​retract ?world :kitchen fork-1 ?_))))
 +</​code>​
 +In the fourth argument of ''​retract'',​ which is kept blank here, it is possible to specify the link name, the object should be detached from, but this is only needed if the object is attached to multiple links. ​
 +
 +
 +
 +
 +
 +
 +==== Visualizing coordinate frames of poses ====
 +
 +In bullet_world it is not possible to view an object in relation to its orientation. However, the knowledge of the orientation is important for many manipulation tasks that the robot has to perform. There is a function called ''​btr:​add-vis-axis-object''​ for this. Here you can specify as parameters either a pose or an object name e.g:
 +
 +<code lisp>
 +
 +(btr:​add-vis-axis-object (cl-transforms:​make-pose (cl-transforms:​make-3d-vector 1 0 1) (cl-transforms:​make-identity-rotation)))
 +
 +;;After spawning an object (like above in the code) with the name mug-1
 +(btr:​add-vis-axis-object '​mug-1)
 +</​code>​
 +
 +{{:​tutorials:​intermediate:​axis_spawning_object.png |}}
 +                 
 +                 
 +                 
 +                                 
 +<​html>​ <!--
 +==== Light-weight simulation (projection of plans) ====
 +
 +Now, let's use light-weight simulation to try to find our particular red mug:
 +<code lisp>
 +DEMO> ​
 +(with-projection-environment pr2-bullet-projection-environment
 +  (top-level
 +    (with-designators
 +        ((on-counter :location `((:on "​CounterTop"​)
 +                                 ​(:​name "​kitchen_island_counter_top"​)))
 +         ​(an-object :object `((:type :mug)
 +                              (:at ,​on-counter))))
 +      (reference on-counter)
 +      (format t "​Searching for the mug~%"​)
 +      (let ((perceived-object (perceive-object 'a an-object)))
 +        (unless (desig-equal an-object perceived-object)
 +          (equate an-object perceived-object)) ​     ​
 +        an-object))))
 +</​code>​
 +We execute our plan in a projection environment which substitutes the real robot controllers and sensors with their simplified models. Next, we define our object that we are searching for in a symbolic abstract way. After that, we execute the ''​perceive-object''​ plan from the standard CRAM plan library. We search for "**a** mug", as opposed to "​**the** mug", the latter would throw an error if there would be multiple mugs found on the table. Finally, we specify that the new ''​perceived-object''​ that the robot found while executing the plan is the same object that we described in the designator ''​an-object'',​ so that whenever we use the variable ''​an-object''​ from now on, the robot will know exactly what mug we are referring to.
 +
 +To see an execution of a complete pick and place plan try the following:
 +<code lisp>
 +DEMO> (start-ros-and-bullet)
 +      (execute-demo :​pancake-making)
 +</​code>​
 +This will execute the plan from the pancake making demo. The code can be found in the ''​spatial_relations_demo''​ package, in the ''​pancake-making.lisp''​ file. 
 +--> </​html>​
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +==== Executing motions ====
 +
 From now on we will use the utility functions from ''​cram_bullet_reasoning_utilities''​ package, to save time on writing lengthy Prolog queries. From now on we will use the utility functions from ''​cram_bullet_reasoning_utilities''​ package, to save time on writing lengthy Prolog queries.
 +The package ''​cram_bullet_reasoning_utilities''​ has a number of utility functions to make the rapid prototyping with the Bullet world faster and easier, there are functions such as ''​spawn-object'',​ ''​move-object'',​ ''​kill-all-objects'',​ ''​move-robot''​ etc. that execute the Prolog queries with default values of parameters that are not important, e.g. default colors for objects. ​
 +By pressing ''​.''​ while holding ''​Alt''​ (''​Alt-.''​) while the Emacs cursor is on the function name you will be redirected to the definition of the function to see what exactly it does. ''​Alt-,''​ brings back the previous window. Try this for ''​init-projection'',​ for example.
 +
 +We need a clean environment for this tutorial, so let's clean the world:
 +<code lisp>
 +BTW-TUT> (roslisp-utilities:​startup-ros)
 +</​code>​
  
-We need an object ​to grasp. For that we will use a mesh of a bottle loaded from the ''​resources''​ subdirectory of the tutorial.+Now, let's try to grasp an object. For that we will use a mesh of a bottle loaded from the ''​resources''​ subdirectory of the tutorial.
 <code lisp> <code lisp>
 BTW-TUT> (add-objects-to-mesh-list) BTW-TUT> (add-objects-to-mesh-list)
Line 426: Line 559:
     (let ((?​perceived-bottle-desig *perceived-object*))     (let ((?​perceived-bottle-desig *perceived-object*))
       (pr2-proj::​move-torso 0.3)       (pr2-proj::​move-torso 0.3)
 +</​code>​
 +
 +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. This is explained in the [[http://​cram-system.org/​tutorials/​intermediate/​simple_mobile_manipulation_plan|next tutorial on writing simple mobile manipulation plans]].
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +
 +<​html> ​
 +<!--
       (exe:​perform       (exe:​perform
        ​(desig:​an action        ​(desig:​an action
Line 497: Line 652:
 --> -->
 </​html>​ </​html>​
- 
- 
- 
-==== Attach 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 attaches the object to the gripper, more specific, to the link of the rigid body of the gripper. Via prolog it is possible to attach objects to robots parts manually. This comes in handy when cutlery is stored in a drawer and should move, when the drawer opens. Following we will place a fork inside a drawer and attach it to the link of the drawer. This requires the kitchen to be loaded as URDF, not semantic map. See the chapter about how to load the kitchen, if in doubt. If you work on the latest version of this tutorials repository, you should be fine. 
- 
-Assuming that the cram_bullet_world_tutorial is already loaded, we need to reset the world: 
-<code lisp> 
-(roslisp-utilities:​startup-ros) ;; restart world 
-(add-objects-to-mesh-list) ;; load fork mesh 
-</​code>​ 
- 
-The function ''​set-robot-state-from-joints''​ can change the state of joints. A joint can be the hinge of a door, the rails of a drawer, the elbow of an arm of the PR2, basically everything that joins two parts and can be moved in some kind of way. The state of such a joint indicates its current position, e.g. how hard the elbow is bent or how far the drawer is pulled. The function takes two arguments, a list of joints with their desired positions, and the corresponding robot, in this case the kitchen. 
- 
-Since a closed drawer would disguise the fork, lets open it: 
-<code lisp> 
-(btr:​set-robot-state-from-joints 
- '​(("​sink_area_left_upper_drawer_main_joint" ​ 0.5)) 
- ​(btr:​object btr:​*current-bullet-world* :kitchen)) 
-</​code>​ 
- 
-Spawn the fork inside the drawer: 
-<code lisp> 
-(btr-utils:​spawn-object 
- '​fork-1 
- :​fork-plastic 
- :pose '((1.0 0.9 0.75) (0 0 0 1))) 
-</​code>​ 
- 
-If this throws an error, you probably forgot to use ''​(add-objects-to-mesh-list)''​ previously, since we need the fork mesh. The whole scene should now look like this: 
- 
-{{:​tutorials:​intermediate:​btw-tut-opendrawer.png?​800|}} {{:​tutorials:​intermediate:​btw-tut-drawerwfork.png?​800|}} 
- 
-Try and move the drawer a bit with ''​set-robot-state-from-joints''​. You'll see, that the fork stays in place, even if the scene is simulated with ''​(btr:​simulate btr:​*current-bullet-world* 10)''​. To solve this, the fork needs to be attached to this specific drawer. All parts and links of the kitchen can be inspected with ''​(btr:​object btr:​*current-bullet-world* :​kitchen)'',​ but to save some time the following example already contains the correct link. 
-<code lisp> 
-(prolog '(and (btr:​bullet-world ?world) 
-              (btr:​%object ?world fork-1 ?fork) 
-              (assert (btr:​attached ?world :kitchen "​sink_area_left_upper_drawer_main"​ ?fork)))) 
-</​code>​ 
-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: 
-<code lisp> 
-(prolog '(and (btr:​bullet-world ?world) 
-              (btr:​attached ?world :kitchen ?_ fork-1))) 
-</​code>​ 
-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 ''?​link'',​ to get the list of links the object is attached to. To detach an object, the ''​retract''​ predicate does the job. 
-<code lisp> 
-(prolog '(and (btr:​bullet-world ?world) 
-              (assert (btr:​retract ?world :kitchen fork-1 ?_)))) 
-</​code>​ 
-In the fourth argument of ''​retract'',​ which is kept blank here, it is possible to specify the link name, the object should be detached from, but this is only needed if the object is attached to multiple links. ​ 
- 
- 
-                                  
-<​html>​ <!-- 
-==== Light-weight simulation (projection of plans) ==== 
- 
-Now, let's use light-weight simulation to try to find our particular red mug: 
-<code lisp> 
-DEMO> ​ 
-(with-projection-environment pr2-bullet-projection-environment 
-  (top-level 
-    (with-designators 
-        ((on-counter :location `((:on "​CounterTop"​) 
-                                 ​(:​name "​kitchen_island_counter_top"​))) 
-         ​(an-object :object `((:type :mug) 
-                              (:at ,​on-counter)))) 
-      (reference on-counter) 
-      (format t "​Searching for the mug~%"​) 
-      (let ((perceived-object (perceive-object 'a an-object))) 
-        (unless (desig-equal an-object perceived-object) 
-          (equate an-object perceived-object)) ​     ​ 
-        an-object)))) 
-</​code>​ 
-We execute our plan in a projection environment which substitutes the real robot controllers and sensors with their simplified models. Next, we define our object that we are searching for in a symbolic abstract way. After that, we execute the ''​perceive-object''​ plan from the standard CRAM plan library. We search for "**a** mug", as opposed to "​**the** mug", the latter would throw an error if there would be multiple mugs found on the table. Finally, we specify that the new ''​perceived-object''​ that the robot found while executing the plan is the same object that we described in the designator ''​an-object'',​ so that whenever we use the variable ''​an-object''​ from now on, the robot will know exactly what mug we are referring to. 
- 
-To see an execution of a complete pick and place plan try the following: 
-<code lisp> 
-DEMO> (start-ros-and-bullet) 
-      (execute-demo :​pancake-making) 
-</​code>​ 
-This will execute the plan from the pancake making demo. The code can be found in the ''​spatial_relations_demo''​ package, in the ''​pancake-making.lisp''​ file.  
---> </​html>​ 
----- 
- 
- 
-The package ''​cram_bullet_reasoning_utilities''​ has a number of utility functions to make the rapid prototyping with the Bullet world faster and easier, there are functions such as ''​spawn-object'',​ ''​move-object'',​ ''​kill-all-objects'',​ ''​move-robot''​ etc. that execute the Prolog queries with default values of parameters that are not important, e.g. default colors for objects. ​ 
-By pressing ''​.''​ while holding ''​Alt''​ (''​Alt-.''​) while the Emacs cursor is on the function name you will be redirected to the definition of the function to see what exactly it does. ''​Alt-,''​ brings back the previous window. Try this for ''​init-projection'',​ for example.