From fa08a4409b2440bfe653935d71d3dc1af3313c25 Mon Sep 17 00:00:00 2001 From: Shashank_Narla Date: Tue, 4 May 2021 22:07:09 +0200 Subject: [PATCH 1/4] [hero_skills] add first version of suction cup gripper --- hero_skills/src/hero_skills/Suction_cup.py | 66 ++++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 hero_skills/src/hero_skills/Suction_cup.py diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py new file mode 100644 index 0000000000..a6b9514821 --- /dev/null +++ b/hero_skills/src/hero_skills/Suction_cup.py @@ -0,0 +1,66 @@ +#ROS +import rospy + +import PyKDL as kdl +from actionlib import GoalStatus +from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandGoal +from tue_msgs.msg import GripperCommand + + +from robot_skills.arms.gripper import Gripper +# tmc_suction +from tmc_suction.msg import SuctionControlAction, SuctionControlGoal + + +class SuctionGripper(Gripper): + """ + A Suction gripper that has a vacuum suction cup to grasp objects + """ + def __init__(self, robot_name, tf_listener, gripper_name): + """ + constructor + :param robot_name: robot_name + :param tf_listener: tf_server.TFClient() + :param gripper_name: string to identify the gripper + """ + super(SuctionGripper, self).__init__(robot_name=robot_name, tf_listener=tf_listener) + self.gripper_name = gripper_name + offset = self.load_param('skills/' + self.gripper_name + '/grasp_offset/') + self.offset = kdl.Frame(kdl.Rotation.RPY(offset["roll"], offset["pitch"], offset["yaw"]),kdl.Vector(offset["x"], offset["y"], offset["z"])) + + # Init gripper actionlib for SuctionGripper + self._ac_suction = self.create_simple_action_client("/" + robot_name + "/suction_control", SuctionControlAction) + + # Waits until the action server has started up and started + # listening for goals + self._ac_suction.wait_for_server() + + def send_gripper_goal(self, sucking, timeout=5.0): + """ + Send a GripperCommand to the gripper and wait for finishing + :param sucking: turn suction on or off + :type sucking: bool + :param timeout: timeout in seconds; timeout of 0.0 is not allowed + :type timeout: float + :return: True of False + :rtype: bool + """ + + if not isinstance(sucking, bool): + rospy.logerr('State should be true or false, now it is {0}'.format(sucking)) + return False + + goal = SuctionControlGoal() + + goal.suction_on.data = sucking + + self._ac_suction.send_goal(goal) + + goal_status = GoalStatus.SUCCEEDED + + if timeout != 0.0: + self._ac_suction.wait_for_result(rospy.Duration(timeout)) + goal_status = self._ac_suction.get_state() + + return goal_status == GoalStatus.SUCCEEDED + From 6d1be96c05ad2f4f04614e3d7c5345da09f57e1a Mon Sep 17 00:00:00 2001 From: Shashank_Narla Date: Tue, 25 May 2021 22:12:18 +0200 Subject: [PATCH 2/4] Added goal to suction arm --- hero_skills/src/hero_skills/Suction_cup.py | 126 ++++++++++++++++++++- 1 file changed, 124 insertions(+), 2 deletions(-) diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py index a6b9514821..aa28120f79 100644 --- a/hero_skills/src/hero_skills/Suction_cup.py +++ b/hero_skills/src/hero_skills/Suction_cup.py @@ -1,13 +1,14 @@ #ROS import rospy - +import time import PyKDL as kdl from actionlib import GoalStatus from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandGoal +from tue_manipulation_msgs.msg import GraspPrecomputeGoal, GraspPrecomputeAction from tue_msgs.msg import GripperCommand -from robot_skills.arms.gripper import Gripper +from robot_skills.arm.gripper import Gripper # tmc_suction from tmc_suction.msg import SuctionControlAction, SuctionControlGoal @@ -64,3 +65,124 @@ def send_gripper_goal(self, sucking, timeout=5.0): return goal_status == GoalStatus.SUCCEEDED + @property + def occupied_by_suction(self): + """ + The 'occupied_by_suction' property will return the current entity that is in the suction cup of this arm. + :return: robot_skills.util.entity, ED entity + """ + return self._occupied_by_suction + + @occupied_by_suction.setter + def occupied_by_suction(self, value): + """ + Set the entity which occupies the suction cup. + :param value: robot_skills.util.entity, ED entity + :return: no return + """ + self._occupied_by_suction = value + + def send_goal_suction(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_only=False, + allowed_touch_objects=None): + """ + Send a arm to a goal: + Using a combination of position and orientation: a kdl.Frame. A time + out time_out. pre_grasp means go to an offset that is normally needed + for things such as grasping. You can also specify the frame_id which + defaults to base_link + :param frameStamped: A FrameStamped to move the arm's end effector to + :param timeout: timeout in seconds; In case of 0.0, goal is executed without feedback and waiting + :param pre_grasp: Bool to use pre_grasp or not + :param first_joint_pos_only: Bool to only execute first joint position of whole trajectory + :param allowed_touch_objects: List of object names in the worldmodel, which are allowed to be touched + :return: True of False + """ + if allowed_touch_objects is None: + allowed_touch_objects = list() + + # save the arguments for debugging later + myargs = locals() + + # If necessary, prefix frame_id + if frameStamped.frame_id.find(self.robot_name) < 0: + frameStamped.frame_id = "/" + self.robot_name + "/" + frameStamped.frame_id + rospy.loginfo("Grasp precompute frame id = {0}".format(frameStamped.frame_id)) + + # Convert to baselink, which is needed because the offset is defined in the base_link frame + frame_in_baselink = frameStamped.projectToFrame("/" + self.robot_name + "/base_link", self.tf_listener) + + # TODO: Get rid of this custom message type + # Create goal: + grasp_precompute_goal = GraspPrecomputeGoal() + grasp_precompute_goal.goal.header.frame_id = frame_in_baselink.frame_id + grasp_precompute_goal.goal.header.stamp = rospy.Time.now() + + grasp_precompute_goal.PERFORM_PRE_GRASP = pre_grasp + grasp_precompute_goal.FIRST_JOINT_POS_ONLY = first_joint_pos_only + + grasp_precompute_goal.allowed_touch_objects = allowed_touch_objects + + grasp_precompute_goal.goal.x = frame_in_baselink.frame.p.x() + grasp_precompute_goal.goal.y = frame_in_baselink.frame.p.y() + grasp_precompute_goal.goal.z = frame_in_baselink.frame.p.z() + + roll, pitch, yaw = frame_in_baselink.frame.M.GetRPY() + grasp_precompute_goal.goal.roll = roll + grasp_precompute_goal.goal.pitch = pitch + grasp_precompute_goal.goal.yaw = yaw + + self._publish_marker(grasp_precompute_goal, [1, 0, 0], "grasp_point") + + # Add tunable parameters + # todo: generalise send_goal in base arm so it can work with multiple gripper types + offset_frame = frame_in_baselink.frame * self.offset_suction + + grasp_precompute_goal.goal.x = offset_frame.p.x() + grasp_precompute_goal.goal.y = offset_frame.p.y() + grasp_precompute_goal.goal.z = offset_frame.p.z() + + roll, pitch, yaw = offset_frame.M.GetRPY() + grasp_precompute_goal.goal.roll = roll + grasp_precompute_goal.goal.pitch = pitch + grasp_precompute_goal.goal.yaw = yaw + + # rospy.loginfo("Arm goal: {0}".format(grasp_precompute_goal)) + + self._publish_marker(grasp_precompute_goal, [0, 1, 0], "grasp_point_corrected") + + time.sleep(0.001) # This is necessary: the rtt_actionlib in the hardware seems + # to only have a queue size of 1 and runs at 1000 hz. This + # means that if two goals are send approximately at the same + # time (e.g. an arm goal and a torso goal), one of the two + # goals probably won't make it. This sleep makes sure the + # goals will always arrive in different update hooks in the + # hardware TrajectoryActionLib server. + + # Send goal: + + if timeout == 0.0: + self._ac_grasp_precompute.send_goal(grasp_precompute_goal) + return True + else: + result = self._ac_grasp_precompute.send_goal_and_wait( + grasp_precompute_goal, + execute_timeout=rospy.Duration(timeout)) + if result == GoalStatus.SUCCEEDED: + + result_pose = self.tf_listener.lookupTransform(self.robot_name + "/base_link", + self.robot_name + "/grippoint_{}".format(self.side), + rospy.Time(0)) + dx = grasp_precompute_goal.goal.x - result_pose[0][0] + dy = grasp_precompute_goal.goal.y - result_pose[0][1] + dz = grasp_precompute_goal.goal.z - result_pose[0][2] + + if abs(dx) > 0.005 or abs(dy) > 0.005 or abs(dz) > 0.005: + rospy.logwarn("Grasp-precompute error too large: [{}, {}, {}]".format( + dx, dy, dz)) + return True + else: + # failure + rospy.logerr('grasp precompute goal failed: \n%s', repr(myargs)) + return False + + From 887534c53cc3acc05de42028988d7bba0b1156e0 Mon Sep 17 00:00:00 2001 From: Shashank Date: Tue, 22 Jun 2021 21:08:04 +0200 Subject: [PATCH 3/4] Added changes to Suction_cup --- hero_skills/src/hero_skills/Suction_cup.py | 132 +------------------ robot_skills/src/robot_skills/arm/gripper.py | 2 +- 2 files changed, 5 insertions(+), 129 deletions(-) diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py index aa28120f79..dcc27a41cd 100644 --- a/hero_skills/src/hero_skills/Suction_cup.py +++ b/hero_skills/src/hero_skills/Suction_cup.py @@ -1,15 +1,13 @@ #ROS import rospy -import time -import PyKDL as kdl from actionlib import GoalStatus -from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandGoal -from tue_manipulation_msgs.msg import GraspPrecomputeGoal, GraspPrecomputeAction -from tue_msgs.msg import GripperCommand +#General +import PyKDL as kdl +# TUe Robotics from robot_skills.arm.gripper import Gripper -# tmc_suction +# Toyota from tmc_suction.msg import SuctionControlAction, SuctionControlGoal @@ -64,125 +62,3 @@ def send_gripper_goal(self, sucking, timeout=5.0): goal_status = self._ac_suction.get_state() return goal_status == GoalStatus.SUCCEEDED - - @property - def occupied_by_suction(self): - """ - The 'occupied_by_suction' property will return the current entity that is in the suction cup of this arm. - :return: robot_skills.util.entity, ED entity - """ - return self._occupied_by_suction - - @occupied_by_suction.setter - def occupied_by_suction(self, value): - """ - Set the entity which occupies the suction cup. - :param value: robot_skills.util.entity, ED entity - :return: no return - """ - self._occupied_by_suction = value - - def send_goal_suction(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_only=False, - allowed_touch_objects=None): - """ - Send a arm to a goal: - Using a combination of position and orientation: a kdl.Frame. A time - out time_out. pre_grasp means go to an offset that is normally needed - for things such as grasping. You can also specify the frame_id which - defaults to base_link - :param frameStamped: A FrameStamped to move the arm's end effector to - :param timeout: timeout in seconds; In case of 0.0, goal is executed without feedback and waiting - :param pre_grasp: Bool to use pre_grasp or not - :param first_joint_pos_only: Bool to only execute first joint position of whole trajectory - :param allowed_touch_objects: List of object names in the worldmodel, which are allowed to be touched - :return: True of False - """ - if allowed_touch_objects is None: - allowed_touch_objects = list() - - # save the arguments for debugging later - myargs = locals() - - # If necessary, prefix frame_id - if frameStamped.frame_id.find(self.robot_name) < 0: - frameStamped.frame_id = "/" + self.robot_name + "/" + frameStamped.frame_id - rospy.loginfo("Grasp precompute frame id = {0}".format(frameStamped.frame_id)) - - # Convert to baselink, which is needed because the offset is defined in the base_link frame - frame_in_baselink = frameStamped.projectToFrame("/" + self.robot_name + "/base_link", self.tf_listener) - - # TODO: Get rid of this custom message type - # Create goal: - grasp_precompute_goal = GraspPrecomputeGoal() - grasp_precompute_goal.goal.header.frame_id = frame_in_baselink.frame_id - grasp_precompute_goal.goal.header.stamp = rospy.Time.now() - - grasp_precompute_goal.PERFORM_PRE_GRASP = pre_grasp - grasp_precompute_goal.FIRST_JOINT_POS_ONLY = first_joint_pos_only - - grasp_precompute_goal.allowed_touch_objects = allowed_touch_objects - - grasp_precompute_goal.goal.x = frame_in_baselink.frame.p.x() - grasp_precompute_goal.goal.y = frame_in_baselink.frame.p.y() - grasp_precompute_goal.goal.z = frame_in_baselink.frame.p.z() - - roll, pitch, yaw = frame_in_baselink.frame.M.GetRPY() - grasp_precompute_goal.goal.roll = roll - grasp_precompute_goal.goal.pitch = pitch - grasp_precompute_goal.goal.yaw = yaw - - self._publish_marker(grasp_precompute_goal, [1, 0, 0], "grasp_point") - - # Add tunable parameters - # todo: generalise send_goal in base arm so it can work with multiple gripper types - offset_frame = frame_in_baselink.frame * self.offset_suction - - grasp_precompute_goal.goal.x = offset_frame.p.x() - grasp_precompute_goal.goal.y = offset_frame.p.y() - grasp_precompute_goal.goal.z = offset_frame.p.z() - - roll, pitch, yaw = offset_frame.M.GetRPY() - grasp_precompute_goal.goal.roll = roll - grasp_precompute_goal.goal.pitch = pitch - grasp_precompute_goal.goal.yaw = yaw - - # rospy.loginfo("Arm goal: {0}".format(grasp_precompute_goal)) - - self._publish_marker(grasp_precompute_goal, [0, 1, 0], "grasp_point_corrected") - - time.sleep(0.001) # This is necessary: the rtt_actionlib in the hardware seems - # to only have a queue size of 1 and runs at 1000 hz. This - # means that if two goals are send approximately at the same - # time (e.g. an arm goal and a torso goal), one of the two - # goals probably won't make it. This sleep makes sure the - # goals will always arrive in different update hooks in the - # hardware TrajectoryActionLib server. - - # Send goal: - - if timeout == 0.0: - self._ac_grasp_precompute.send_goal(grasp_precompute_goal) - return True - else: - result = self._ac_grasp_precompute.send_goal_and_wait( - grasp_precompute_goal, - execute_timeout=rospy.Duration(timeout)) - if result == GoalStatus.SUCCEEDED: - - result_pose = self.tf_listener.lookupTransform(self.robot_name + "/base_link", - self.robot_name + "/grippoint_{}".format(self.side), - rospy.Time(0)) - dx = grasp_precompute_goal.goal.x - result_pose[0][0] - dy = grasp_precompute_goal.goal.y - result_pose[0][1] - dz = grasp_precompute_goal.goal.z - result_pose[0][2] - - if abs(dx) > 0.005 or abs(dy) > 0.005 or abs(dz) > 0.005: - rospy.logwarn("Grasp-precompute error too large: [{}, {}, {}]".format( - dx, dy, dz)) - return True - else: - # failure - rospy.logerr('grasp precompute goal failed: \n%s', repr(myargs)) - return False - - diff --git a/robot_skills/src/robot_skills/arm/gripper.py b/robot_skills/src/robot_skills/arm/gripper.py index 0767c7500d..238771a8a6 100644 --- a/robot_skills/src/robot_skills/arm/gripper.py +++ b/robot_skills/src/robot_skills/arm/gripper.py @@ -47,7 +47,7 @@ def occupied_by(self): @occupied_by.setter def occupied_by(self, value): """ - Set the entity which occupies the arm. + Set the entity which occupies the Gripper. :param value: robot_skills.util.entity, ED entity :return: no return From 58206abf50c00f95535e1a89a82078e3ee558e76 Mon Sep 17 00:00:00 2001 From: Shashank Date: Tue, 20 Jul 2021 22:40:11 +0200 Subject: [PATCH 4/4] Changed send_goal and created suction object --- hero_skills/src/hero_skills/Suction_cup.py | 19 ++++++++++++------- hero_skills/src/hero_skills/hero.py | 2 ++ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py index dcc27a41cd..59c43b5466 100644 --- a/hero_skills/src/hero_skills/Suction_cup.py +++ b/hero_skills/src/hero_skills/Suction_cup.py @@ -15,14 +15,14 @@ class SuctionGripper(Gripper): """ A Suction gripper that has a vacuum suction cup to grasp objects """ - def __init__(self, robot_name, tf_listener, gripper_name): + def __init__(self, robot_name, tf_buffer, gripper_name): """ constructor :param robot_name: robot_name - :param tf_listener: tf_server.TFClient() + :param tf_buffer: tf2_ros.Buffer :param gripper_name: string to identify the gripper """ - super(SuctionGripper, self).__init__(robot_name=robot_name, tf_listener=tf_listener) + super(SuctionGripper, self).__init__(robot_name=robot_name, tf_listener=tf_buffer) self.gripper_name = gripper_name offset = self.load_param('skills/' + self.gripper_name + '/grasp_offset/') self.offset = kdl.Frame(kdl.Rotation.RPY(offset["roll"], offset["pitch"], offset["yaw"]),kdl.Vector(offset["x"], offset["y"], offset["z"])) @@ -34,7 +34,7 @@ def __init__(self, robot_name, tf_listener, gripper_name): # listening for goals self._ac_suction.wait_for_server() - def send_gripper_goal(self, sucking, timeout=5.0): + def send_gripper_goal(self, sucking, timeout=0.0): """ Send a GripperCommand to the gripper and wait for finishing :param sucking: turn suction on or off @@ -55,10 +55,15 @@ def send_gripper_goal(self, sucking, timeout=5.0): self._ac_suction.send_goal(goal) - goal_status = GoalStatus.SUCCEEDED - if timeout != 0.0: self._ac_suction.wait_for_result(rospy.Duration(timeout)) goal_status = self._ac_suction.get_state() + goal_status_bool = goal_status == GoalStatus.SUCCEEDED + else: + rospy.logerr("A timeout value of 0.0 is given, which is not allowed!") + goal_status = self._ac_suction.get_state() + while goal_status == GoalStatus.PENDING: + goal_status = self._ac_suction.get_state() + goal_status_bool = goal_status == GoalStatus.SUCCEEDED - return goal_status == GoalStatus.SUCCEEDED + return goal_status_bool diff --git a/hero_skills/src/hero_skills/hero.py b/hero_skills/src/hero_skills/hero.py index a781c7da69..c25a8846aa 100644 --- a/hero_skills/src/hero_skills/hero.py +++ b/hero_skills/src/hero_skills/hero.py @@ -8,6 +8,7 @@ from robot_skills import api, base, ebutton, head, ears, lights, perception, robot, speech, torso, world_model_ed from robot_skills.arm import arms, force_sensor, gripper, handover_detector from robot_skills.simulation import is_sim_mode, SimEButton +from hero_skills import Suction_cup class Hero(robot.Robot): @@ -31,6 +32,7 @@ def __init__(self, wait_services=False): hero_arm = arms.Arm(self.robot_name, self.tf_buffer, self.get_joint_states, "arm_center") hero_arm.add_part('force_sensor', force_sensor.ForceSensor(self.robot_name, self.tf_buffer, "/" + self.robot_name + "/wrist_wrench/raw")) hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_buffer, 'gripper')) + hero_arm.add_part('suction_gripper', Suction_cup.SuctionGripper(self.robot_name, self.tf_buffer, 'Suction_gripper')) hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_buffer, 'handover_detector')) self.add_arm_part('arm_center', hero_arm)