Example #1
0
    def grasp(self,
              width,
              force,
              speed=None,
              epsilon_inner=0.005,
              epsilon_outer=0.005,
              wait_for_result=True,
              cb=None):
        """
        Grasps an object.
       
        An object is considered grasped if the distance :math:`d` between the gripper fingers satisfies
        :math:`(width - epsilon\_inner) < d < (width + epsilon\_outer)`.
       
        :param width: Size of the object to grasp. [m]
        :param speed: Closing speed. [m/s]
        :param force: Grasping force. [N]
        :param epsilon_inner: Maximum tolerated deviation when the actual grasped width is smaller
                                than the commanded grasp width.
        :param epsilon_outer: Maximum tolerated deviation when the actual grasped width is wider
                                than the commanded grasp width.
        :param cb: Optional callback function to use when the service call is done

        :type width: float
        :type speed: float
        :type force: float
        :type epsilon_inner: float
        :type epsilon_outer: float

        :return: True if an object has been grasped, false otherwise.
        :rtype: bool
        """
        self._caller = "grasp_action"

        if not speed:
            speed = self._gripper_speed

        goal = GraspGoal()
        goal.width = width
        goal.speed = speed
        goal.force = force
        goal.epsilon = GraspEpsilon(inner=epsilon_inner, outer=epsilon_outer)

        if not cb:
            cb = self._done_cb

        self._grasp_action_client.send_goal(goal,
                                            done_cb=cb,
                                            active_cb=self._active_cb,
                                            feedback_cb=self._feedback_cb)

        if wait_for_result:
            result = self._grasp_action_client.wait_for_result(
                rospy.Duration(15.))
            return result

        return True
Example #2
0
 def graspGripper(self, width, speed=0.5, force=10, epsilon_inner=0.002, epsilon_outer=0.002):
     width_normalized = self._normalize(width, self._MIN_WIDTH, self._MAX_WIDTH)
     force_normalized = self._normalize(force, self._MIN_FORCE, self._MAX_FORCE)
     if self._real_robot:
         # Create goal
         goal = GraspGoal()
         goal.width = width_normalized
         goal.epsilon = GraspEpsilon(epsilon_inner, epsilon_outer)
         goal.speed = speed
         goal.force = force_normalized
         # Send goal
         self._client_grasp.send_goal_and_wait(goal, rospy.Duration.from_sec(self._timeout))
     else:
         self._moveHand(width_normalized)
         print("GRASPING...")
     self._current_width = width_normalized
Example #3
0
    def grasp(
        self,
        width: float,
        force: float,
        speed: float = 0.05,
        epsilon_inner: float = 0.005,
        epsilon_outer: float = 0.005,
        wait_for_result: bool = False,
    ) -> bool:
        """Grasps an object.

        First, the gripper moves with speed `speed` in the direction of `width`:
            - open if param `width` if higher than current gripper width
            - close if param `width` if lower than current gripper width
        Opening/closing stops when a collision is detected (if no collision is
        detected, the action is aborted).
        Then, the force `force` is applied. The force is released as soon as the
        gripper width goes outside the interval
        $[(\text{width} - \text{epsilon_inner}) ; (\text{width} + \text{epsilon_outer})]$.

        Args:
            width (float): Size [m] of the object to grasp.
            force (float): Grasping force [N].
            speed (float, optionnal): Closing speed [m/s]. Default to 0.05.
            epsilon_inner (float, optionnal): Maximum inner tolerated deviation
                [m]. Defaults to 0.005.
            epsilon_outer (float, optionnal): Maximum outter tolerated
                deviation [m]. Defaults to 0.005.

        Returns:
            bool: Whether command was successful.
        """
        width = self._clip_width(width)
        force = self._clip_force(force)
        goal = GraspGoal()
        goal.width = width
        goal.speed = speed
        goal.force = force
        goal.epsilon = GraspEpsilon(inner=epsilon_inner, outer=epsilon_outer)
        self._grasp_action_client.send_goal(goal)
        if wait_for_result:
            return self._grasp_action_client.wait_for_result(
                timeout=rospy.Duration(15.0))
        else:
            return True
Example #4
0
    tf_listener = tf.TransformListener()

    joy_listener = JoyListener()
    pub = rospy.Publisher('/pose_cmd', Pose, queue_size=1)

    client = actionlib.SimpleActionClient('/franka_gripper/grasp', GraspAction)
    client.wait_for_server()

    grasp_goal = GraspGoal()
    grasp_eps  = GraspEpsilon()
    grasp_eps.inner = 0.005
    grasp_eps.outer = 0.005
    grasp_goal.width = 0.1
    grasp_goal.epsilon = grasp_eps
    grasp_goal.speed   = 0.1
    grasp_goal.force   = 0.001

    hz      = 10.0
    dt      = 1.0/hz
    rate    = rospy.Rate(hz)

    pose_cmd = Pose()

    expert_demo = ExpertDemo()

    open_grasp = True

    frames = 0

    while not rospy.is_shutdown():