Merge pull request #1028 from tue-robotics/feature/navigation_refactor

separate constraint functions from states

robot_skills/src/robot_skills/mockbot.py

@@ -95,6 +95,10 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side):
     def collect_gripper_types(self, gripper_type):
         return gripper_type
 
+    @property
+    def base_offset(self):
+        return self._base_offset
+
 
 class Base(MockedRobotPart):
     def __init__(self, robot_name, tf_listener, *args, **kwargs):

robot_smach_states/examples/human_interaction/example_give_directions.py

@@ -21,6 +21,6 @@
     e_id = args.target_entity_id
 
     # Instantiate GuideToSymbolic machine
-    state = give_directions.GiveDirections(r, ds.EntityByIdDesignator(r, id=e_id))
+    state = give_directions.GiveDirections(robot, ds.EntityByIdDesignator(robot, id=e_id))
     # Execute the state
     state.execute()

robot_smach_states/examples/navigation/constraint_functions/example_arms_reach_constraints.py

@@ -0,0 +1,53 @@
+import rospy
+import argparse
+
+from robot_skills.get_robot import get_robot
+from robot_skills.util.entity import Entity
+from robot_skills.util.kdl_conversions import frame_stamped, FrameStamped
+# Robot Smach States
+from robot_smach_states.navigation.constraint_functions.arms_reach_constraints import arms_reach_constraint
+from robot_smach_states.util.designators.arm import ArmDesignator
+from robot_smach_states.util.designators.core import Designator
+from robot_smach_states.util.designators.utility import AttrDesignator
+
+if __name__ == "__main__":
+    """
+    Example demonstrating how to use the arms_reach_constraint function to generate navigation constraints
+    Make sure you have a (robot)-start running since a robot object is created for this.
+    """
+
+    parser = argparse.ArgumentParser(description="Example arms_reach_constaint() function")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node("example_arms_reach_constraints")
+    robot = get_robot(args.robot)
+
+    # get an armdesignator and a pose designator
+    arm_designator = ArmDesignator(robot, {}, name="arm_designator")
+    pose = frame_stamped("/map", 0, 0, 1.3)
+    pose_designator = Designator(pose, name="frame designator")
+
+    # basic functionality
+    rospy.loginfo("Example of the basic funtionality")
+    rospy.loginfo("Running: pc, oc = arms_reach_constraint(pose_designator, arm_designator)")
+    pc, oc = arms_reach_constraint(pose_designator, arm_designator)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    # dont look at the frame
+    rospy.loginfo("Example where we don't require the robot to look at the pose")
+    rospy.loginfo("Running: pc, oc = arms_reach_constraint(pose_designator, arm_designator, look=False)")
+    pc, oc = arms_reach_constraint(pose_designator, arm_designator, look=False)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    # Use an entity
+    entity = Entity("dummy_entity", "dummy_type", "/map", pose.frame, None, None, None, None)
+    entity_designator = Designator(entity)
+
+    rospy.loginfo("When you have an entity rather than a pose, consider wrapping it in an attribute designator")
+    rospy.loginfo("Running: pc, oc = arms_reach_constraint(AttrDesignator(entity_designator, 'pose', resolve_type=FrameStamped), arm_designator)")
+    pc, oc = arms_reach_constraint(AttrDesignator(entity_designator, 'pose', resolve_type=FrameStamped), arm_designator)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))

robot_smach_states/examples/navigation/constraint_functions/example_compound_constraints.py

@@ -0,0 +1,34 @@
+import rospy
+import argparse
+
+# Robot Smach States
+from robot_smach_states.navigation.constraint_functions.compound_constraints import combine_constraints
+from robot_smach_states.navigation.constraint_functions.pose_constraints import pose_constraints
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="Example combine_constraints function")
+    args = parser.parse_args()
+    rospy.init_node("example_combine_constraints")
+
+    rospy.loginfo("Example combine constraints function")
+
+    # function 1
+    rospy.loginfo("First constraint function resolves to:")
+    fun1 = lambda: pose_constraints(0, 3.0, radius=3.0)
+    pc, oc = fun1()
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    # function 2
+    rospy.loginfo("Second constraint function resolves to:")
+    fun2 = lambda: pose_constraints(0, 0.0, 1.57, radius=1.0)
+    pc, oc = fun2()
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    # combined constraints
+    rospy.loginfo("The combined constraints become:")
+    pc, oc = combine_constraints([fun1, fun2])
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))

robot_smach_states/examples/navigation/constraint_functions/example_look_at_constraints.py

@@ -0,0 +1,34 @@
+import rospy
+import argparse
+
+from robot_skills.util.entity import Entity
+import PyKDL as kdl
+# Robot Smach States
+from robot_smach_states.navigation.constraint_functions.look_at_constraints import look_at_constraint
+from robot_smach_states.util.designators.core import Designator
+
+if __name__ == "__main__":
+    """
+    Example demonstrating how to use the look_at_constraints function
+    """
+
+    parser = argparse.ArgumentParser(description="Example look_at_constaints function")
+    args = parser.parse_args()
+
+    rospy.init_node("example_look_at_constaints")
+
+    # get an entitydesignator
+    pose = kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(1, 2, 1.3))
+    e = Entity("dummy", "dummy_type", "/map", pose, None, None, None, None)
+    entity = Designator(e, name="entity designator")
+
+    # example base function
+    rospy.loginfo("look at an entity")
+    pc, oc = look_at_constraint(entity)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    rospy.loginfo("orient yourself w.r.t an entity")
+    pc, oc = look_at_constraint(entity, offset=1.57)  # keep the object 90 degrees to your left
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))

robot_smach_states/examples/navigation/constraint_functions/example_pose_constraints.py

@@ -0,0 +1,34 @@
+import rospy
+import argparse
+
+# Robot Smach States
+from robot_smach_states.navigation.constraint_functions.pose_constraints import pose_constraints
+
+if __name__ == "__main__":
+    """
+    Example demonstrating how to use the pose_constraints() function
+    """
+
+    parser = argparse.ArgumentParser(description="Example pose_constaints() function")
+    args = parser.parse_args()
+
+    rospy.init_node("example_pose_constaints")
+
+    # base function
+    rospy.loginfo("Pose constraint function with only x and y resolves to:")
+    pc, oc = pose_constraints(x=1.0, y=2.0)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    # with rotation
+    rospy.loginfo("Pose constraint function with x, y and rz resolves to:")
+    pc, oc = pose_constraints(x=1.0, y=2.0, rz=1.57)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    # extra inputs
+    rospy.loginfo("You may also specify the radius and the frame_id:")
+    pc, oc = pose_constraints(x=1.0, y=2.0, rz=1.57, radius=3.0, frame_id="/example_frame")
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+

robot_smach_states/examples/navigation/constraint_functions/example_radius_constraints.py

@@ -0,0 +1,30 @@
+import rospy
+import argparse
+
+from robot_skills.get_robot import get_robot
+# Robot Smach States
+from robot_smach_states.navigation.constraint_functions.radius_constraints import radius_constraint
+from robot_smach_states.util.designators.ed_designators import EntityByIdDesignator
+
+if __name__ == "__main__":
+    """
+    Example demonstrating how to use the radius_constraint() function
+    Make sure you have both (robot)-start and ED running.
+    """
+
+    parser = argparse.ArgumentParser(description="Example radius_constaints function()")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node("example_radius_constaints")
+    robot = get_robot(args.robot)
+
+    # get an entitydesignator
+    entity = EntityByIdDesignator(robot, 'dinner_table', name='entity_designator')
+
+    # base function
+    rospy.loginfo("Stay 0.5 meter away from the dinner table with a margin of 10cm")
+    rospy.loginfo("radius_constraint(entity, radius=0.5, margin=0.1)")
+    pc, oc = radius_constraint(entity, radius=0.5, margin=0.1)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))

robot_smach_states/examples/navigation/constraint_functions/example_symbolic_constraints.py

@@ -0,0 +1,38 @@
+import rospy
+import argparse
+
+from robot_skills.get_robot import get_robot
+# Robot Smach States
+from robot_smach_states.navigation.constraint_functions.symbolic_constraints import symbolic_constraint, room_constraint
+from robot_smach_states.util.designators.ed_designators import EntityByIdDesignator
+
+if __name__ == "__main__":
+    """
+    Example demonstrating how to use the symbolic_constraint() function
+    Make sure you have both (robot)-start and ED running.
+    """
+
+    parser = argparse.ArgumentParser(description="Example symbolic_constaint() function")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node("example_symbolic_constraints")
+    robot = get_robot(args.robot)
+
+    # get an entitydesignator
+    entity = EntityByIdDesignator(robot, 'dinner_table', name='entity_designator')
+
+    # in front of the dinner table
+    rospy.loginfo("symbolic constraint corresponding to 'in_front_of' the 'dinner_table':")
+    pc, oc = symbolic_constraint(robot, {entity: "in_front_of"})
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))
+
+    # get a roomdesignator
+    room = EntityByIdDesignator(robot, 'kitchen', name='room_designator')
+
+    # room constraint
+    rospy.loginfo("room constraint corresponding to 'kitchen':")
+    pc, oc = room_constraint(robot, room)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))

robot_smach_states/examples/navigation/constraint_functions/example_waypoint_constraints.py

@@ -0,0 +1,29 @@
+import rospy
+import argparse
+
+from robot_skills.util.entity import Entity
+import PyKDL as kdl
+# Robot Smach States
+from robot_smach_states.navigation.constraint_functions.waypoint_constraints import waypoint_constraint
+from robot_smach_states.util.designators.core import Designator
+
+if __name__ == "__main__":
+    """
+    Example demonstrating how to use the waypoint_constraint() function
+    """
+
+    parser = argparse.ArgumentParser(description="Example waypoint_constraint() function")
+    args = parser.parse_args()
+
+    rospy.init_node("example_waypoint_constaints")
+
+    # get an entitydesignator
+    pose = kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(1, 2, 0))
+    e = Entity("dummy_waypoint", "dummy_type", "/map", pose, None, None, None, None)
+    waypoint= Designator(e, name="entity designator")
+
+    # waypoint example
+    rospy.loginfo("waypoint_constraint resolves to:")
+    pc, oc = waypoint_constraint(waypoint, radius=1.0)
+    rospy.loginfo("pc becomes : {}".format(pc))
+    rospy.loginfo("oc becomes : {}".format(oc))

robot_smach_states/src/robot_smach_states/human_interaction/give_directions.py

@@ -15,7 +15,7 @@
 from robot_skills.util.kdl_conversions import point_msg_to_kdl_vector, VectorStamped
 
 # Robot Smach States
-from ..navigation.navigate_to_symbolic import NavigateToSymbolic
+from ..navigation.constraint_functions.symbolic_constraints import symbolic_constraint
 from ..util.designators.ed_designators import Designator
 
 
@@ -61,10 +61,9 @@ def execute(self, userdata=None):
         else:
             possible_areas = ["in_front_of", "near"]
         for area in possible_areas:
-            nav_constraints[area] = NavigateToSymbolic.generate_constraint(
+            nav_constraints[area] = symbolic_constraint(
                 robot=self._robot,
-                entity_designator_area_name_map={self._entity_designator: area},
-                entity_lookat_designator=self._entity_designator)
+                entity_designator_area_name_map={self._entity_designator: area})
 
         if not nav_constraints:
             rospy.logerr("Cannot give directions if I don't know where to go")

robot_smach_states/src/robot_smach_states/navigation/constraint_functions/__init__.py

@@ -0,0 +1,7 @@
+from arms_reach_constraints import arms_reach_constraint
+from compound_constraints import combine_constraints
+from look_at_constraints import look_at_constraint
+from pose_constraints import pose_constraints
+from radius_constraints import radius_constraint
+from symbolic_constraints import symbolic_constraint, room_constraint
+from waypoint_constraints import waypoint_constraint

robot_smach_states/src/robot_smach_states/navigation/constraint_functions/arms_reach_constraints.py

@@ -0,0 +1,53 @@
+from __future__ import absolute_import
+
+# System
+import math
+
+# ROS
+import rospy
+from geometry_msgs.msg import Point
+
+# TU/e Robotics
+from cb_planner_msgs_srvs.msg import OrientationConstraint, PositionConstraint
+
+
+def arms_reach_constraint(pose_designator, arm_designator, look=True):
+    """
+    Position so that the arm can reach the position/entity
+
+    :param pose_designator: designator that resolves to a FrameStamped of the point to be reached
+    :param arm_designator: PublicArmDesignator, arm to use for manipulation
+    :param look: bool, whether or not the orientation must be constrained as well
+    :return: navigation constraints, if a designator does not resolve None is returned
+    :rtype: tuple(PositionConstraint, OrientationConstraint)
+    """
+    arm = arm_designator.resolve()
+    if not arm:
+        rospy.logerr("Could not resolve arm, Designator {} did not resolve".format(arm_designator))
+        return None
+
+    radius = math.hypot(arm.base_offset.x(), arm.base_offset.y())
+
+    pose = pose_designator.resolve()
+    if not pose:
+        rospy.logerr("No such place_pose, Designator {} did not resolve".format(pose_designator))
+        return None
+
+    try:
+        x = pose.frame.p.x()
+        y = pose.frame.p.y()
+    except KeyError as ke:
+        rospy.logerr("Could not determine pose: ".format(ke))
+        return None
+
+    # Outer radius
+    ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075)
+    ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075)
+    pc = PositionConstraint(constraint=ri + " and " + ro, frame="/map")
+
+    oc = None
+    if look:
+        angle_offset = -math.atan2(arm.base_offset.y(), arm.base_offset.x())
+        oc = OrientationConstraint(look_at=Point(x, y, 0.0), frame="/map", angle_offset=angle_offset)
+
+    return pc, oc