From fa08a4409b2440bfe653935d71d3dc1af3313c25 Mon Sep 17 00:00:00 2001
From: Shashank_Narla <s.narla@student.tue.nl>
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 <s.narla@student.tue.nl>
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 <shanki98@gmail.com>
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 <shanki98@gmail.com>
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)