Merge pull request #1010 from tue-robotics/feature/grasping_object_high

Grasping object high

robot_skills/src/robot_skills/amigo.py

@@ -39,3 +39,15 @@ def __init__(self, dontInclude=None, wait_services=False):
         self.add_body_part('ed', world_model_ed.ED(self.robot_name, self.tf_listener))
 
         self.configure()
+
+    def move_to_pregrasp_pose(self, arm, grasp_target):
+        """
+        This poses the robot for an inspect.
+
+        :param arm: PublicArm with an available joint_trajectory 'prepare_grasp' to use for grasping the target
+        :param grasp_target: kdl.Frame with the pose of the entity to be grasped.
+        :return: boolean, false if something went wrong.
+        """
+
+        arm.send_joint_trajectory('prepare_grasp', timeout=0)
+        return True

robot_skills/src/robot_skills/base.py

@@ -128,7 +128,7 @@ def getPlan(self, position_constraint):
         end_time = rospy.Time.now()
         plan_time = (end_time-start_time).to_sec()
 
-        path_length = self.computePathLength(resp.plan)
+        self.path_length = self.computePathLength(resp.plan)
 
         return resp.plan
 
@@ -198,6 +198,18 @@ def move(self, position_constraint_string, frame):
 
         return plan
 
+    def turn_towards(self, x, y, frame, offset=0.0):
+        current_pose = self.get_location()
+        p = PositionConstraint()
+        p.constraint = "(x-%f)^2+(y-%f)^2 < %f^2" % (current_pose.frame.p.x(), current_pose.frame.p.y(), 0.1)
+        p.frame = current_pose.frame_id
+        plan = self.global_planner.getPlan(p)
+        o = OrientationConstraint(look_at=geometry_msgs.msg.Point(x, y, 0.0), angle_offset=offset)
+        o.frame = frame
+        self.local_planner.setPlan(plan, p, o)
+
+        return plan
+
     def force_drive(self, vx, vy, vth, timeout, loop_rate=10, stop=True, ax=float('inf'), ay=float('inf'), ath=float('inf')):
         """ Forces the robot to drive by sending a command velocity directly to the base controller. N.B.: all collision
         avoidance is bypassed.
@@ -277,6 +289,29 @@ def set_initial_pose(self, x, y, phi):
 
         return True
 
+    def wait_for_motion_done(self, timeout=10.0, cancel=False):
+        """
+        Waits until local planner is done
+
+        :param timeout: timeout in seconds; in case 0.0, no sensible output is provided, just False
+        :param cancel: bool specifying whether goals should be cancelled
+            if timeout is exceeded
+        :return: bool indicates whether motion was done (True if reached, False otherwise)
+        """
+        #TODO implement navigation using an actionserver
+        starttime = rospy.Time.now()
+        r = rospy.Rate(10)  # Hz
+        while not rospy.is_shutdown():
+            if self.local_planner.getStatus() == "arrived":
+                return True
+            passed_time = (rospy.Time.now() - starttime).to_sec()
+            if passed_time > timeout:
+                if cancel:
+                    self.local_planner.cancelCurrentPlan()
+                return False
+            r.sleep()
+        return False
+
     def reset(self):
         loc = self.local_planner.reset()
         glob = self.global_planner.reset()

robot_skills/src/robot_skills/hero.py

@@ -2,7 +2,7 @@
 from .simulation import is_sim_mode, SimEButton
 
 import rospy
-
+import math
 
 class Hero(robot.Robot):
     """docstring for Hero"""
@@ -55,6 +55,10 @@ def __init__(self, wait_services=False):
         # verify joint goal required for posing
         assert 'arm_out_of_way' in self.parts['leftArm'].default_configurations,\
             "arm_out_of_way joint goal is not defined in {}_describtion skills.yaml".format(self.robot_name)
+        # parameters for posing
+        self.z_over = 0.4  # height the robot should look over the surface
+        self.z_hh = 0.9  # height of the robots head at z_arm=0
+        self.torso_to_arm_ratio = 2.0  # motion of arm/motion of torso
 
         self.configure()
 
@@ -66,33 +70,26 @@ def move_to_inspect_pose(self, inspect_target):
         :return: boolean, false if something went wrong.
         """
         # calculate the arm_lift_link which must be sent
-        z_over = 0.4  # height the robot should look over the surface
-        z_hh = 0.9  # height of the robots head at z_arm=0
-        torso_to_arm_ratio = 2.0  # motion of arm/motion of torso
-        z_head = inspect_target.z() + z_over
+        z_head = inspect_target.z() + self.z_over
 
         # check whether moving the arm is necessary
-        if z_head < 1.1:
+        if z_head < 1.3:
             rospy.logdebug("Entity is low enough. we don't need to move the arm")
             return True
 
         # saturate the arm lift goal
-        z_arm = (z_head - z_hh) * torso_to_arm_ratio
+        z_arm = (z_head - self.z_hh) * self.torso_to_arm_ratio
         z_arm = min(0.69, max(z_arm, 0.0))  # arm_lift_joint limit
 
-        arm = self.parts['leftArm']
-
-        # turn the robot
-        rotation = 1.57
-        rotation_speed = 1.0
-        rotation_duration = rotation / rotation_speed
+        arm = self.get_arm(required_goals=['arm_out_of_way'])
 
-        pose = arm.default_configurations['arm_out_of_way']
+        pose = arm._arm.default_configurations['arm_out_of_way']
         pose[0] = z_arm
-        arm._send_joint_trajectory([pose])
+        arm._arm._send_joint_trajectory([pose])
 
-        self.base.force_drive(0, 0, rotation_speed, rotation_duration)
+        self.base.turn_towards(inspect_target.x(), inspect_target.y(), "/map", 1.57)
         arm.wait_for_motion_done()
+        self.base.wait_for_motion_done()
         return True
 
     def move_to_hmi_pose(self):
@@ -109,3 +106,37 @@ def move_to_hmi_pose(self):
         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()
+
+    def move_to_pregrasp_pose(self, arm, grasp_target):
+        """
+        This poses the robot for an inspect.
+
+        :param arm: PublicArm to use for grasping the target, must have joint trajectory 'prepare_grasp'
+        :param grasp_target: kdl.Frame with the pose of the entity to be grasp.
+        :return: boolean, false if something went wrong.
+        """
+        # calculate the arm_lift_link which must be sent
+        z_head = grasp_target.z() + self.z_over
+
+        if z_head < 1.3:
+            # we dont need to do stupid stuff
+            arm.send_joint_trajectory('prepare_grasp')
+            return True
+
+        # saturate the arm lift goal
+        z_arm = (z_head - self.z_hh) * self.torso_to_arm_ratio
+        z_arm = min(0.69, max(z_arm, 0.0))
+
+        pose = arm._arm.default_trajectories['prepare_grasp']
+        pose[1][0] = z_arm
+        arm._arm._send_joint_trajectory(pose)
+
+        angle_offset = -math.atan2(arm.base_offset.y(), arm.base_offset.x())
+        self.base.turn_towards(grasp_target.x(), grasp_target.y(), "/map", angle_offset)
+        arm.wait_for_motion_done()
+        self.base.wait_for_motion_done()
+        return True
+
+    def go_to_driving_pose(self):
+        arm = self.get_arm(required_goals=['carrying_pose'])
+        arm.send_joint_goal('carrying_pose')

robot_skills/src/robot_skills/mockbot.py

@@ -100,9 +100,11 @@ class Base(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):
         super(Base, self).__init__(robot_name, tf_listener)
         self.move = mock.MagicMock()
+        self.turn_towards = mock.MagicMock()
         self.force_drive = mock.MagicMock()
         self.get_location = lambda: FrameStamped(random_kdl_frame(), "/map")
         self.set_initial_pose = mock.MagicMock()
+        self.wait_for_motion_done = mock.MagicMock()
         self.go = mock.MagicMock()
         self.reset_costmap = mock.MagicMock()
         self.cancel_goal = mock.MagicMock()

robot_skills/src/robot_skills/robot.py

@@ -369,6 +369,17 @@ def move_to_inspect_pose(self, inspection_position):
         rospy.logdebug("move_to_inspect_pose() not implemented for {} object".format(self.robot_name))
         return True
 
+    def move_to_pregrasp_pose(self, arm, grasp_position):
+        """
+        This poses the robot for an inspect.
+
+        :param arm: PublicArm to use for grasping the target
+        :param grasp_position: kdl.Frame with the pose of the entity to be grasp.
+        :return: boolean, false if something went wrong.
+        """
+        rospy.logdebug("move_to_grasp_pose() not implemented for {} object".format(self.robot_name))
+        return True
+
     def move_to_hmi_pose(self):
         """
         This poses the robot for conversations.
@@ -378,6 +389,15 @@ def move_to_hmi_pose(self):
         rospy.logdebug("move_to_hmi_pose() not implemented for {} object".format(self.robot_name))
         pass
 
+    def go_to_driving_pose(self):
+        """
+        This poses the robot for driving.
+
+        :return: None
+        """
+        rospy.logdebug("go_to_driving_pose() not implemented for {} object".format(self.robot_name))
+        pass
+
     def __enter__(self):
         pass
 

robot_skills/src/robot_skills/sergio.py

@@ -39,3 +39,15 @@ def __init__(self, wait_services=False):
         self.add_body_part('ed', world_model_ed.ED(self.robot_name, self.tf_listener))
 
         self.configure()
+
+    def move_to_pregrasp_pose(self, arm, grasp_target):
+        """
+        This poses the robot for an inspect.
+
+        :param arm: PublicArm with an available joint_trajectory 'prepare_grasp' to use for grasping the target
+        :param grasp_target: kdl.Frame with the pose of the entity to be grasp.
+        :return: boolean, false if something went wrong.
+        """
+
+        arm.send_joint_trajectory('prepare_grasp', timeout=0)
+        return True

robot_smach_states/src/robot_smach_states/manipulation/grab.py

@@ -43,6 +43,7 @@ def execute(self, userdata=None):
             rospy.logerr("Could not resolve grab_entity")
             return "failed"
 
+        self.robot.move_to_inspect_pose(entity._pose.p)
         self.robot.head.look_at_point(VectorStamped(vector=entity._pose.p, frame_id="/map"), timeout=0.0)
         self.robot.head.wait_for_motion_done()
         segm_res = self.robot.ed.update_kinect("%s" % entity.id)
@@ -57,7 +58,7 @@ def execute(self, userdata=None):
         self.robot.torso.reset()
 
         # Arm to position in a safe way
-        arm.send_joint_trajectory('prepare_grasp', timeout=0)
+        self.robot.move_to_pregrasp_pose(arm, entity._pose.p)
         arm.wait_for_motion_done()
 
         # Open gripper

robot_smach_states/src/robot_smach_states/navigation/navigation.py

@@ -81,13 +81,14 @@ def execute(self, userdata=None):
 
 
 class executePlan(smach.State):
-    def __init__(self, robot, breakout_function, blocked_timeout = 4, reset_head=True):
+    def __init__(self, robot, breakout_function, blocked_timeout = 4, reset_head=True, reset_pose=True):
         smach.State.__init__(self,outcomes=['succeeded','arrived','blocked','preempted'])
         self.robot = robot
         self.t_last_free = None
         self.blocked_timeout = blocked_timeout
         self.breakout_function = breakout_function
         self.reset_head = reset_head
+        self.reset_pose = reset_pose
 
     def execute(self, userdata=None):
         """
@@ -103,6 +104,10 @@ def execute(self, userdata=None):
         if self.reset_head:
             self.robot.head.close()
 
+        # Move the robot to a suitable driving pose
+        if self.reset_pose and self.robot.base.global_planner.path_length > 0.5:
+            self.robot.go_to_driving_pose()
+
         while not rospy.is_shutdown():
             rospy.Rate(10).sleep() # 10hz
 
@@ -201,7 +206,7 @@ def execute(self, userdata=None):
 
 
 class NavigateTo(smach.StateMachine):
-    def __init__(self, robot, reset_head=True, speak=True, input_keys=[], output_keys=[]):
+    def __init__(self, robot, reset_head=True, speak=True, reset_pose=True, input_keys=[], output_keys=[]):
         smach.StateMachine.__init__(self, outcomes=['arrived', 'unreachable', 'goal_not_defined'],
                                     input_keys=input_keys, output_keys=output_keys)
         self.robot = robot
@@ -219,7 +224,8 @@ def __init__(self, robot, reset_head=True, speak=True, input_keys=[], output_key
                                  'goal_not_defined'                     :   'goal_not_defined',
                                  'goal_ok'                              :   'EXECUTE_PLAN'})
 
-                smach.StateMachine.add('EXECUTE_PLAN',                      executePlan(self.robot, self.breakOut, reset_head=reset_head),
+                smach.StateMachine.add('EXECUTE_PLAN',                      executePlan(self.robot, self.breakOut,
+                                                                                        reset_head=reset_head, reset_pose=reset_pose),
                     transitions={'succeeded'                            :   'arrived',
                                  'arrived'                              :   'GET_PLAN',
                                  'blocked'                              :   'PLAN_BLOCKED',

robot_smach_states/src/robot_smach_states/world_model.py

@@ -14,6 +14,7 @@
 from robot_skills.util.kdl_conversions import VectorStamped
 from .navigation import NavigateToObserve, NavigateToSymbolic
 from .util import designators as ds
+from .rise import RiseForInspect
 
 
 def _color_info(string):
@@ -260,6 +261,7 @@ def execute(self, userdata=None):
 class Inspect(smach.StateMachine):
     """
     Class to navigate to a(n) (furniture) object and segment the objects on top of it.
+    Note that when inspecting a high entity the robot will end the Inspect in a high position.
     """
     def __init__(self, robot, entityDes, objectIDsDes=None, searchArea="on_top_of", navigation_area="",
                  unknown_threshold=0.0, filter_threshold=0.0):
@@ -287,12 +289,16 @@ def __init__(self, robot, entityDes, objectIDsDes=None, searchArea="on_top_of",
                                                                                  entityDes),
                                        transitions={'unreachable': 'failed',
                                                     'goal_not_defined': 'failed',
-                                                    'arrived': 'SEGMENT'})
+                                                    'arrived': 'RISE'})
             else:
                 smach.StateMachine.add('NAVIGATE_TO_INSPECT', NavigateToObserve(robot, entityDes, radius=1.0),
                                        transitions={'unreachable': 'failed',
                                                     'goal_not_defined': 'failed',
-                                                    'arrived': 'SEGMENT'})
+                                                    'arrived': 'RISE'})
+
+            smach.StateMachine.add('RISE', RiseForInspect(robot, entityDes, searchArea),
+                                   transitions={'succeeded': 'SEGMENT',
+                                                'failed': 'SEGMENT'})
 
             smach.StateMachine.add('SEGMENT',
                                    SegmentObjects(robot, objectIDsDes.writeable, entityDes, searchArea,