From fab9dec9cba8d9a8479b1423fe623336c663a153 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 5 Jul 2023 15:47:35 +0200 Subject: [PATCH 01/13] use cartesian poses --- .../navigate_to_and_place_item_on_table.py | 68 +++++++++---------- .../src/challenge_serve_breakfast/tuning.py | 25 +++++++ 2 files changed, 58 insertions(+), 35 deletions(-) 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..ae33a9018 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,8 +11,10 @@ import rospy import visualization_msgs.msg from geometry_msgs.msg import PoseStamped, Vector3 +from pykdl_ros import FrameStamped from challenge_serve_breakfast.tuning import ( + get_item_place_pose, JOINTS_PRE_PRE_PLACE, JOINTS_PRE_PLACE_HORIZONTAL, JOINTS_PRE_PLACE_VERTICAL, @@ -40,9 +42,8 @@ def __init__(self, robot, table_id): # noinspection PyProtectedMember arm = robot.get_arm()._arm - 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 +51,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 +60,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 = 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 = 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) + 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): diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 97493d544..29735beca 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -14,6 +14,10 @@ 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.2, 0, 0.76)) + +# vectors of the items with respect to the breakfast frame ITEM_VECTOR_DICT = { "spoon": PyKDL.Vector(0.0, -0.15, 0), "bowl": PyKDL.Vector(0.0, 0.0, 0), @@ -21,6 +25,14 @@ "cereal_box": PyKDL.Vector(-0.05, -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, 0), PyKDL.Vector(0.0, 0.0, 0.0)), + "bowl": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.0)), + "milk_carton": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.0)), + "cereal_box": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.0)), +} + COLOR_DICT = { "spoon": ColorRGBA(1, 0, 1, 1), "bowl": ColorRGBA(0, 1, 1, 1), @@ -68,6 +80,19 @@ 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 = 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 item_frame_to_pose(item_frame, frame_id): goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time() From f62d744e9f78b3da3bf1331d00d53d909288c52a Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Thu, 6 Jul 2023 15:53:19 +0200 Subject: [PATCH 02/13] fix deepcopy --- .../navigate_to_and_place_item_on_table.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 ae33a9018..216da4fc6 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 @@ -63,10 +63,10 @@ def _place(user_data): item_place_pose = get_item_place_pose(user_data["item_picked"]) place_fs = FrameStamped(item_place_pose, rospy.Time(0), table_id) - pre_place_fs = place_fs + pre_place_fs = copy.deepcopy(place_fs) pre_place_fs.frame.p.z(place_fs.frame.p.z() + 0.1) - post_place_fs = place_fs + post_place_fs = copy.deepcopy(place_fs) post_place_fs.frame.p.z(place_fs.frame.p.z() + 0.2) rospy.loginfo("Pre Placing...") From 5fccc80289a41e23f9d67d50afe32a94d738acf6 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Thu, 6 Jul 2023 15:53:48 +0200 Subject: [PATCH 03/13] fix visualize points --- .../navigate_to_and_place_item_on_table.py | 1 - 1 file changed, 1 deletion(-) 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 216da4fc6..bffd58099 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 @@ -180,7 +180,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) From 78323996c4b6ea4015da9e3518ac66037d31002e Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Thu, 6 Jul 2023 15:54:27 +0200 Subject: [PATCH 04/13] add place poses --- .../src/challenge_serve_breakfast/tuning.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 29735beca..de3ad49e7 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -27,10 +27,10 @@ # 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, 0), PyKDL.Vector(0.0, 0.0, 0.0)), - "bowl": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.0)), - "milk_carton": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.0)), - "cereal_box": PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0), PyKDL.Vector(0.0, 0.0, 0.0)), + "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)), } COLOR_DICT = { @@ -66,7 +66,7 @@ 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 = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, math.pi), PyKDL.Vector(0.2, 0, 0.76)) item_placement_vector = item_vector item_frame = frame From a6993856bc992adb1585427472015166c3ddd8a8 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 18:47:20 +0200 Subject: [PATCH 05/13] fix BREAKFAST_POSE --- .../src/challenge_serve_breakfast/tuning.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index de3ad49e7..3edf98390 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 @@ -66,7 +67,7 @@ def item_vector_to_item_frame(item_vector): - frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, math.pi), PyKDL.Vector(0.2, 0, 0.76)) + frame = copy.deepcopy(BREAKFAST_POSE) item_placement_vector = item_vector item_frame = frame From 8284ec35394314bdf4e7d141a46876a62e7ee63c Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 18:47:42 +0200 Subject: [PATCH 06/13] tuning for impuls --- .../src/challenge_serve_breakfast/tuning.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 3edf98390..fb1d06b81 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -16,14 +16,14 @@ 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.2, 0, 0.76)) +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 From 7a50f552313dfb96ba2cbaf7f17057d03321ad11 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 19:32:12 +0200 Subject: [PATCH 07/13] make pour cartesian --- .../pick_pour_place_cereal.py | 91 +++++++++---------- .../src/challenge_serve_breakfast/tuning.py | 17 ++++ 2 files changed, 59 insertions(+), 49 deletions(-) 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..3b7e6826e 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 @@ -4,16 +4,33 @@ # # \author Rein Appeldoorn +import copy import os import PyKDL import rospy - -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, \ +from pykdl_ros import FrameStamped + +from challenge_serve_breakfast.tuning import ( + get_item_place_pose, + get_item_pour_poses, + JOINTS_POST_PICK, + JOINTS_PRE_PRE_PLACE, + JOINTS_PRE_PLACE_HORIZONTAL, + JOINTS_PRE_PLACE_VERTICAL, + 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, + POUR_OFFSET_Y, JOINTS_PRE_POUR, JOINTS_POUR, JOINTS_PLACE_HORIZONTAL, JOINTS_RETRACT, \ POUR_OFFSET_X +) +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 +49,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 +61,35 @@ 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") - - robot.head.reset() - 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": "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/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index fb1d06b81..8654cba1c 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -34,6 +34,11 @@ "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))] +} + COLOR_DICT = { "spoon": ColorRGBA(1, 0, 1, 1), "bowl": ColorRGBA(0, 1, 1, 1), @@ -93,6 +98,18 @@ def get_item_place_pose(item_name): return item_place_pose +def get_item_pour_poses(item_name): + item_vector = ITEM_VECTOR_DICT[item_name] + + item_frame = 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() From 1c75f7d35bc91cd95e07482de1c612ce0284714a Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 20:04:04 +0200 Subject: [PATCH 08/13] set userdata for PlaceItemOnTable --- .../challenge_serve_breakfast/pick_pour_place_cereal.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 3b7e6826e..e990ce15c 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 @@ -84,10 +84,15 @@ def _pour(_): arm.send_joint_goal("carrying_pose") return "done" + @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": "POUR"}) - self.add("POUR", CBState(_pour), transitions={"done": "PLACE"}) + 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"}) From 2111d8c9090f5df36e9a1e53ce0e885b2f98be49 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 20:04:14 +0200 Subject: [PATCH 09/13] fix tuning of pouring --- .../src/challenge_serve_breakfast/tuning.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 8654cba1c..755bbe77e 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -36,7 +36,8 @@ 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.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 = { From dd3868608881f88a82f399b92ee1a4b6233efc13 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 20:04:30 +0200 Subject: [PATCH 10/13] add retract as flag --- .../navigate_to_and_place_item_on_table.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 bffd58099..0a593f9e1 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 @@ -37,10 +37,11 @@ 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_goal(pose_goal, wait_for_motion_done=True): arm.send_goal(pose_goal, timeout=0.0) @@ -79,7 +80,7 @@ def _place(user_data): send_goal(place_fs) send_gripper_goal("open") - if item_name != "cereal_box": + 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 @@ -155,7 +156,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"}, ) From 4a7f00ae08536c693ae215e1b981c1bb67c7e059 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 20:13:07 +0200 Subject: [PATCH 11/13] dont speak --- .../src/challenge_serve_breakfast/serve_breakfast.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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", From 1513c361b95a3197bbc38b20f3348300f5ee2891 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 20:24:28 +0200 Subject: [PATCH 12/13] remove unused imports and definitions --- .../navigate_to_and_pick_item.py | 4 +--- .../navigate_to_and_place_item_on_table.py | 10 ++------- .../pick_pour_place_cereal.py | 16 -------------- .../src/challenge_serve_breakfast/tuning.py | 21 ------------------- 4 files changed, 3 insertions(+), 48 deletions(-) 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 0a593f9e1..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 @@ -15,17 +15,11 @@ from challenge_serve_breakfast.tuning import ( get_item_place_pose, - JOINTS_PRE_PRE_PLACE, - JOINTS_PRE_PLACE_HORIZONTAL, - JOINTS_PRE_PLACE_VERTICAL, 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 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 e990ce15c..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 @@ -4,30 +4,14 @@ # # \author Rein Appeldoorn -import copy import os -import PyKDL import rospy from pykdl_ros import FrameStamped from challenge_serve_breakfast.tuning import ( - get_item_place_pose, get_item_pour_poses, JOINTS_POST_PICK, - JOINTS_PRE_PRE_PLACE, - JOINTS_PRE_PLACE_HORIZONTAL, - JOINTS_PRE_PLACE_VERTICAL, - 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, - POUR_OFFSET_Y, JOINTS_PRE_POUR, JOINTS_POUR, JOINTS_PLACE_HORIZONTAL, JOINTS_RETRACT, \ - POUR_OFFSET_X ) from challenge_serve_breakfast.navigate_to_and_place_item_on_table import PlaceItemOnTable from robot_skills import get_robot diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 755bbe77e..22bef1cb9 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -47,31 +47,10 @@ "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 = copy.deepcopy(BREAKFAST_POSE) From 982e9d59f7a1e72e4337c0ff1426b1b65a4d010c Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 1 Aug 2023 20:28:31 +0200 Subject: [PATCH 13/13] fix deepcopy error --- .../src/challenge_serve_breakfast/tuning.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py index 22bef1cb9..a51d393d1 100644 --- a/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py +++ b/challenge_serve_breakfast/src/challenge_serve_breakfast/tuning.py @@ -69,7 +69,7 @@ def item_vector_to_item_frame(item_vector): def get_item_place_pose(item_name): item_vector = ITEM_VECTOR_DICT[item_name] - item_frame = BREAKFAST_POSE + item_frame = copy.deepcopy(BREAKFAST_POSE) item_frame.p = BREAKFAST_POSE * item_vector item_place_offset = ITEM_OFFSET_DICT[item_name] @@ -81,7 +81,7 @@ def get_item_place_pose(item_name): def get_item_pour_poses(item_name): item_vector = ITEM_VECTOR_DICT[item_name] - item_frame = BREAKFAST_POSE + item_frame = copy.deepcopy(BREAKFAST_POSE) item_frame.p = BREAKFAST_POSE * item_vector item_pour_offsets = POUR_OFFSET_DICT[item_name]