Merge pull request #857 from tue-robotics/feature/rise_for_inspect

Feature/rise for inspect

robot_skills/src/robot_skills/hero.py

@@ -48,8 +48,65 @@ def __init__(self, wait_services=False):
         for arm in self.arms.itervalues():
             arm._operational = True
 
+        # 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)
+
         self.configure()
 
+    def move_to_inspect_pose(self, inspect_target):
+        """
+        This poses the robot for an inspect.
+
+        :param inspection_position: kdl.Frame with the pose of the entity to be inspected.
+        :return result: boolean, false if something went wrong.
+        """
+        # calculate the arm_lift_link which must be sent
+        z_over = 0.3  # 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
+
+        # check whether moving the arm is necessary
+        if z_head < 1.2:
+            rospy.logdebug('Entity is low enough. we dont need to move the arm')
+            return True
+
+        # saturate the arm lift goal
+        z_arm = (z_head - z_hh) * 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
+
+        pose = arm.default_configurations['arm_out_of_way']
+        pose[0] = z_arm
+        arm._send_joint_trajectory([pose])
+
+        self.base.force_drive(0, 0, rotation_speed, rotation_duration)
+        arm.wait_for_motion_done()
+        return True
+
+    def move_to_hmi_pose(self):
+        """
+        This poses the robot for conversations.
+
+        :return None
+        """
+        arm = self.get_arm()
+
+        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.wait_for_motion_done()
+
 
 if __name__ == "__main__":
     rospy.init_node("hero")

robot_skills/src/robot_skills/robot.py

@@ -314,6 +314,25 @@ def handle_hardware_status(self, diagnostic_array):
             if name not in self._ignored_parts:
                 part.process_hardware_status(diagnostic_dict)
 
+    def move_to_inspect_pose(self, inspection_position):
+        """
+        This poses the robot for an inspect.
+
+        :param inspection_position: kdl.Frame with the pose of the entity to be inspected.
+        :return result: boolean, false if something went wrong.
+        """
+        rospy.logdebug("move_to_inspect_pose() not implemented for {} object".format(self.robot_name))
+        return True
+
+    def move_to_hmi_pose(self):
+        """
+        This poses the robot for conversations.
+
+        :return None
+        """
+        rospy.logdebug("move_to_hmi_pose() not implemented for {} object".format(self.robot_name))
+        pass
+
     def __enter__(self):
         pass
 

robot_smach_states/src/robot_smach_states/rise.py

@@ -2,69 +2,67 @@
 # ROS
 
 import smach
+import rospy
+
+import PyKDL as kdl
 
 from robot_skills.util.kdl_conversions import VectorStamped
 
 
-class HeroHmiPose(smach.State):
+class RiseForHMI(smach.State):
     """
-    State to ensure that hero's arm is out of the way so that the robot
-    may move his torso up.
+    State to pose the robot for conversations
     :param robot: Robot to execute state with
     """
-    def __init__(self, robot, rotation=1.57, rotational_speed=1):
+    def __init__(self, robot):
         smach.State.__init__(self, outcomes=['succeeded', 'failed'])
         self._robot = robot
-        self._rotation = rotation
-        self._rot_speed = rotational_speed
 
     def execute(self, _):
-        arm = self._robot.get_arm()
-
-        # Get position to look at after turning. Transform the position to map frame since turning will move base_link
+        # Get position to look at. Transform the position to map frame since taking the hmi pose may move base link
         goal = VectorStamped(1.0, 0.0, 1.6, frame_id="/" + self._robot.robot_name + "/base_link")
         tf_goal = goal.projectToFrame('/map', self._robot.tf_listener)
 
-        rotation_duration = self._rotation / self._rot_speed
-        if arm.has_joint_goal('arm_out_of_way'):
-            arm.send_joint_goal('arm_out_of_way', 0.0)
-            self._robot.base.force_drive(0, 0, self._rot_speed, rotation_duration)
+        self._robot.move_to_hmi_pose()
+
         self._robot.head.look_at_point(tf_goal)
-        arm.wait_for_motion_done()
         self._robot.head.wait_for_motion_done()
         return "succeeded"
 
-class HmiPose(smach.State):
+
+class RiseForInspect(smach.State):
     """
-    State to puts the robot in a position for hmi
+    State to ensure that the robot is in a proper position to inspect. This means its head is at the correct height
+    and his vision is unobstructed. (no arm in front of its face)
     :param robot: Robot to execute state with
+    :param entity: entity which is to be inspected
+    :param volume: volume of the entity which is to be inspected
     """
-    def __init__(self, robot):
+    def __init__(self, robot, entity, volume=None):
         smach.State.__init__(self, outcomes=['succeeded', 'failed'])
         self._robot = robot
+        self._entity = entity
+        self._volume = volume
 
     def execute(self, _):
-        self._robot.head.look_at_standing_person()
-        self._robot.head.wait_for_motion_done()
-        return "succeeded"
-
 
-class RiseForHMI(smach.StateMachine):
-    """
-    Get the robot in a nice pose for human machine interaction. This is more complicated for HERO compared to
-    AMIGO and SERGIO.
-    :param robot: Robot to execute state with
-    """
+        # Determine the height of the head target
+        # Start with a default
+        entity = self._entity.resolve()
+        volume = self._volume
 
-    def __init__(self, robot):
-        smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
+        # Check if we have areas: use these
+        if volume in entity.volumes:
+            search_volume = entity.volumes[volume]
+            x_obj = 0.5 * (search_volume.min_corner.x() + search_volume.max_corner.x())
+            y_obj = 0.5 * (search_volume.min_corner.y() + search_volume.max_corner.y())
+            z_obj = search_volume.min_corner.z()
+            pos = entity.pose.frame * kdl.Vector(x_obj, y_obj, z_obj)
+        else:
+            # Look at the top of the entity to inspect
+            pos = entity.pose.frame.p
 
-        with self:
-            if robot.robot_name == 'hero':
-                smach.StateMachine.add('HERO_HMI_POSE', HeroHmiPose(robot),
-                                       transitions={'succeeded': 'done',
-                                                    'failed': 'failed'})
-            else:
-                smach.StateMachine.add('HMI_POSE', HmiPose(robot),
-                                       transitions={'succeeded': 'done',
-                                                    'failed': 'failed'})
+        if self._robot.move_to_inspect_pose(pos):
+            return "succeeded"
+        else:
+            return "failed"

robot_smach_states/src/robot_smach_states/world_model.py

@@ -13,6 +13,7 @@
 from robot_skills.util.entity import Entity
 from robot_skills.util.kdl_conversions import VectorStamped
 from robot_smach_states.navigation import NavigateToObserve, NavigateToSymbolic
+from robot_smach_states.rise import RiseForInspect
 import robot_smach_states.util.designators as ds
 
 
@@ -203,12 +204,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,
                                                              threshold=threshold),