suction cup interface for HERO

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
+
+