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
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
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
rospy.init_node('joy_interface')
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