Merge pull request cram2#250 from LucaKro/dev_processmodule_merge

[TorsoState] Introduction of abstract torso states to better represent complex torsos

README.md

@@ -50,7 +50,7 @@ robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve()
 with simulated_robot:
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-    MoveTorsoAction([0.3]).resolve().perform()
+    MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
 
     pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()
     pickup_arm = pickup_pose.reachable_arms[0]

demos/pycram_bullet_world_demo/demo.py

@@ -50,7 +50,7 @@ def move_and_detect(obj_type):
 with simulated_robot:
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-    MoveTorsoAction([0.25]).resolve().perform()
+    MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
 
     milk_desig = move_and_detect(Milk)
 
@@ -89,7 +89,7 @@ def move_and_detect(obj_type):
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-    MoveTorsoAction([0.15]).resolve().perform()
+    MoveTorsoAction([TorsoState.MID]).resolve().perform()
 
     # Find a pose to place the spoon, move and then place it
     spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1])

doc/source/index.rst

@@ -59,7 +59,7 @@ The code for this plan can be seen below.
     from pycram.designators.location_designator import *
     from pycram.designators.action_designator import *
     from pycram.designators.object_designator import *
-    from pycram.datastructures.enums import ObjectType, Arms, Grasp, WorldMode
+    from pycram.datastructures.enums import ObjectType, Arms, Grasp, WorldMode, TorsoState
 
     world = BulletWorld(WorldMode.GUI)
     kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
@@ -73,7 +73,7 @@ The code for this plan can be seen below.
     with simulated_robot:
         ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-        MoveTorsoAction([0.3]).resolve().perform()
+        MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
 
         pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()
         pickup_arm = pickup_pose.reachable_arms[0]

examples/action_designator.md

@@ -93,8 +93,9 @@ a {meth}`~pycram.process_module.simulated_robot` environment.
 ```python
 from pycram.designators.action_designator import MoveTorsoAction
 from pycram.process_module import simulated_robot
+from pycram.datastructures.enums import TorsoState
 
-torso_pose = 0.2
+torso_pose = TorsoState.HIGH
 
 torso_desig = MoveTorsoAction([torso_pose]).resolve()
 
@@ -151,7 +152,7 @@ world.reset_world()
 from pycram.designators.action_designator import PickUpAction, PlaceAction, ParkArmsAction, MoveTorsoAction,NavigateAction
 from pycram.designators.object_designator import BelieveObject
 from pycram.process_module import simulated_robot
-from pycram.datastructures.enums import Arms, Grasp
+from pycram.datastructures.enums import Arms, Grasp, TorsoState
 from pycram.datastructures.pose import Pose
 
 milk_desig = BelieveObject(names=["milk"])
@@ -160,7 +161,7 @@ arm = Arms.RIGHT
 with simulated_robot:
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-    MoveTorsoAction([0.3]).resolve().perform()
+    MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
 
     NavigateAction([Pose([1.8, 2, 0.0],
                          [0.0, 0.0, 0., 1])]).resolve().perform()
@@ -239,7 +240,7 @@ from pycram.designators.action_designator import *
 from pycram.designators.object_designator import *
 from pycram.process_module import simulated_robot
 from pycram.datastructures.pose import Pose
-from pycram.datastructures.enums import Arms
+from pycram.datastructures.enums import Arms, TorsoState
 
 milk_desig = BelieveObject(names=["milk"])
 
@@ -248,7 +249,7 @@ description = TransportAction(milk_desig,
                                        [0, 0, 0, 1])],
                               [Arms.LEFT])
 with simulated_robot:
-    MoveTorsoAction([0.2]).resolve().perform()
+    MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
     description.resolve().perform()
 ```
 
@@ -267,15 +268,15 @@ world.reset_world()
 ```python
 from pycram.designators.action_designator import *
 from pycram.designators.object_designator import *
-from pycram.datastructures.enums import Arms
+from pycram.datastructures.enums import Arms, TorsoState
 from pycram.process_module import simulated_robot
 from pycram.datastructures.pose import Pose
 
 apartment_desig = BelieveObject(names=["apartment"]).resolve()
 handle_deisg = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig)
 
 with simulated_robot:
-    MoveTorsoAction([0.25]).resolve().perform()
+    MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
     ParkArmsAction([Arms.BOTH]).resolve().perform()
     NavigateAction([Pose([1.7474915981292725, 2.6873629093170166, 0.0],
                          [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform()
@@ -301,7 +302,7 @@ apartment_desig = BelieveObject(names=["apartment"]).resolve()
 handle_deisg = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig)
 
 with simulated_robot:
-    MoveTorsoAction([0.25]).resolve().perform()
+    MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
     ParkArmsAction([Arms.BOTH]).resolve().perform()
     NavigateAction([Pose([1.7474915981292725, 2.8073629093170166, 0.0],
                          [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform()

examples/cram_plan_tutorial.md

@@ -146,11 +146,12 @@ from pycram.datastructures.enums import Grasp
 
 
 @pycram.tasktree.with_tree
-def plan(obj_desig: ObjectDesignatorDescription.Object, torso=0.2, place=counter_name):
+def plan(obj_desig: ObjectDesignatorDescription.Object, torso=None, place=counter_name):
     world.reset_world()
     with simulated_robot:
         ParkArmsActionPerformable(Arms.BOTH).perform()
-
+        if torso is None:
+            torso={"torso_lift_joint": 0.2}
         MoveTorsoActionPerformable(torso).perform()
         location = CostmapLocation(target=obj_desig, reachable_for=robot_desig)
         pose = location.resolve()
@@ -206,7 +207,7 @@ def plan(obj_desig: ObjectDesignatorDescription.Object, torso=0.2, place=counter
 good_torsos = []
 for obj_name in object_names:
     done = False
-    torso = 0.3 if len(good_torsos) == 0 else good_torsos[-1]
+    torso = {"torso_lift_joint": 0.3}if len(good_torsos) == 0 else good_torsos[-1]
     while not done:
         try:
             plan(object_desig[obj_name], torso=torso, place=counter_name)

examples/improving_actions.md

@@ -203,8 +203,9 @@ from pycram.designators.action_designator import ParkArmsActionPerformable
 
 world.reset_world()
 milk.set_pose(Pose([0.5, 3.15, 1.04]))
+torso_joint = RobotDescription.current_robot_description.torso_joint
 with simulated_robot:
-    MoveTorsoActionPerformable(0.3).perform()
+    MoveTorsoActionPerformable({torso_joint: 0.3}).perform()
     for sample in fpa:
         try:
             ParkArmsActionPerformable(Arms.RIGHT).perform()

examples/intro.md

@@ -326,15 +326,15 @@ To get familiar with the PyCRAM Framework we will write a simple pick and place
 a cereal box from the kitchen counter and place it on the kitchen island. This is a simple pick and place plan.
 
 ```python
-from pycram.datastructures.enums import Grasp
+from pycram.datastructures.enums import Grasp, TorsoState
 
 cereal_desig = ObjectDesignatorDescription(names=["cereal"])
 kitchen_desig = ObjectDesignatorDescription(names=["kitchen"])
 robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve()
 with simulated_robot:
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-    MoveTorsoAction([0.33]).resolve().perform()
+    MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
 
     pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()
     pickup_arm = pickup_pose.reachable_arms[0]

examples/language.md

@@ -200,7 +200,7 @@ world.reset_world()
 
 navigate = NavigateAction([Pose([1, 1, 0])])
 park = ParkArmsAction([Arms.BOTH])
-move_torso = MoveTorsoAction([0.3])
+move_torso = MoveTorsoAction([TorsoState.HIGH]) 
 
 plan = navigate | park + move_torso
 
@@ -286,9 +286,10 @@ You can see an example of how to use Repeat below.
 ```python
 from pycram.designators.action_designator import *
 from pycram.process_module import simulated_robot
+from pycram.datastructures.enums import TorsoState
 
-move_torso_up = MoveTorsoAction([0.3])
-move_torso_down = MoveTorsoAction([0.])
+move_torso_up = MoveTorsoAction([TorsoState.HIGH])
+move_torso_down = MoveTorsoAction([TorsoState.LOW])
 
 plan = (move_torso_up + move_torso_down) * 5
 
@@ -311,9 +312,10 @@ from pycram.designators.action_designator import *
 from pycram.process_module import simulated_robot
 from pycram.language import Monitor
 import time
+from pycram.datastructures.enums import TorsoState
 
-move_torso_up = MoveTorsoAction([0.3])
-move_torso_down = MoveTorsoAction([0.])
+move_torso_up = MoveTorsoAction([TorsoState.HIGH])
+move_torso_down = MoveTorsoAction([TorsoState.LOW])
 
 
 def monitor_func():

examples/migrate_neems.md

@@ -56,6 +56,7 @@ from pycram.tasktree import with_tree
 from pycram.worlds.bullet_world import BulletWorld
 from pycram.world_concepts.world_object import Object
 from pycram.designators.object_designator import *
+from pycram.datastructures.enums import TorsoState
 import pycrap
 
 
@@ -75,7 +76,7 @@ class ExamplePlans:
     def pick_and_place_plan(self):
         with simulated_robot:
             ParkArmsAction([Arms.BOTH]).resolve().perform()
-            MoveTorsoAction([0.3]).resolve().perform()
+            MoveTorsoAction([TorsoState.HIGH]).resolve().perform()
             pickup_pose = CostmapLocation(target=self.cereal_desig.resolve(), reachable_for=self.robot_desig).resolve()
             pickup_arm = pickup_pose.reachable_arms[0]
             NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()

examples/minimal_task_tree.md

@@ -29,7 +29,7 @@ from pycram.designators.location_designator import *
 from pycram.process_module import simulated_robot
 from pycram.designators.object_designator import *
 from pycram.datastructures.pose import Pose
-from pycram.datastructures.enums import ObjectType, WorldMode
+from pycram.datastructures.enums import ObjectType, WorldMode, TorsoState
 import anytree
 import pycram.failures
 import pycrap
@@ -56,7 +56,7 @@ Finally, we create a plan where the robot parks his arms, walks to the kitchen c
 def plan():
     with simulated_robot:
         ParkArmsActionPerformable(Arms.BOTH).perform()
-        MoveTorsoAction([0.22]).resolve().perform()
+        MoveTorsoAction([TorsoState.MID]).resolve().perform()
         pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()
         pickup_arm = pickup_pose.reachable_arms[0]
         NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()

examples/orm_example.md

@@ -39,7 +39,7 @@ Next we will write a simple plan where the robot parks his arms and then moves s
 from pycram.designators.action_designator import *
 from pycram.designators.location_designator import *
 from pycram.process_module import simulated_robot
-from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode
+from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode, TorsoState
 from pycram.tasktree import with_tree
 import pycram.tasktree
 from pycram.worlds.bullet_world import BulletWorld
@@ -64,7 +64,7 @@ kitchen_desig = ObjectDesignatorDescription(names=["kitchen"])
 def plan():
     with simulated_robot:
         ParkArmsActionPerformable(Arms.BOTH).perform()
-        MoveTorsoAction([0.2]).resolve().perform()
+        MoveTorsoAction([TorsoState.MID]).resolve().perform()
         pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()
         pickup_arm = pickup_pose.reachable_arms[0]
         NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()

src/pycram/datastructures/enums.py

@@ -94,6 +94,15 @@ class Shape(Enum):
     CAPSULE = 7
 
 
+class TorsoState(Enum):
+    """
+    Enum for the different states of the torso.
+    """
+    HIGH = auto()
+    MID = auto()
+    LOW = auto()
+
+
 class WorldMode(Enum):
     """
     Enum for the different modes of the world.