def set_gripper_state(self, jval, gripper, wait=False):
""" Sets goal for indicated gripper (r_gripper/l_gripper) using provided joint angle"""
# Build trajectory message
if not (type(jval) is list):
jval = [jval]
if len(jval) == 0:
rospy.logwarn("No %s_gripper goal given" % gripper[0])
if gripper[0] == "l":
self.l_gripper_done = True
else:
self.r_gripper_done = True
return
if gripper[0] == "l":
client = self.l_gripper_client
self.l_gripper_done = False
elif gripper[0] == "r":
client = self.r_gripper_client
self.r_gripper_done = False
goal = Pr2GripperCommandGoal()
goal.command.max_effort = -1
goal.command.position = jval[0]
if gripper[0] == "l":
client.send_goal(goal, done_cb=self.__l_gripper_done_cb)
elif gripper[0] == "r":
client.send_goal(goal, done_cb=self.__r_gripper_done_cb)
if wait:
client.wait_for_result()
def _send_gripper_command(self,
pos=DEFAULT_GRIPPER_POSITION,
eff=DEFAULT_GRIPPER_EFFORT,
wait=DEFAULT_GRIPPER_WAIT):
'''Sets the position of the gripper.
Args:
pos (float, optional): The position ofthe gripper to move
to. Close is GRIPPER_CLOSE_POSITION, open is
GRIPPER_OPEN_POSITION. Defaults to
DEFAULT_GRIPPER_POSITION.
eff (float, optional): How much effort to exert when moving
the gripper to its position. Units are in PSI (I think).
Defaults to DEFAULT_GRIPPER_EFFORT.
wait (bool, optional): Whether to wait for the gripper to
complete its goal before returning. Defaults to
DEFAULT_GRIPPER_WAIT.
'''
goal = Pr2GripperCommandGoal()
goal.command.position = pos
goal.command.max_effort = eff
self.gripper_client.send_goal(goal)
if wait:
self.gripper_client.wait_for_result(
rospy.Duration(MAX_GRIPPER_WAIT_TIME))
def grip(self, position, max_effort):
goal = Pr2GripperCommandGoal()
command = Pr2GripperCommand()
command.position = position
command.max_effort = max_effort
goal.command = command
self.GCA.send_goal(goal)
def move_hand(positions, blocking=True):
client = actionlib.SimpleActionClient(
rospy.get_param('gripper_controller_name', 'l_gripper_controller') +
'/gripper_action', Pr2GripperCommandAction)
if len(positions) != 1:
return False, "Wrong joint number", positions
if not client.wait_for_server():
success = False
reason = 'failed to connect to action server'
return success, reason, positions
rospy.loginfo("Connected to Finger server")
client.send_goal(
Pr2GripperCommandGoal(
Pr2GripperCommand(position=positions[0], max_effort=-1)))
try:
client.wait_for_result()
except KeyboardInterrupt:
rospy.loginfo("Program interrupted, pre-empting goal")
client.cancel_all_goals()
success = False
reason = 'Program interrupted from keyboard'
return success, reason, positions
result = client.get_result()
positions = [result.position]
success = True
reason = None
return success, reason, positions
def moveGrip(p = 0.08, arm_side = 'r'):
""" Open/close the gripper
"""
open = Pr2GripperCommandGoal()
open.command.position = p
open.command.max_effort = -1.0
r_g_controller.send_goal(open)
def gripper_controll(self, position, max_effort=-1):
goal = Pr2GripperCommandGoal()
goal.command.position = position
goal.command.max_effort = max_effort
return self.gripper_client.send_goal_and_wait(
goal,
execute_timeout=rospy.Duration(2.0),
preempt_timeout=rospy.Duration(0.1))
def _send_gripper_command(self, pos=0.08, eff=30.0, wait=True):
'''Sets the position of the gripper'''
command = Pr2GripperCommandGoal()
command.command.position = pos
command.command.max_effort = eff
self.gripper_client.send_goal(command)
if wait:
self.gripper_client.wait_for_result(rospy.Duration(5.0))
def open_close_gripper(arm):
if arm is "right":
service = "r_gripper_controller/gripper_action"
else:
service = "l_gripper_controller/gripper_action"
client = actionlib.SimpleActionClient(service, Pr2GripperCommandAction)
client.wait_for_server()
goal = Pr2GripperCommandGoal()
goal.command.position = 0.08
goal.command.max_effort = -1 # open fast.
client.send_goal(goal)
client.wait_for_result()
time.sleep(5)
goal = Pr2GripperCommandGoal()
goal.command.position = 0.0
goal.command.max_effort = 50 # close slowly
client.send_goal(goal)
client.wait_for_result()
def gripper_action(self,
position,
max_effort=-1.0,
block=False,
timeout=20.0):
"""Send goal to gripper action server"""
goal = Pr2GripperCommandGoal(Pr2GripperCommand(position, max_effort))
self.def_gripper_ac.send_goal(goal)
if block:
return self.def_gripper_ac.wait_for_result(rospy.Duration(timeout))
def __init__(self, side=None):
rospy.init_node('gripper_client')
self._client = actionlib.SimpleActionClient(
'/' + side + '_gripper_controller/gripper_action',
Pr2GripperCommandAction)
self._goal = Pr2GripperCommandGoal()
if not self._client.wait_for_server(rospy.Duration(10.0)):
rospy.logerr('The gripper server is not connected')
rospy.signal_shutdown("Action Server not found")
import sys
sys.exit(1)
def set_gripper(gripper, pos, wait=True, max_effort=25):
msg = Pr2GripperCommandGoal()
msg.command.position = pos
msg.command.max_effort = max_effort
if gripper.lower() == 'l':
l_gripper_client.send_goal(msg)
if wait:
l_gripper_client.wait_for_result()
elif gripper.lower() == 'r':
r_gripper_client.send_goal(msg)
if wait:
r_gripper_client.wait_for_result()
def moveGripper(whichgripper, position, effort):
gripperGoal = Pr2GripperCommandGoal()
gripperGoal.command.position = position
gripperGoal.command.max_effort = effort
print 'sending gripper command goal'
if whichgripper == 'l':
l_gripper_action_client.send_goal(gripperGoal)
l_gripper_action_client.wait_for_result()
elif whichgripper == 'r':
r_gripper_action_client.send_goal(gripperGoal)
r_gripper_action_client.wait_for_result()
else:
print whichgripper, 'needs to be either l or r'
def main():
rospy.init_node("load_tool")
from optparse import OptionParser
p = OptionParser()
p.add_option('-w', '--wait', dest="wait", default=6, help="Set wait time.")
p.add_option('-r',
'--relax',
dest="relax",
default=False,
action="store_true",
help="Set the gripper torque to 0.")
p.add_option('-t',
'--tighten',
dest="tighten",
default=False,
action="store_true",
help="Set the gripper torque to 30.")
(opts, args) = p.parse_args()
g_client = actionlib.SimpleActionClient(
'l_gripper_controller/gripper_action', Pr2GripperCommandAction)
g_client.wait_for_server()
g_goal = Pr2GripperCommandGoal()
if opts.relax:
g_goal.command.position = 0
g_goal.command.max_effort = 0
g_client.send_goal(g_goal)
#g_client.wait_for_result()
return
if opts.tighten:
g_goal.command.position = 0
g_goal.command.max_effort = 30
g_client.send_goal(g_goal)
g_client.wait_for_result()
return
g_goal.command.position = 1
g_goal.command.max_effort = -1
g_client.send_goal(g_goal)
g_client.wait_for_result()
rospy.sleep(float(opts.wait))
g_goal.command.position = 0
g_goal.command.max_effort = 30
g_client.send_goal(g_goal)
g_client.wait_for_result()
def command_gripper(self,
arm,
position,
max_effort=MAX_EFFORT,
timeout=2.0,
blocking=True):
goal = Pr2GripperCommandGoal()
goal.command.position = position
goal.command.max_effort = max_effort
client = "%s_gripper" % arm
return self._send_command(client,
goal,
blocking=blocking,
timeout=timeout)
def command_gripper(self, position, max_effort, blocking=0):
if not self.gripper_controller_state == 'running':
self.check_controller_states()
if not self.gripper_controller_state == 'running':
rospy.logwarn(
"gripper controller not running! Attempting to start")
self.start_gripper_controller()
goal = Pr2GripperCommandGoal()
goal.command.position = position
goal.command.max_effort = max_effort
self.gripper_action_client.send_goal(goal)
# if blocking, wait for the gripper to finish
if blocking:
self.gripper_action_client.wait_for_result(rospy.Duration(4.0))
time.sleep(.5)
def open_right_gripper(self):
right_gripper_client = actionlib.SimpleActionClient(
"l_gripper_controller/gripper_action", Pr2GripperCommandAction)
print("waiting for server")
right_gripper_client.wait_for_server()
print("server active")
goal = Pr2GripperCommandGoal()
# goal.command.position = 0.0
# goal.command.max_effort = 50.0
goal.command.position = 0.08
goal.command.max_effort = -1.0
return right_gripper_client.send_goal_and_wait(goal,
rospy.Duration(30.0),
rospy.Duration(5.0))
def openGripper(self, a):
'''
Opens the gripper using the given simpleActionClient(Either /l_gripper_controller/gripper_action or
/r_gripper_controller/gripper_action). Specified the size of the operation to be, which is the distance between the two fingertips to be 0.08(max is 0.99 and min 0.00). The max_effort to be -1.0. This field places a limit on the amount of effort (force in N) to apply while moving to that position. If 'max_effort' is negative, it is ignored.
@param a: simpleActionClient which must take Pr2GripperCommandGoal as a goal
@todo : make it take the max_effort and position as parameter
'''
rospy.loginfo("Waiting for open gripper action")
a.wait_for_server()
goalGripper = Pr2GripperCommandGoal()
goalGripper.command.position = 0.08
goalGripper.command.max_effort = -1.0
rospy.loginfo("Opening gripper")
a.send_goal_and_wait(goalGripper)
return
def spin(self):
l_client = actionlib.SimpleActionClient('l_gripper_controller/gripper_action', Pr2GripperCommandAction)
r_client = actionlib.SimpleActionClient('r_gripper_controller/gripper_action', Pr2GripperCommandAction)
l_client.wait_for_server()
r_client.wait_for_server()
goal = Pr2GripperCommandGoal()
goal.command.position = 0.0
goal.command.max_effort = 100.0
while not rospy.is_shutdown():
if self.goal_received:
l_client.send_goal(goal)
r_client.send_goal(goal)
l_client.wait_for_result()
r_client.wait_for_result()
rospy.sleep(1)
def gripper_action(self,
position,
max_effort=-1,
blocking=False,
block_timeout=20.0):
print "Performing Gripper Action"
if self.gripper_action_ac:
command = Pr2GripperCommand(position, max_effort)
goal = Pr2GripperCommandGoal(command)
self.gripper_action_ac.send_goal(goal)
if blocking:
return self.gripper_action_ac.wait_for_result(
rospy.Duration(block_timeout))
else:
self.def_gripper_ac.send_goal(
Pr2GripperCommand(position, max_effort))
if blocking:
return self.def_gripper_ac.wait_for_result(
rospy.Duration(block_timeout))
def close(self, a):
'''
Close the gripper using the given simpleActionClient(Either /l_gripper_controller/gripper_action or
/r_gripper_controller/gripper_action). Specified the size of the operation to be, which is
the distance between the two fingertips to be 0.00(max is 0.99 and min 0.00). The max_effort set to be
50.0. This field places a limit on the amount of effort (force in N) to apply while moving to that
position.
@param a: simpleActionClient which must take Pr2GripperCommandGoal as a goal
@todo : make it take the max_effort and position as parameter
'''
rospy.loginfo("Waiting for close gripper action")
a.wait_for_server()
squeeze = Pr2GripperCommandGoal()
squeeze.command.position = 0.0
squeeze.command.max_effort = 50.0
rospy.loginfo("Closing gripper")
a.send_goal_and_wait(squeeze)
return
def grip(self,
openGripper=True,
maxEffort=200.0,
rightArm=True,
miniOpen=False,
stopMovement=False):
msg = Pr2GripperCommandGoal()
if stopMovement:
pos = rospy.wait_for_message('/r_gripper_controller/state',
JointControllerState).process_value
msg.command.position = pos
else:
msg.command.position = 0.1 if openGripper else (
0.005 if miniOpen else 0.0)
msg.command.max_effort = maxEffort if stopMovement or not openGripper else -1.0
if rightArm:
self.rightGripperClient.send_goal(msg)
else:
self.leftGripperClient.send_goal(msg)
def run_demo(self):
try:
if not self.published:
point = PointStamped()
pos = 0.8
pos2 = 0.4
pos3 = 0.0
goal = Pr2GripperCommandGoal()
goal.command.max_effort = 30
goal.command.position = pos
goal2 = Pr2GripperCommandGoal()
goal2.command.max_effort = 30
goal2.command.position = pos
self.ph.send_goal(goal)
self.ph2.send_goal_and_wait(goal2)
goal = Pr2GripperCommandGoal()
goal.command.max_effort = 30
goal.command.position = pos2
goal2 = Pr2GripperCommandGoal()
goal2.command.max_effort = 30
goal2.command.position = pos2
self.ph.send_goal(goal)
self.ph2.send_goal_and_wait(goal2)
goal = Pr2GripperCommandGoal()
goal.command.max_effort = 30
goal.command.position = pos3
goal2 = Pr2GripperCommandGoal()
goal2.command.max_effort = 30
goal2.command.position = pos3
self.ph.send_goal(goal)
self.ph2.send_goal_and_wait(goal2)
print goal
rospy.sleep(1.)
self.published = True
rospy.sleep(0.1)
except KeyboardInterrupt:
print('interrupted!')
sys.exit(0)
def gripper(self, position, effort=30):
pos = np.clip(position, 0.0, 0.09)
goal = Pr2GripperCommandGoal()
goal.command.position = pos
goal.command.max_effort = effort
self.gripper_client.send_goal(goal)
finished_within_time = self.gripper_client.wait_for_result(
rospy.Duration(15))
if not (finished_within_time):
self.gripper_client.cancel_goal()
rospy.loginfo("Timed out moving " + self.arm + " gripper")
return False
else:
state = self.gripper_client.get_state()
result = self.gripper_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Gripper Action Succeeded")
return True
else:
rospy.loginfo("Gripper Action Failed")
rospy.loginfo("Failure Result: %s" % result)
return False
def release(self,
event=0,
acc_thresh=3,
slip_thresh=0.005,
blocking=False,
block_timeout=20):
print "Performing Gripper Release"
if self.release_ac:
release_event = PR2GripperEventDetectorCommand()
release_event.trigger_conditions = event
release_event.acceleration_trigger_magnitude = acc_thresh
release_event.slip_trigger_magnitude = slip_thresh
release_command = PR2GripperReleaseCommand(release_event)
self.release_ac.send_goal(PR2GripperReleaseGoal(release_command))
if blocking:
return self.release_ac.wait_for_result(
rospy.Duration(block_timeout))
else:
self.def_gripper_ac.send_goal(
Pr2GripperCommandGoal(Pr2GripperCommand(0.09, -1.0)))
if blocking:
return self.def_gripper_ac.wait_for_result(
rospy.Duration(block_timeout))
def moveGripperPR2(self, dist=0.08, effort = 15): self.gripper_action_client.send_goal(Pr2GripperCommandGoal(Pr2GripperCommand(position=dist, max_effort = effort)))
def close_gripper(self, arm, effort=15):
self.gripper_action_client.send_goal(
Pr2GripperCommandGoal(
Pr2GripperCommand(position=0.0, max_effort=effort)))
def open_gripper(self, arm):
self.gripper_action_client.send_goal(
Pr2GripperCommandGoal(
Pr2GripperCommand(position=0.08, max_effort=-1)))
def command_gripper(self, arm, position, max_effort, blocking, timeout):
goal = Pr2GripperCommandGoal()
goal.command.position = position
goal.command.max_effort = max_effort
client = "%s_gripper" % arm
return self.send_command("%s_gripper" % arm, goal, blocking, timeout)
def move_gripper(self, arm, amount=0.08, effort=15):
self.gripper_action_client[arm].send_goal(
Pr2GripperCommandGoal(
Pr2GripperCommand(position=amount, max_effort=effort)))
def l_action_gripper(self, value=0):
goal = Pr2GripperCommandGoal() #GripperCommandGoal()
goal.command.position = value
goal.command.max_effort = 40
self.lgripper.send_goal_and_wait(goal)