Use cartesian goals in serve breakfast (#1328)

Author: MatthijsBurgh

challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py

@@ -8,12 +8,10 @@
 import rospkg
 import rospy
 
-from challenge_serve_breakfast.tuning import REQUIRED_ITEMS, JOINTS_HANDOVER, PICK_ROTATION
+from challenge_serve_breakfast.tuning import REQUIRED_ITEMS, JOINTS_HANDOVER
 from robot_skills import get_robot
-from robot_skills.arm.arms import GripperTypes
 # ROS
 from pykdl_ros import VectorStamped
-from robot_smach_states.human_interaction import Say
 from robot_smach_states.navigation import NavigateToSymbolic
 from robot_smach_states.util.designators import EdEntityDesignator
 from smach import StateMachine, cb_interface, CBState

challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py

@@ -11,19 +11,15 @@
 import rospy
 import visualization_msgs.msg
 from geometry_msgs.msg import PoseStamped, Vector3
+from pykdl_ros import FrameStamped
 
 from challenge_serve_breakfast.tuning import (
-    JOINTS_PRE_PRE_PLACE,
-    JOINTS_PRE_PLACE_HORIZONTAL,
-    JOINTS_PRE_PLACE_VERTICAL,
+    get_item_place_pose,
     ITEM_VECTOR_DICT,
     item_vector_to_item_frame,
     item_frame_to_pose,
-    JOINTS_PLACE_HORIZONTAL,
-    JOINTS_PLACE_HORIZONTAL_MILK,
-    JOINTS_PLACE_VERTICAL,
-    JOINTS_RETRACT,
-    COLOR_DICT, REQUIRED_ITEMS,
+    COLOR_DICT,
+    REQUIRED_ITEMS,
 )
 from robot_skills import get_robot
 from robot_smach_states.human_interaction import Say
@@ -35,22 +31,22 @@
 
 
 class PlaceItemOnTable(StateMachine):
-    def __init__(self, robot, table_id):
+    def __init__(self, robot, table_id, retract = True):
         StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=["item_picked"])
         # noinspection PyProtectedMember
         arm = robot.get_arm()._arm
+        self.retract = retract
 
-        def send_joint_goal(position_array, wait_for_motion_done=True):
-            # noinspection PyProtectedMember
-            arm._send_joint_trajectory([position_array], timeout=0.0)
+        def send_goal(pose_goal, wait_for_motion_done=True):
+            arm.send_goal(pose_goal, timeout=0.0)
             if wait_for_motion_done:
                 arm.wait_for_motion_done()
 
         def send_gripper_goal(open_close_string, wait_for_motion_done=True):
             arm.gripper.send_goal(open_close_string)
 
         @cb_interface(outcomes=["done"], input_keys=["item_picked"])
-        def _pre_place(user_data):
+        def _place(user_data):
             item_name = user_data["item_picked"]
             rospy.loginfo(f"Preplacing {item_name}...")
 
@@ -59,55 +55,52 @@ def _pre_place(user_data):
             robot.head.look_up()
             robot.head.wait_for_motion_done()
 
-            send_joint_goal(JOINTS_PRE_PRE_PLACE)
+            item_place_pose = get_item_place_pose(user_data["item_picked"])
+            place_fs = FrameStamped(item_place_pose, rospy.Time(0), table_id)
 
-            if item_name in ["milk_carton", "cereal_box"]:
-                send_joint_goal(JOINTS_PRE_PLACE_HORIZONTAL)
-            else:
-                send_joint_goal(JOINTS_PRE_PLACE_VERTICAL)
+            pre_place_fs = copy.deepcopy(place_fs)
+            pre_place_fs.frame.p.z(place_fs.frame.p.z() + 0.1)
 
-            return "done"
-
-        @cb_interface(outcomes=["done"], input_keys=["item_picked"])
-        def _align_with_table(user_data):
-            item_placement_vector = ITEM_VECTOR_DICT[user_data["item_picked"]]
-            item_frame = item_vector_to_item_frame(item_placement_vector)
+            post_place_fs = copy.deepcopy(place_fs)
+            post_place_fs.frame.p.z(place_fs.frame.p.z() + 0.2)
 
-            goal_pose = item_frame_to_pose(item_frame, table_id)
-            rospy.loginfo("Placing {} at {}".format(user_data["item_picked"], goal_pose))
+            rospy.loginfo("Pre Placing...")
+            send_goal(pre_place_fs)
             robot.head.look_down()
-            ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({})
-            return "done"
+            robot.head.look_up()
+            robot.head.wait_for_motion_done()
 
-        @cb_interface(outcomes=["done"], input_keys=["item_picked"])
-        def _place_and_retract(user_data):
             rospy.loginfo("Placing...")
-            item_name = user_data["item_picked"]
-            if item_name in ["milk_carton", "cereal_box"]:
-                send_joint_goal(JOINTS_PLACE_HORIZONTAL_MILK)
-            else:
-                send_joint_goal(JOINTS_PLACE_VERTICAL)
-
-            rospy.loginfo("Dropping...")
+            send_goal(place_fs)
             send_gripper_goal("open")
-            robot.head.look_up()
-            robot.head.wait_for_motion_done()
 
-            if item_name != "cereal_box":
-                rospy.loginfo("Retract...")
-                send_joint_goal(JOINTS_RETRACT, wait_for_motion_done=False)
+            if item_name != "cereal_box" or self.retract:
+                rospy.loginfo("Retracting...")
+                send_goal(post_place_fs)
                 robot.base.force_drive(-0.1, 0, 0, 3)  # Drive backwards at 0.1m/s for 3s, so 30cm
                 send_gripper_goal("close", wait_for_motion_done=False)
                 arm.send_joint_goal("carrying_pose")
 
+            #if item_name in ["milk_carton", "cereal_box"]:
+            #    send_joint_goal(JOINTS_PRE_PLACE_HORIZONTAL)
+            #else:
+            #    send_joint_goal(JOINTS_PRE_PLACE_VERTICAL)
+
+            #rospy.loginfo("Placing...")
+            #item_name = user_data["item_picked"]
+            #if item_name in ["milk_carton"]:
+            #    send_joint_goal(JOINTS_PLACE_HORIZONTAL_MILK)
+            #elif item_name in ["milk_carton", "cereal_box"]:
+            #    send_joint_goal(JOINTS_PLACE_HORIZONTAL)
+            #else:
+            #    send_joint_goal(JOINTS_PLACE_VERTICAL)
+
             robot.head.reset()
 
             return "done"
 
         with self:
-            self.add_auto("PRE_PLACE", CBState(_pre_place), ["done"])
-            self.add_auto("ALIGN_WITH_TABLE", CBState(_align_with_table), ["done"])
-            self.add("PLACE_AND_RETRACT", CBState(_place_and_retract), transitions={"done": "succeeded"})
+            self.add("PLACE", CBState(_place), {"done": "succeeded"})
 
 
 class NavigateToAndPlaceItemOnTable(StateMachine):
@@ -157,7 +150,7 @@ def __init__(self, robot, table_id, table_close_navigation_area):
 
             StateMachine.add(
                 "PLACE_ITEM_ON_TABLE",
-                PlaceItemOnTable(robot, table_id),
+                PlaceItemOnTable(robot, table_id, retract=False),
                 transitions={"succeeded": "succeeded", "failed": "failed"},
             )
 
@@ -182,7 +175,6 @@ def _publish_item_poses(user_data, marker_array_pub, items):
         marker_msg.type = visualization_msgs.msg.Marker.SPHERE
         marker_msg.action = 0
         marker_msg.pose = posestamped.pose
-        marker_msg.pose.position.z = 1.0
         marker_msg.scale = Vector3(0.05, 0.05, 0.05)
         marker_msg.color = COLOR_DICT[k]
         array_msg.markers.append(marker_msg)

challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py

@@ -6,14 +6,15 @@
 
 import os
 
-import PyKDL
 import rospy
+from pykdl_ros import FrameStamped
 
-from challenge_serve_breakfast.tuning import JOINTS_POST_PICK, ITEM_VECTOR_DICT, item_vector_to_item_frame, \
-    item_frame_to_pose, POUR_OFFSET_Y, JOINTS_PRE_POUR, JOINTS_POUR, JOINTS_PLACE_HORIZONTAL, JOINTS_RETRACT, \
-    POUR_OFFSET_X
+from challenge_serve_breakfast.tuning import (
+    get_item_pour_poses,
+    JOINTS_POST_PICK,
+)
+from challenge_serve_breakfast.navigate_to_and_place_item_on_table import PlaceItemOnTable
 from robot_skills import get_robot
-from robot_smach_states.navigation.control_to_pose import ControlToPose, ControlParameters
 from smach import StateMachine, cb_interface, CBState
 
 
@@ -32,71 +33,52 @@ def send_joint_goal(position_array, wait_for_motion_done=True):
         def send_gripper_goal(open_close_string, max_torque=0.1, wait_for_motion_done=True):
             arm.gripper.send_goal(open_close_string, max_torque=max_torque)
 
+        def send_goal(pose_goal, wait_for_motion_done=True):
+            arm.send_goal(pose_goal, timeout=0.0)
+            if wait_for_motion_done:
+                arm.wait_for_motion_done()
+
         @cb_interface(outcomes=["done"])
         def _pick(_):
             robot.speech.speak("Lets grab some cereal", block=False)
             send_gripper_goal("close")
             send_joint_goal(JOINTS_POST_PICK)
             return "done"
 
-        @cb_interface(outcomes=["done"])
-        def _align_pour(_):
-            item_placement_vector = ITEM_VECTOR_DICT["cereal_box"] + PyKDL.Vector(POUR_OFFSET_X, POUR_OFFSET_Y, 0)
-            item_frame = item_vector_to_item_frame(item_placement_vector)
-
-            goal_pose = item_frame_to_pose(item_frame, table_id)
-            rospy.loginfo("Moving to pouring pose at {}".format(goal_pose))
-            robot.head.look_down()
-            ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({})
-            return "done"
 
         @cb_interface(outcomes=["done"])
         def _pour(_):
-            robot.speech.speak("Hope this goes well", block=False)
-            send_joint_goal(JOINTS_PRE_POUR)
-            rospy.sleep(0.5)
-            send_joint_goal(JOINTS_POUR)
-            send_joint_goal(JOINTS_PRE_POUR)
-            send_joint_goal(JOINTS_POST_PICK)
-            return "done"
+            pour_target = "bowl"
+            item_name = "cereal"
+            rospy.loginfo(f"pouring in {pour_target}...")
 
-        @cb_interface(outcomes=["done"])
-        def _align_place(_):
-            robot.speech.speak("Awesome", block=False)
-            item_placement_vector = ITEM_VECTOR_DICT["cereal_box"]
-            item_frame = item_vector_to_item_frame(item_placement_vector)
-
-            goal_pose = item_frame_to_pose(item_frame, table_id)
-            rospy.loginfo("Moving to place pose at {}".format(goal_pose))
-            robot.head.look_down()
-            ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({})
-            return "done"
+            robot.speech.speak(f"I am going to pour the {item_name}", block=False)
 
-        @cb_interface(outcomes=["done"])
-        def _place(_):
-            robot.speech.speak("Putting back the cereal", block=False)
-            rospy.loginfo("Placing...")
-            send_joint_goal(JOINTS_PLACE_HORIZONTAL)
-            send_gripper_goal("open")
             robot.head.look_up()
             robot.head.wait_for_motion_done()
 
-            rospy.loginfo("Retract...")
-            send_joint_goal(JOINTS_RETRACT, wait_for_motion_done=False)
+            item_pour_poses = get_item_pour_poses(pour_target)
+            for pose in item_pour_poses:
+                goal_fs = FrameStamped(pose, rospy.Time(0), table_id)
+                rospy.loginfo("Pouring...")
+                send_goal(goal_fs)
+            robot.head.reset()
+            rospy.loginfo("Retracting...")
             robot.base.force_drive(-0.1, 0, 0, 3)  # Drive backwards at 0.1m/s for 3s, so 30cm
-            send_gripper_goal("close", wait_for_motion_done=False)
             arm.send_joint_goal("carrying_pose")
+            return "done"
 
-            robot.head.reset()
-
+        @cb_interface(outcomes=["done"], output_keys=["item_picked"])
+        def _set_userdata(user_data):
+            user_data["item_picked"] = "cereal_box"
             return "done"
 
         with self:
-            self.add("PICK", CBState(_pick), transitions={"done": "ALIGN_POUR"})
-            self.add("ALIGN_POUR", CBState(_align_pour), transitions={"done": "POUR"})
-            self.add("POUR", CBState(_pour), transitions={"done": "ALIGN_PLACE"})
-            self.add("ALIGN_PLACE", CBState(_align_place), transitions={"done": "PLACE"})
-            self.add("PLACE", CBState(_place), transitions={"done": "succeeded"})
+            self.add("PICK", CBState(_pick), transitions={"done": "POUR"})
+            self.add("POUR", CBState(_pour), transitions={"done": "SET_USERDATA"})
+            self.add("SET_USERDATA", CBState(_set_userdata), transitions={"done": "PLACE"})
+            self.add("PLACE", PlaceItemOnTable(robot, table_id), transitions={"succeeded": "succeeded",
+                                                                              "failed": "succeeded"})
 
 
 if __name__ == "__main__":

challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py

@@ -142,7 +142,7 @@ def setup_statemachine(robot):
 
         StateMachine.add(
             "NAVIGATE_TO_EXIT",
-            NavigateToWaypoint(robot, EdEntityDesignator(robot, uuid=exit_id)),
+            NavigateToWaypoint(robot, EdEntityDesignator(robot, uuid=exit_id), speak=False),
             transitions={
                 "arrived": "GOODBYE",
                 "unreachable": "GOODBYE",