Merge pull request #980 from tue-robotics/fix/reintroduce_arm_require…

…ments This reverts commit cf87033.
tue-robotics · Feb 6, 2020 · 8de7d9f · 8de7d9f
2 parents 0ff0ac4 + a04bfc5
commit 8de7d9f
Show file tree

Hide file tree

Showing 17 changed files with 135 additions and 88 deletions.
diff --git a/challenge_cleanup/src/challenge_cleanup/self_cleanup.py b/challenge_cleanup/src/challenge_cleanup/self_cleanup.py
@@ -5,6 +5,7 @@
 
 from robot_skills.util.kdl_conversions import FrameStamped
 from robot_skills.util.entity import Entity
+from robot_skills import arms
 import robot_smach_states.util.designators as ds
 
 from robocup_knowledge import load_knowledge
@@ -261,20 +262,25 @@ def __init__(self, robot, selected_entity_designator, room_des):
                                                                    block=True),
                                    transitions={"spoken": "GRAB"})
 
-            smach.StateMachine.add("GRAB",
-                                   robot_smach_states.Grab(robot, selected_entity_designator,
-                                                           ds.UnoccupiedArmDesignator(robot, {},
-                                                                                      name="empty_arm_designator")),
-                                   transitions={"done": "SAY_GRAB_SUCCESS",
-                                                "failed": "ARM_RESET"})
-
-            smach.StateMachine.add("ARM_RESET", robot_smach_states.ArmToJointConfig(robot,
-                                                                                    ds.UnoccupiedArmDesignator(robot,
-                                                                                                               {},
-                                                                                    name="empty_arm_designator"),
-                                                                                    "reset"),
-                                   transitions={"succeeded": "SAY_GRAB_FAILED",
-                                                "failed": "SAY_GRAB_FAILED"})
+            smach.StateMachine.add(
+                "GRAB",
+                robot_smach_states.Grab(
+                    robot,
+                    selected_entity_designator,
+                    ds.UnoccupiedArmDesignator(robot,
+                                               arm_properties={"required_trajectories": ["prepare_grasp"],
+                                                               "required_goals": ["carrying_pose"],
+                                                               "required_gripper_types": [arms.GripperTypes.GRASPING]},
+                                               name="empty_arm_designator")),
+                transitions={"done": "SAY_GRAB_SUCCESS", "failed": "ARM_RESET"})
+
+            smach.StateMachine.add("ARM_RESET",robot_smach_states.ArmToJointConfig(
+                    robot,
+                    ds.UnoccupiedArmDesignator(robot,
+                                               arm_properties={"required_goals": ["reset"]},
+                                               name="empty_arm_designator"),
+                    "reset"),
+                transitions={"succeeded": "SAY_GRAB_FAILED", "failed": "SAY_GRAB_FAILED"})
 
             smach.StateMachine.add('SAY_GRAB_SUCCESS',
                                    robot_smach_states.Say(robot, ["Now I am going to move this item",
@@ -330,22 +336,27 @@ def __init__(self, robot, selected_entity_designator, room_des):
                                    transitions={"done": "PLACE_IN_TRASH",
                                                 "failed": "SAY_PLACE_FAILED"})
 
+            arm_properties_place = {"required_trajectories": ["prepare_place"],
+                                    "required_gripper_types": [arms.GripperTypes.GRASPING]}
+            arm_designator_place = ds.OccupiedArmDesignator(robot, arm_properties_place, name="occupied_arm_designator")
+
             smach.StateMachine.add('PLACE_IN_TRASH',
                                    robot_smach_states.Place(robot,
                                                             selected_entity_designator,
                                                             trash_place_pose,
-                                                            ds.OccupiedArmDesignator(robot, {},
-                                                                                  name="occupied_arm_designator")),
-                                   transitions={"done": "SAY_PLACE_SUCCESS",
-                                                "failed": "SAY_PLACE_FAILED"})
-
-            smach.StateMachine.add('PLACE_TO_STORE', robot_smach_states.Place(robot, selected_entity_designator,
-                                                                              store_entity_des,
-                                                                              ds.OccupiedArmDesignator(robot, {}, name=
-                                                                                "occupied_arm_designator"),
-                                                                                "on_top_of"),
+                                                            arm_designator_place),
                                    transitions={"done": "SAY_PLACE_SUCCESS",
                                                 "failed": "SAY_PLACE_FAILED"})
+
+            arm_designator_place_store = ds.OccupiedArmDesignator(robot, arm_properties_place,
+                                                                  name="occupied_arm_designator")
+            smach.StateMachine.add('PLACE_TO_STORE',
+                                   robot_smach_states.Place(robot,
+                                                            selected_entity_designator,
+                                                            store_entity_des,
+                                                            arm_designator_place_store,
+                                                            "on_top_of"),
+                                   transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"})
 
             smach.StateMachine.add('SAY_PLACE_SUCCESS',
                                    robot_smach_states.Say(robot, ["Bye bye!",

diff --git a/challenge_help_me_carry/src/challenge_help_me_carry.py b/challenge_help_me_carry/src/challenge_help_me_carry.py
@@ -7,7 +7,7 @@
 import robot_smach_states.util.designators as ds
 from robot_smach_states.manipulation.place_designator import EmptySpotDesignator
 
-from robot_skills.arms import GripperState
+from robot_skills.arms import GripperState, GripperTypes
 from robocup_knowledge import load_knowledge
 from challenge_hmc_functions import hmc_states
 from robot_skills.util import kdl_conversions
@@ -28,7 +28,14 @@ def __init__(self, robot):
 
         self.car_waypoint = ds.EntityByIdDesignator(robot, id=challenge_knowledge.waypoint_car['id'])
 
-        self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator")
+        self.empty_arm_designator = ds.UnoccupiedArmDesignator(
+            robot,
+            arm_properties={"required_goals": ["handover_to_human",
+                                               "reset",
+                                               challenge_knowledge.driving_bag_pose,
+                                               challenge_knowledge.drop_bag_pose],
+                            "required_gripper_types": [GripperTypes.GRASPING]},
+            name="empty_arm_designator")
 
         # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
         # For this challenge, unlocking is not needed.
@@ -42,10 +49,7 @@ def __init__(self, robot):
                                                                           area=challenge_knowledge.default_area),
                                                    name="place_position")
 
-
-
         # We don't actually grab something, so there is no need for an actual thing to grab
-
         self.current_item = ds.VariableDesignator(Entity("dummy", "dummy", "/{}/base_link".format(robot.robot_name),
                                                          kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
                                                          datetime.datetime.now()), name="current_item")

diff --git a/challenge_manipulation/src/manipulation.py b/challenge_manipulation/src/manipulation.py
@@ -21,7 +21,6 @@
 
 import rospy
 import smach
-import random
 
 # ED
 from ed_robocup_msgs.srv import FitEntityInImage, FitEntityInImageRequest
@@ -32,13 +31,11 @@
 from robot_smach_states.util.startup import startup
 from robot_smach_states import Grab
 from robot_smach_states import Place
-from robot_smach_states.util.geometry_helpers import *
 from robot_skills.util.kdl_conversions import VectorStamped
 
 # Robot Skills
 from robot_skills.util.entity import Entity
-from robot_skills.util import msg_constructors as geom
-from robot_skills.util import transformations
+from robot_skills import arms
 
 # RoboCup knowledge
 from robocup_knowledge import load_knowledge
@@ -458,9 +455,18 @@ def weight_function(entity):
                                                                         name="placement", area=PLACE_SHELF),
                                                    name="place_position")
 
-        self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot, {'required_arm_name': PREFERRED_ARM},
+        self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot,
+                                                               {'required_trajectories': ['prepare_grasp'],
+                                                                'required_goals': ['carrying_pose'],
+                                                                'required_gripper_types': [arms.GripperTypes.GRASPING],
+                                                                'required_arm_name': PREFERRED_ARM},
                                                                name="empty_arm_designator")
-        self.arm_with_item_designator = ds.ArmHoldingEntityDesignator(robot, {'required_objects':[self.current_item]},
+        self.arm_with_item_designator = ds.ArmHoldingEntityDesignator(robot,
+                                                                      {'required_objects': [self.current_item]},
+                                                                      {"required_trajectories": ["prepare_place"],
+                                                                       "required_goals": ["reset", "handover_to_human"],
+                                                                       'required_gripper_types': [
+                                                                           arms.GripperTypes.GRASPING]},
                                                                       name="arm_with_item_designator")
 
         # print "{0} = pick_shelf".format(self.pick_shelf)

diff --git a/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py b/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py
@@ -4,6 +4,7 @@
 
 # TU/e
 import robot_skills
+from robot_skills import arms
 import robot_smach_states as states
 import robot_smach_states.util.designators as ds
 from robot_smach_states.manipulation.place_designator import EmptySpotDesignator
@@ -76,7 +77,11 @@ def __init__(self, robot, grab_designator=None):
         smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
 
         # Create designators
-        self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator")
+        self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot,
+                                                               {"required_trajectories": ["prepare_grasp"],
+                                                                "required_goals": ["carrying_pose"],
+                                                                "required_gripper_types": [arms.GripperTypes.GRASPING]},
+                                                               name="empty_arm_designator")
         self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator)
 
         with self:
@@ -132,7 +137,11 @@ def __init__(self, robot, place_designator):
     def execute(self, userdata=None):
         # Try to place the object
         item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
-        arm_designator = ds.OccupiedArmDesignator(self._robot)
+        arm_designator = ds.OccupiedArmDesignator(self._robot,
+                                                  arm_properties={
+                                                      "required_trajectories": ["prepare_place"],
+                                                      "required_goals": ["reset", "handover_to_human"],
+                                                      "required_gripper_types": [arms.GripperTypes.GRASPING]})
         resolved_arm = arm_designator.resolve()
         if resolved_arm is None:
             rospy.logwarn("No arm holding an entity")

diff --git a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py
@@ -6,6 +6,7 @@
 import robot_skills
 import robot_smach_states as states
 import robot_smach_states.util.designators as ds
+from robot_skills import arms
 from robot_smach_states.manipulation.place_designator import EmptySpotDesignator
 
 # Challenge storing groceries
@@ -83,7 +84,11 @@ def __init__(self, robot, grab_designator=None):
         smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
 
         # Create designators
-        self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator")
+        self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot, {"required_trajectories": ["prepare_grasp"],
+                                                                       "required_goals": ["carrying_pose"],
+                                                                       "required_gripper_types": [
+                                                                           arms.GripperTypes.GRASPING]},
+                                                               name="empty_arm_designator")
         self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator)
 
         with self:
@@ -154,7 +159,8 @@ def __init__(self, robot, place_designator):
     def execute(self, userdata=None):
         # Try to place the object
         item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
-        arm_designator = ds.OccupiedArmDesignator(self._robot)
+        arm_designator = ds.OccupiedArmDesignator(self._robot, {"required_goals": ["reset", "handover_to_human"],
+                                                                "required_gripper_types": [arms.GripperTypes.GRASPING]})
         resolved_arm = arm_designator.resolve()
         if resolved_arm is None:
             rospy.logwarn("No arm holding an entity")

diff --git a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py
@@ -5,7 +5,7 @@
 # TU/e
 import robot_smach_states as states
 import robot_smach_states.util.designators as ds
-from robot_skills.util.kdl_conversions import FrameStamped
+from robot_skills import arms
 from robocup_knowledge import load_knowledge
 CHALLENGE_KNOWLEDGE = load_knowledge('challenge_take_out_the_garbage')
 
@@ -91,7 +91,10 @@ def __init__(self, robot, drop_zone_id):
         """
         smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
 
-        arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
+        arm_designator = ds.OccupiedArmDesignator(
+            robot=robot,
+            arm_properties={"required_goals": ["handover", "reset", "handover_to_human"],
+                            "required_gripper_types": [arms.GripperTypes.GRASPING]})
 
         with self:
             smach.StateMachine.add("GO_TO_COLLECTION_ZONE",

diff --git a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py
@@ -333,7 +333,8 @@ def _joint_path(_):
                                                 "failed": "failed",
                                                 "timeout": "TIMEOUT"})
 
-            arm_occupied_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
+            arm_occupied_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={"required_goals": ["reset"
+                                                                                                               ]})
 
             smach.StateMachine.add("LOWER_ARM", states.ArmToJointConfig(robot=robot,
                                                                         arm_designator=arm_occupied_designator,
@@ -353,13 +354,14 @@ def _joint_path(_):
 if __name__ == '__main__':
     import os
     import robot_smach_states.util.designators as ds
-    from robot_skills import Hero
+    from robot_skills import Hero, arms
 
     rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
     hero = Hero()
     hero.reset()
 
-    arm = ds.UnoccupiedArmDesignator(hero, {})
+    arm = ds.UnoccupiedArmDesignator(hero, {"required_goals": ["reset", "handover"], "force_sensor_required": True,
+                                            "required_gripper_types": [arms.GripperTypes.GRASPING]})
 
     # sm = HandoverFromHumanFigure(hero, arm, grabbed_entity_label='trash')
     # sm.execute()

diff --git a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/take_out_the_garbage.py b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/take_out_the_garbage.py
@@ -6,6 +6,7 @@
 import robot_smach_states as states
 from robocup_knowledge import load_knowledge
 import robot_smach_states.util.designators as ds
+from robot_skills import arms
 from challenge_take_out_the_garbage.pick_up import PickUpTrash
 from challenge_take_out_the_garbage.drop_down import DropDownTrash, DropTrash
 CHALLENGE_KNOWLEDGE = load_knowledge('challenge_take_out_the_garbage')
@@ -42,7 +43,11 @@ def __init__(self, robot):
         # drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id)
         helper_waypoint_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint)
         end_waypoint_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint)
-        arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator")
+        arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator(
+            robot,
+            {"required_goals": ["handover", "reset", "handover_to_human"], "force_sensor_required": True,
+             "required_gripper_types": [arms.GripperTypes.GRASPING]},
+            name="empty_arm_designator")
 
         with self:
             smach.StateMachine.add("START_CHALLENGE_ROBUST",
@@ -122,8 +127,13 @@ def __init__(self, dummy_robot):
         dummy_trashbin_designator = ds.EdEntityDesignator(dummy_robot,
                                                     id=CHALLENGE_KNOWLEDGE.trashbin_id2,
                                                     name='trashbin_designator')
-        dummy_arm_designator_un = ds.UnoccupiedArmDesignator(dummy_robot, {})
-        dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {})
+        dummy_arm_designator_un = ds.UnoccupiedArmDesignator(
+            dummy_robot,
+            {"required_goals": ["handover", "reset", "handover_to_human"], "force_sensor_required": True,
+             "required_gripper_types": [arms.GripperTypes.GRASPING]})
+        dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {"required_goals": ["handover", "reset"],
+                                                                         "required_gripper_types": [
+                                                                             arms.GripperTypes.GRASPING]})
 
         with self:
             smach.StateMachine.add("PICK_UP_TRASH", PickUpTrash(dummy_robot, dummy_trashbin_designator,

diff --git a/robot_skills/examples/example_move_down_until_force_sensor_edge_up.py b/robot_skills/examples/example_move_down_until_force_sensor_edge_up.py
@@ -4,6 +4,6 @@
 
 rospy.init_node('test_mode_down_until_force_sensor_edge_up', anonymous=True)
 robot = robot_constructor("hero")
-arm = robot.get_arm()
+arm = robot.get_arm(force_sensor_required=True)
 
 arm.move_down_until_force_sensor_edge_up()
diff --git a/robot_skills/src/robot_skills/hero.py b/robot_skills/src/robot_skills/hero.py
@@ -101,12 +101,11 @@ def move_to_hmi_pose(self):
 
         :return: None
         """
-        arm = self.get_arm()
+        arm = self.get_arm(required_goals=['arm_out_of_way'])
 
         rotation = 1.57
         rotation_speed = 1
         rotation_duration = rotation / rotation_speed
-        if arm.has_joint_goal('arm_out_of_way'):
-            arm.send_joint_goal('arm_out_of_way', 0.0)
-            self.base.force_drive(0, 0, rotation_speed, rotation_duration)
+        arm.send_joint_goal('arm_out_of_way', 0.0)
+        self.base.force_drive(0, 0, rotation_speed, rotation_duration)
         arm.wait_for_motion_done()