Merge pull request #108 from Tigul/real-process-modules

Real process modules

demos/pycram_bullet_world_demo/demo.py

@@ -4,22 +4,24 @@
 from pycram.pose import Pose
 from pycram.bullet_world import BulletWorld, Object
 from pycram.process_module import simulated_robot, with_simulated_robot
+from pycram.enums import ObjectType
 
 world = BulletWorld()
-robot = Object("pr2", "robot", "pr2.urdf", pose=Pose([1, 2, 0]))
-apartment = Object("apartment", "environment", "apartment.urdf")
+robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf", pose=Pose([1, 2, 0]))
+apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf")
 
-milk = Object("milk", "milk", "milk.stl", pose=Pose([2.5, 2, 1.02]), color=[1, 0, 0, 1])
-cereal = Object("cereal", "cereal", "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), color=[0, 1, 0, 1])
-spoon = Object("spoon", "spoon", "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=[0, 0, 1, 1])
-bowl = Object("bowl", "bowl", "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=[1, 1, 0, 1])
+milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=[1, 0, 0, 1])
+cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), color=[0, 1, 0, 1])
+spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=[0, 0, 1, 1])
+bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=[1, 1, 0, 1])
 apartment.attach(spoon, 'cabinet10_drawer_top')
 
 pick_pose = Pose([2.7, 2.15, 1])
 
 robot_desig = BelieveObject(names=["pr2"])
 apartment_desig = BelieveObject(names=["apartment"])
 
+
 @with_simulated_robot
 def move_and_detect(obj_type):
     NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform()
@@ -36,15 +38,15 @@ def move_and_detect(obj_type):
 
     MoveTorsoAction([0.25]).resolve().perform()
 
-    milk_desig = move_and_detect("milk")
+    milk_desig = move_and_detect(ObjectType.MILK)
 
     TransportAction(milk_desig, ["left"], [Pose([4.8, 3.55, 0.8])]).resolve().perform()
 
-    cereal_desig = move_and_detect("cereal")
+    cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL)
 
     TransportAction(cereal_desig, ["right"], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform()
 
-    bowl_desig = move_and_detect("bowl")
+    bowl_desig = move_and_detect(ObjectType.BOWL)
 
     TransportAction(bowl_desig, ["left"], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform()
 
@@ -60,13 +62,17 @@ def move_and_detect(obj_type):
     # Detect and pickup the spoon
     LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform()
 
-    spoon_desig = DetectAction(BelieveObject(types=["spoon"])).resolve().perform()
+    spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform()
 
     pickup_arm = "left" if drawer_open_loc.arms[0] == "right" else "right"
     PickUpAction(spoon_desig, [pickup_arm], ["top"]).resolve().perform()
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
+    close_loc = drawer_open_loc.pose
+    close_loc.position.y += 0.1
+    NavigateAction([close_loc]).resolve().perform()
+
     CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform()
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()

doc/source/examples.rst

@@ -27,6 +27,13 @@ Misc
     notebooks/bullet_world
     notebooks/minimal_task_tree
     notebooks/pose
+    notebooks/custom_resolver
+
+Interface Examples
+==================
+
+.. nbgallery::
+    notebooks/giskard
 
 Object Relational Mapping
 =========================

doc/source/installation.rst

@@ -270,9 +270,10 @@ Here is an example how a setup script can look like.
     from pycram.designators.object_designator import *
     from pycram.process_module import simulated_robot
     from pycram.pose import Pose
+    from pycram.enums import ObjectType
 
     world = BulletWorld()
 
-    robot = Object("pr2", "robot", "pr2.urdf")
-    kitchen = Object("kitchen", "environment", "kitchen.urdf")
-    cereal = Object("cereal", "cereal", "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95]))
+    robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
+    kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
+    cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95]))

doc/source/notebooks/giskard.ipynb

@@ -0,0 +1 @@
+../../../examples/interface_examples/giskard.ipynb

doc/source/ros_utils.rst

@@ -78,3 +78,21 @@ publishes constantly in the background.
     from pycram.ros.viz_marker_publisher import VizMarkerPublisher
 
     v = VizMarkerPublisher()
+
+-------------------
+Robot State Updater
+-------------------
+The robot state updater is intended for working with a real robot, since the robot in the bullet world should mimic
+the real robot there has to be a module which takes care of updating the state of the robot in the BulletWorld. The
+robot state updated hooks into the TF and joint_state topic to get the current position as well as the current joint
+configuration of the robot and applies it to the robt in the BulletWorld.
+
+As all the other ROS utils the robot state updater only needs to be initialized and then takes care of himself.
+
+To initialize the robot state updater a TF topic as well as a joint state topic have to be provided.
+
+.. code-block:: python
+
+    from pycram.ros.robot_state_updater import RobotStateUpdater
+
+    r = RobotStateUpdater("/tf", "/joint_states")