diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py index 039f8940d..20238d067 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_pick_item.py +++ b/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 diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py index 4d523df03..69619e0dc 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/navigate_to_and_place_item_on_table.py +++ b/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,14 +31,14 @@ 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() @@ -50,7 +46,7 @@ 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) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py index f8fa53780..ebecaae74 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/pick_pour_place_cereal.py +++ b/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,6 +33,11 @@ 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) @@ -39,64 +45,40 @@ def _pick(_): 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__": diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py index bea8d7a9e..db7fd052c 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/serve_breakfast.py +++ b/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", diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 97493d544..a51d393d1 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -4,6 +4,7 @@ # # \author Rein Appeldoorn +import copy import math import PyKDL @@ -14,11 +15,29 @@ REQUIRED_ITEMS = ["spoon", "bowl", "milk_carton", "cereal_box"] +# pose of the breakfast on the table +BREAKFAST_POSE = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, math.pi), PyKDL.Vector(0.7, 0, 0.76)) + +# vectors of the items with respect to the breakfast frame ITEM_VECTOR_DICT = { - "spoon": PyKDL.Vector(0.0, -0.15, 0), + "spoon": PyKDL.Vector(0.0, -0.1, 0), "bowl": PyKDL.Vector(0.0, 0.0, 0), - "milk_carton": PyKDL.Vector(-0.05, 0.15, 0), - "cereal_box": PyKDL.Vector(-0.05, -0.2, 0), + "milk_carton": PyKDL.Vector(0.0, 0.15, 0), + "cereal_box": PyKDL.Vector(0.0, -0.2, 0), +} + +# frame indicating the pose of the hand with respect to the vector in ITEM_VECTOR_DICT +ITEM_OFFSET_DICT = { + "spoon": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0.5*math.pi, 0), PyKDL.Vector(0.0, 0.0, 0.1)), + "bowl": PyKDL.Frame(PyKDL.Rotation.RPY(0.5*math.pi, 0.25*math.pi, 0.0), PyKDL.Vector(-0.08, 0.0, 0.07)), + "milk_carton": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.07)), + "cereal_box": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.07)), +} + +POUR_OFFSET_DICT = { + "bowl": [PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, -0.07, 0.20)), + PyKDL.Frame(PyKDL.Rotation.RPY(-0.5*math.pi, 0, 0), PyKDL.Vector(0.0, -0.07, 0.20)), + PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, -0.07, 0.20))] } COLOR_DICT = { @@ -28,33 +47,12 @@ "cereal_box": ColorRGBA(1, 1, 0, 1), } -PICK_ROTATION = 0 - JOINTS_HANDOVER = [0.4, -0.2, 0.0, -1.37, 0] -JOINTS_PRE_PRE_PLACE = [0.69, 0, 0, -0.7, 0] - -JOINTS_PRE_PLACE_HORIZONTAL = [0.8, -1.2, 0, 0, 0] -JOINTS_PLACE_HORIZONTAL = [0.65, -1.75, 0, 0, 0] -JOINTS_PLACE_HORIZONTAL_MILK = [0.55, -1.75, 0, 0, 0] - -JOINTS_PRE_PLACE_VERTICAL = [0.8, -1.2, 0, -1.57, 0] -JOINTS_PLACE_VERTICAL = [0.65, -1.57, 0, -1.57, 0] - -JOINTS_RETRACT = [0.7, 0, 0, -1.57, 0] - JOINTS_POST_PICK = [0.7, -1.2, 0, 0, 0] -JOINTS_PRE_POUR = [0.35, -1.2, 0, 0, 0] - -JOINTS_POUR = [0.4, -1.2, -2.5, 0, 0] - -POUR_OFFSET_X = -0.15 -POUR_OFFSET_Y = 0.15 - - def item_vector_to_item_frame(item_vector): - frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, math.pi), PyKDL.Vector(0.7, 0, 0)) + frame = copy.deepcopy(BREAKFAST_POSE) item_placement_vector = item_vector item_frame = frame @@ -68,6 +66,31 @@ def item_vector_to_item_frame(item_vector): return item_frame +def get_item_place_pose(item_name): + item_vector = ITEM_VECTOR_DICT[item_name] + + item_frame = copy.deepcopy(BREAKFAST_POSE) + item_frame.p = BREAKFAST_POSE * item_vector + + item_place_offset = ITEM_OFFSET_DICT[item_name] + item_place_pose = item_frame * item_place_offset + rospy.loginfo(f"Placing at frame {item_frame} with place pose {item_place_pose}") + + return item_place_pose + +def get_item_pour_poses(item_name): + item_vector = ITEM_VECTOR_DICT[item_name] + + item_frame = copy.deepcopy(BREAKFAST_POSE) + item_frame.p = BREAKFAST_POSE * item_vector + + item_pour_offsets = POUR_OFFSET_DICT[item_name] + item_pour_poses = [] + for offset in item_pour_offsets: + item_pour_poses.append(item_frame * offset) + return item_pour_poses + + def item_frame_to_pose(item_frame, frame_id): goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time()