def grasp_client():
client = actionlib.SimpleActionClient('/franka_gripper/grasp', GraspAction)
print("waiting for server")
client.wait_for_server()
eps = GraspEpsilon()
eps.inner = 0.001
eps.outer = 0.001
goal = GraspGoal(width=0.001, epsilon=eps, speed=0.08, force=70.0)
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
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 btn_left_gripper_close_on_click(self):
force = self._widget.spinbox_left_gripper_force.value()
length = self._widget.spinbox_left_gripper_width.value()
epsilon = GraspEpsilon(inner=0.027, outer=0.027)
req = GraspGoal(width=length, epsilon=epsilon, speed=0.1, force=force)
if self.panda_gripper_grasp_client.wait_for_server(
rospy.Duration(1.0)):
self.panda_gripper_grasp_client.send_goal(req)
self._widget.textBrowser.append("LEFT: gripper_close | force: " +
str(force) + " | length: " +
str(length))
else:
self._widget.textBrowser.append(
"SERVER NOT WORKING -- LEFT: gripper_close")
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 gripper_grasp(self,
width,
epsilon_inner=0.05,
epsilon_outer=0.05,
speed=0.1,
force=60):
epsilon = GraspEpsilon(inner=epsilon_inner, outer=epsilon_outer)
goal = GraspGoal(width=width,
epsilon=epsilon,
speed=speed,
force=force)
rospy.loginfo('Grasping:\n{}'.format(goal))
self._gripper_grasp.send_goal(goal)
self._gripper_grasp.wait_for_result()
while not self._gripper_grasp.get_result().success:
rospy.logerr('Failed to grasp, retry')
self.gripper_move(width=0.025)
self._gripper_grasp.send_goal(goal)
self._gripper_grasp.wait_for_result()
if rospy.is_shutdown():
sys.exit(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
def close_gripper():
"""Closes robot gripper
If using ``real_panda`` then call move gripper action server. If using gazebo then publish desired
grip width to */franka/gripper_width* topic.
Returns:
bool: returns True when done.
"""
if real_panda:
client = actionlib.SimpleActionClient('/franka_gripper/grasp', GraspAction)
rospy.loginfo("CONNECTING")
client.wait_for_server()
action = GraspGoal(width=0.5,speed=0.08,force=1)
rospy.loginfo("SENDING ACTION")
client.send_goal(action)
client.wait_for_result(rospy.Duration.from_sec(5.0))
rospy.loginfo("DONE")
else:
pub_gripper.publish(0.05)
rospy.sleep(2)
return True
char = screen.getch()
if char == ord('q'):
break
elif char == curses.KEY_RIGHT:
# print doesn't work with curses, use addstr instead
screen.addstr(0, 0, 'open gripper (right)')
if (set_width <= max_open):
client = actionlib.SimpleActionClient('/franka_gripper/move',
MoveAction)
client.wait_for_server()
#set_width += step_size
set_width = release
goal = MoveGoal(width=set_width, speed=0.04)
client.send_goal(goal)
elif char == curses.KEY_LEFT:
screen.addstr(0, 0, 'close gripper (left)')
if (set_width >= min_open):
client = actionlib.SimpleActionClient('/franka_gripper/grasp',
GraspAction)
client.wait_for_server()
#set_width -= step_size
set_width = grasp
action = GraspGoal(width=set_width, speed=0.04, force=1)
client.send_goal(action)
elif char == curses.KEY_UP:
curses.nocbreak()
screen.keypad(0)
curses.echo()
curses.endwin()
loop = False
file_dir = os.path.dirname(os.path.abspath(__file__)) + '/data/expert_demo/demo.pkl'
logger = {'inputs': [], 'outputs' : []}
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()