Merge pull request #1061 from tue-robotics/update_robot_interface

Update robot interface

challenge_dishwasher/src/challenge_dishwasher/custom_place.py

@@ -57,7 +57,7 @@ def place_pose(ud):
 
         @cb_interface(outcomes=['done'])
         def open_gripper(ud):
-            arm.resolve().send_gripper_goal("open", timeout=0)
+            arm.resolve().gripper.send_goal("open", timeout=0)
             rospy.sleep(3.0)
             robot.speech.speak("This is where I usually place my cup")
             return 'done'

challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py

@@ -105,7 +105,7 @@ def __init__(self, robot, dishwasher_id):
 
         @cb_interface(outcomes=['done'])
         def _pre_grab_handle(ud):
-            robot.rightArm.send_gripper_goal("open", timeout=0)
+            robot.rightArm.gripper.send_goal("open", timeout=0)
             robot.rightArm._send_joint_trajectory([[0, 0.2519052373022729913, 0.7746500794619434, 1.3944848321343395,
                                                    -1.829999276180074, 0.6947045024700284, 0.1889253710114966]],
                                                  timeout=rospy.Duration(0))
@@ -118,7 +118,7 @@ def _grab_handle(ud):
             robot.speech.speak('I hope this goes right!', block=False)
             fs = frame_stamped("dishwasher", 0.42, 0, 0.8, roll=math.pi / 2, pitch=0, yaw=math.pi)
             robot.rightArm.send_goal(fs.projectToFrame(robot.robot_name + "/base_link", robot.tf_listener))
-            robot.rightArm.send_gripper_goal("close")
+            robot.rightArm.gripper.send_goal("close")
             return 'done'
 
         @cb_interface(outcomes=['done'])

challenge_dishwasher/src/challenge_dishwasher/simple_grab.py

@@ -86,13 +86,13 @@ def execute(self, ud):
             self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False)
             rospy.logerr('Grasp failed')
             arm.reset()
-            arm.send_gripper_goal('close', timeout=0.0)
+            arm.gripper.send_goal('close', timeout=0.0)
             return 'failed'
 
         # Close gripper
-        arm.send_gripper_goal('close')
+        arm.gripper.send_goal('close')
 
-        arm.occupied_by = None  # TODO: what to do if we don't have an entity
+        arm.gripper.occupied_by = None  # TODO: what to do if we don't have an entity
 
         # Lift
         rospy.loginfo('Start lifting')
@@ -142,7 +142,7 @@ def __init__(self, robot, arm):
         @cb_interface(outcomes=['succeeded'])
         def prepare_grasp(ud):
             # Open gripper (non-blocking)
-            arm.resolve().send_gripper_goal('open', timeout=0)
+            arm.resolve().gripper.send_goal('open', timeout=0)
 
             # Torso up (non-blocking)
             robot.torso.high()
@@ -151,7 +151,7 @@ def prepare_grasp(ud):
             arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0)
 
             # Open gripper
-            arm.resolve().send_gripper_goal('open', timeout=0.0)
+            arm.resolve().gripper.send_goal('open', timeout=0.0)
             return 'succeeded'
 
         with self:

challenge_final/src/challenge_final/navigate_to_and_grab_rack.py

@@ -35,7 +35,7 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string, max_torque=1.0):
-            arm.send_gripper_goal(open_close_string, max_torque=max_torque)
+            arm.gripper.send_goal(open_close_string, max_torque=max_torque)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         def show_image(package_name, path_to_image_in_package):

challenge_presentation/src/challenge_presentation/presentation.py

@@ -110,11 +110,11 @@ def execute(self, userdata=None):
             if self.right_arm is not None:
                 function_list.append(partial(self.right_arm.send_joint_trajectory, "show_gripper"))
             if self.left_arm is not None:
-                function_list.append(partial(self.left_arm.send_gripper_goal, "open"))
-                function_list.append(partial(self.left_arm.send_gripper_goal, "close"))
+                function_list.append(partial(self.left_arm.gripper.send_goal, "open"))
+                function_list.append(partial(self.left_arm.gripper.send_goal, "close"))
             if self.right_arm is not None:
-                function_list.append(partial(self.right_arm.send_gripper_goal, "open"))
-                function_list.append(partial(self.right_arm.send_gripper_goal, "close"))
+                function_list.append(partial(self.right_arm.gripper.send_goal, "open"))
+                function_list.append(partial(self.right_arm.gripper.send_goal, "close"))
             if self.left_arm is not None:
                 function_list.append(partial(self.left_arm.reset))
             if self.right_arm is not None:
@@ -165,9 +165,9 @@ def execute(self, userdata=None):
                 if self.right_arm is not None:
                     self.right_arm.reset()
                 if self.left_arm is not None:
-                    self.left_arm.send_gripper_goal("close")
+                    self.left_arm.gripper.send_goal("close")
                 if self.right_arm is not None:
-                    self.right_arm.send_gripper_goal("close")
+                    self.right_arm.gripper.send_goal("close")
                 self.robot.torso.reset()
                 self.robot.head.reset()
 

challenge_presentation/src/challenge_presentation/presentation_hero.py

@@ -120,8 +120,8 @@ def execute(self, userdata=None):
         function_list.append(partial(self.robot.speech.speak, self.trans.GRIPPER, language=self.language,
                                      voice=self.voice, block=False))
         function_list.append(partial(self.arm.send_joint_goal, "show_gripper"))
-        function_list.append(partial(self.arm.send_gripper_goal, "open"))
-        function_list.append(partial(self.arm.send_gripper_goal, "close"))
+        function_list.append(partial(self.arm.gripper.send_goal, "open"))
+        function_list.append(partial(self.arm.gripper.send_goal, "close"))
         function_list.append(partial(self.robot.speech.speak, self.trans.GRIPPER_CAMERA, language=self.language,
                                      voice=self.voice, block=True))
         function_list.append(partial(self.arm.reset))
@@ -179,7 +179,7 @@ def execute(self, userdata=None):
             if self.preempt_requested():
                 self.robot.speech.speak("Sorry, but I have to stop my introduction")
                 self.arm.reset()
-                self.arm.send_gripper_goal("close")
+                self.arm.gripper.send_goal("close")
                 self.robot.torso.reset()
                 self.robot.head.reset()
                 self.arm.wait_for_motion_done()

challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py

@@ -136,7 +136,7 @@ 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)
+        item = ds.EdEntityDesignator(robot=self._robot, id=arm.gripper.occupied_by.id)
         arm_designator = ds.OccupiedArmDesignator(self._robot,
                                                   arm_properties={
                                                       "required_trajectories": ["prepare_place"],

challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py

@@ -28,12 +28,12 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string):
-            arm.send_gripper_goal(open_close_string)
+            arm.gripper.send_goal(open_close_string)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         @cb_interface(outcomes=['done'])
         def _pre_grab_handle(_):
-            arm.send_gripper_goal("open", timeout=0)
+            arm.gripper.send_goal("open", timeout=0)
             send_joint_goal([0, -0, 0, -1.57, 1.57])
             return 'done'
 

challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py

@@ -28,12 +28,12 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string):
-            arm.send_gripper_goal(open_close_string)
+            arm.gripper.send_goal(open_close_string)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         @cb_interface(outcomes=['done'])
         def _pre_grab_handle(_):
-            arm.send_gripper_goal("open", timeout=0)
+            arm.gripper.send_goal("open", timeout=0)
             send_joint_goal([0, -0, 0, -1.57, 1.57])
             return 'done'
 

challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py

@@ -37,7 +37,7 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string, max_torque=0.1):
-            arm.send_gripper_goal(open_close_string, max_torque=max_torque)
+            arm.gripper.send_goal(open_close_string, max_torque=max_torque)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         def show_image(package_name, path_to_image_in_package):

challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py

@@ -91,7 +91,7 @@ def send_joint_trajectory(goal_array, wait_for_motion_done=True):
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string):
-            arm.send_gripper_goal(open_close_string)
+            arm.gripper.send_goal(open_close_string)
             rospy.sleep(1.0)  # Does not work with motion_done apparently
 
         @cb_interface(outcomes=['done'], input_keys=["item_picked"])

challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py

@@ -158,7 +158,7 @@ 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)
+        item = ds.EdEntityDesignator(robot=self._robot, id=arm.gripper.occupied_by.id)
         arm_designator = ds.OccupiedArmDesignator(self._robot, {"required_goals": ["reset", "handover_to_human"],
                                                                 "required_gripper_types": [arms.GripperTypes.GRASPING]})
         resolved_arm = arm_designator.resolve()

challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py

@@ -36,15 +36,15 @@ def execute(self, userdata=None):
         # Arm to position in a safe way
         arm.send_joint_goal('handover')
         arm.wait_for_motion_done()
-        arm.send_gripper_goal('open')
+        arm.gripper.send_goal('open')
         arm.wait_for_motion_done()
         arm._arm._send_joint_trajectory(
             [[0.4, -1.0, 0.0, -1.0, 0.0],[0.4, -1.0, 0.0, -1.57, 0.0], [0.4, -1.0, 0.0, -1.0, 0.0],
              [0.4, -1.0, 0.0, -1.57, 0.0]])
         arm.wait_for_motion_done()
         arm.send_joint_goal('reset')
         arm.wait_for_motion_done()
-        # arm.send_gripper_goal('close')
+        # arm.gripper.send_goal('close')
         # arm.wait_for_motion_done()
         return "succeeded"
 

challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py

@@ -132,16 +132,16 @@ def execute(self, userdata=None):
             # This opening and closing is to make sure that the gripper is empty and closed before measuring the forces
             # It is necessary to close the gripper since the gripper is also closed at the final measurement
 
-            # arm.send_gripper_goal('open')
+            # arm.gripper.send_goal('open')
             # arm.wait_for_motion_done()
-            # arm.send_gripper_goal('close', max_torque=1.0)
+            # arm.gripper.send_goal('close', max_torque=1.0)
             # arm.wait_for_motion_done()
 
             arm_weight = measure_force.get_force()
             rospy.loginfo("Empty weight %s", arm_weight)
 
             # Open gripper
-            arm.send_gripper_goal('open')
+            arm.gripper.send_goal('open')
             arm.wait_for_motion_done()
 
             # Go down and grab
@@ -154,7 +154,7 @@ def execute(self, userdata=None):
             # self._robot.torso.send_goal("grab_trash_down")
             # self._robot.torso.wait_for_motion_done()
             # rospy.sleep(3)
-            arm.send_gripper_goal('close', max_torque=1.0)
+            arm.gripper.send_goal('close', max_torque=1.0)
             arm.wait_for_motion_done()
 
             # Go up and back to pre grasp position
@@ -182,7 +182,7 @@ def execute(self, userdata=None):
 
         self._robot.speech.speak("Look at this I can pick up the trash!")
         handed_entity = EntityInfo(id="trash")
-        arm.occupied_by = handed_entity
+        arm.gripper.occupied_by = handed_entity
 
         return "succeeded"
 

robot_skills/package.xml

@@ -40,6 +40,7 @@
   <exec_depend>python-mock</exec_depend>
 
   <test_depend>test_tools</test_depend>
+  <test_depend>python-six</test_depend>
 
   <doc_depend>python-sphinx</doc_depend>
   <doc_depend>python-sphinx-autoapi-pip</doc_depend>