Bep/cutlery grasp

robot_smach_states/examples/manipulation/example_top_grasp.py

@@ -0,0 +1,46 @@
+# System
+import argparse
+
+# ROS
+import PyKDL as kdl
+import rospy
+
+# TU/e Robotics
+from ed.entity import Entity
+from ed.shape import RightPrism
+from pykdl_ros import FrameStamped
+from robot_skills.get_robot import get_robot
+
+# Robot Smach States
+import robot_smach_states.util.designators as ds
+from robot_smach_states.manipulation.top_grasp import TopGrab
+
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="Put an imaginary object in the world model and grasp it using the "
+                                                 "'topgrasp' smach state")
+    parser.add_argument("x", type=float, help="x-coordinate (in map) of the imaginary object")
+    parser.add_argument("y", type=float, help="y-coordinate (in map) of the imaginary object")
+    parser.add_argument("z", type=float, help="z-coordinate (in map) of the imaginary object")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node("test_top_grasping")
+
+    robot = get_robot(args.robot)
+
+    entity_id = "test_item"
+    pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)),
+                        stamp=rospy.Time.now(),
+                        frame_id="map")
+    robot.ed.update_entity(uuid=entity_id, frame_stamped=pose)
+    shape = RightPrism([kdl.Vector(0, 0, 0), kdl.Vector(0, 0.05, 0), kdl.Vector(0.05, 0.05, 0), kdl.Vector(0.05, 0, 0)], -0.1, 0.1)
+    item = Entity(entity_id, "test_type", pose.header.frame_id, pose.frame, shape, None, None, rospy.Time.now())
+
+    item = ds.Designator(item)
+
+    arm = ds.UnoccupiedArmDesignator(robot).lockable()
+
+    grab_state = TopGrab(robot, item, arm)
+    grab_state.execute()

robot_smach_states/src/robot_smach_states/manipulation/cutlery_detector.py

@@ -0,0 +1,64 @@
+import rospy
+import cv2
+import numpy as np
+from ultralytics import YOLO
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+
+
+class YoloSegmentor:
+    def __init__(self) -> None:
+        model_path = "~/MEGA/developers/Donal/yolov8x-seg.pt"
+        device = "cuda"
+        self.model = YOLO(model_path).to(device)
+        self.class_ids = [42, 43, 44]  # See the COCO dataset for class id to label info (table=60, person = 0)
+        self.active = False
+
+        self.publisher = rospy.Publisher('/hero/segmented_image', Image, queue_size=10)
+        self.subscriber = rospy.Subscriber('/hero/hand_camera/image_raw', Image, self.callback)
+
+    def start(self):
+        self.active = True
+
+    def stop(self):
+        self.active = False
+
+    @staticmethod
+    def detect(model, frame):
+        results = model(frame)
+        result = results[0]
+        segmentation_contours_idx = [np.array(seg, dtype=np.int32) for seg in result.masks.xy]
+        class_ids = np.array(result.boxes.cls.cpu(), dtype="int")
+        return class_ids, segmentation_contours_idx
+
+    def extract_table_segment(self, image, class_ids, segmentations):
+        table_mask = np.zeros_like(image, dtype=np.uint8)
+        for class_id, seg in zip(class_ids, segmentations):
+            if class_id in self.class_ids:
+                cv2.fillPoly(table_mask, [seg], color=(255, 0, 255))
+            else:
+                cv2.fillPoly(table_mask, [seg], color=(255, 0, 0))
+        return table_mask
+
+    def callback(self, data):
+        #rospy.loginfo("got message")
+        if not self.active:
+            return
+        bridge = CvBridge()
+        cv_image = bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
+        cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
+        #rospy.loginfo("converted message")
+
+        classes, segmentations = self.detect(self.model, cv_image)
+        table_segment = self.extract_table_segment(cv_image, classes, segmentations)
+
+        # Publish the table segment as a binary mask
+        table_message = bridge.cv2_to_imgmsg(table_segment, encoding="passthrough")
+        self.publisher.publish(table_message)
+
+
+if __name__ == '__main__':
+    rospy.init_node("cutlery_detector")
+    ts = YoloSegmentor()
+    ts.start()
+    rospy.spin()

robot_smach_states/src/robot_smach_states/manipulation/top_grasp.py

@@ -0,0 +1,216 @@
+from __future__ import absolute_import
+
+# ROS
+import PyKDL as kdl
+from pykdl_ros import VectorStamped, FrameStamped
+import rospy
+import smach
+import tf2_ros
+from geometry_msgs.msg import Twist
+
+# TU/e Robotics
+from robot_skills.robot import Robot
+from robot_skills.arm.arms import PublicArm, GripperTypes
+from ..utility import check_arm_requirements, ResolveArm
+from ..util.designators import check_type
+from ..navigation.navigate_to_grasp import NavigateToGrasp
+from ..manipulation.grasp_point_determination import GraspPointDeterminant
+from ..util.designators.arm import ArmDesignator
+from ..util.designators.core import Designator
+
+from robot_smach_states.manipulation.cutlery_detector import YoloSegmentor
+from robot_skills.util.exceptions import TimeOutException
+
+
+class PrepareGrasp(smach.State):
+    REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING],
+                               "required_trajectories": ["prepare_grasp"], }
+
+    def __init__(self, robot: Robot, arm: ArmDesignator) -> None:
+        """
+        Set the arm in the appropriate position before actually grabbing
+
+        :param robot: robot to execute state with
+        :param arm: Designator that resolves to the arm to grasp with
+        """
+        smach.State.__init__(self, outcomes=['succeeded', 'failed'])
+
+        # Assign member variables
+        self.robot = robot
+        self.arm_designator = arm
+
+    def execute(self, userdata=None) -> str:
+        arm = self.arm_designator.resolve()
+        if not arm:
+            rospy.logerr("Could not resolve arm")
+            return "failed"
+
+        # Arm to position in a safe way
+        arm.send_joint_trajectory("prepare_grasp")
+        arm.wait_for_motion_done()
+
+        # Open gripper
+        arm.gripper.send_goal('open', timeout=0.0)
+        arm.wait_for_motion_done()
+        return 'succeeded'
+
+
+class TopGrasp(smach.State):
+    REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING],
+                               "required_goals": ["carrying_pose"], }
+
+    def __init__(self, robot: Robot, arm: ArmDesignator) -> None:
+        """
+        Pick up an item given an arm and an entity to be picked up
+
+        :param robot: robot to execute this state with
+        :param arm: Designator that resolves to the arm to grasp with
+        """
+        smach.State.__init__(self, outcomes=['succeeded', 'failed'])
+
+        # Assign member variables
+        self.robot = robot
+        self.arm_designator = arm
+
+        assert self.robot.get_arm(**self.REQUIRED_ARM_PROPERTIES) is not None,\
+            "None of the available arms meets all this class's requirements: {}".format(self.REQUIRED_ARM_PROPERTIES)
+
+        self.yolo_segmentor = YoloSegmentor()
+
+    def execute(self, userdata=None) -> str:
+        arm = self.arm_designator.resolve()
+        if not arm:
+            rospy.logerr("Could not resolve arm")
+            return "failed"
+
+        grasp_succeeded=False
+        rate = rospy.Rate(10) # loop rate in hz
+
+        base_to_gripper = self.frame_from_xyzrpy(0.5, # x distance to the robot
+                                                0.08, # y distance off center from the robot (fixed if rpy=0)
+                                                0.7, # z height of the gripper
+                                                0, 0, 0) # Roll pitch yaw. 0,0,0 for a horizontal gripper.
+        # start segmentation
+        self.yolo_segmentor.start()
+
+        move_arm = True
+        while not grasp_succeeded:
+            # control loop
+
+            #TODO get grasp pose wrt wrist
+
+            #TODO force sensor does not provide a good interface for this.
+            try:
+                arm._arm.force_sensor.wait_for_edge_up(1.0)  # wait 1 second for a force detection
+            except TimeOutException:
+                rospy.loginfo("No edge up detected within timeout")
+                pass
+
+            # example base command
+            v = Twist()
+            v.linear.x = 0 # forward
+            v.linear.y = 0 # linear left
+            v.angular.z = 0 # rotation speed to the left
+            self.robot.base._cmd_vel.publish(v) # send command to the robot
+
+            # example arm pose command
+            if (move_arm):
+                pose_goal = FrameStamped(base_to_gripper,
+                                        rospy.Time.now(), #timestamp when this pose was created
+                                        "base_link" # the frame in which the pose is expressed. base link lies in the center of the robot at the height of the floor.
+                                        )
+                arm.send_goal(pose_goal) # send the command to the robot.
+                arm.wait_for_motion_done() # wait until the motion is complete
+                move_arm = False # reset flag to move the arm.
+                continue # dont wait for the rest of the loop.
+            #TODO get base-gripper transform
+
+            # check if done
+            if False: # check if the grasp has succeeded
+             grasp_succeeded = True
+
+            rospy.loginfo(f"print a message to show we are still running.")
+            rate.sleep()
+        # stop segmentation to preserve computation power
+        self.yolo_segmentor.stop()
+
+        return "succeeded"
+
+    @staticmethod
+    def frame_from_xyzrpy(x, y, z, roll, pitch, yaw):
+        """
+        Helper function to create a kdl frame based on an x,y,z position and a RPY rotation
+        """
+        return kdl.Frame(kdl.Rotation.RPY(roll, pitch, yaw), kdl.Vector(x, y, z))
+
+
+class ResetOnFailure(smach.State):
+    """ Class to reset the robot after a grab has failed """
+
+    REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING], }
+
+    def __init__(self, robot, arm):
+        """
+        Constructor
+
+        :param robot: robot object
+        :param arm: arm designator
+        """
+        smach.State.__init__(self, outcomes=['done'])
+
+        self._robot = robot
+        self.arm_designator = arm
+
+    def execute(self, userdata=None):
+        """ Execute hook """
+        arm = self.arm_designator.resolve()
+        arm.reset()
+
+        if self._robot.robot_name == "amigo":
+            self._robot.torso.reset()  # Move up to make resetting of the arm safer.
+        if arm is not None:
+            arm.gripper.send_goal('open')
+        self._robot.head.reset()  # Sends a goal
+        self._robot.head.cancel_goal()  # And cancels it...
+        if arm is not None:
+            arm.reset()
+        self._robot.torso.reset()
+        return 'done'
+
+
+class TopGrab(smach.StateMachine):
+    def __init__(self, robot: Robot, item: Designator, arm: ArmDesignator):
+        """
+        Let the given robot move to an entity and grab that entity using some arm
+
+        :param robot: Robot to use
+        :param item: Designator that resolves to the item to grab. E.g. EntityByIdDesignator
+        :param arm: Designator that resolves to the arm to use for grabbing. E.g. UnoccupiedArmDesignator
+        """
+        smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
+
+        # Check types or designator resolve types
+        check_type(arm, PublicArm)
+
+        with self:
+            smach.StateMachine.add('RESOLVE_ARM', ResolveArm(arm, self),
+                                   transitions={'succeeded': 'NAVIGATE_TO_GRAB',
+                                                'failed': 'failed'})
+
+            smach.StateMachine.add('NAVIGATE_TO_GRAB', NavigateToGrasp(robot, arm, item),
+                                   transitions={'unreachable': 'RESET_FAILURE',
+                                                'goal_not_defined': 'RESET_FAILURE',
+                                                'arrived': 'PREPARE_GRASP'})
+
+            smach.StateMachine.add('PREPARE_GRASP', PrepareGrasp(robot, arm),
+                                   transitions={'succeeded': 'GRAB',
+                                                'failed': 'RESET_FAILURE'})
+
+            smach.StateMachine.add('GRAB', TopGrasp(robot, arm),
+                                   transitions={'succeeded': 'done',
+                                                'failed': 'RESET_FAILURE'})
+
+            smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot, arm),
+                                   transitions={'done': 'failed'})
+
+        check_arm_requirements(self, robot)