def move(move_pos=110, move_speed=50):
#move pos range: 0 - 110 mm
#move speed range: 5- 450 mm/s
ack()
command = 'move'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
#time.sleep(0.05)
while True:
try:
error = srv(move_pos, move_speed)
print '[Gripper] move, return:', error
break
except:
print '[Gripper] move,', ErrorMessage, command
# publish to joint state publisher for visualization without real hand
jnames = [
'wsg_50_gripper_base_joint_gripper_left',
'wsg_50_gripper_base_joint_gripper_right'
]
js = sensor_msgs.msg.JointState()
js.name = jnames
js.position = [-move_pos / 2.0 / 1000.0, move_pos / 2.0 / 1000.0]
exec_joint_pub.publish(js)
if not toRetry: break
time.sleep(0.5)
def set_force(val = 5):
command = 'set_force'
srv=rospy.ServiceProxy('/wsg_50_driver/%s' % command, Conf)
try:
error = srv(val)
except:
print '[Gripper] set_force,', ErrorMessage, command
def grasp(move_pos=108, move_speed=80, grasp_type = 'default'):
ack()
command = 'grasp'
srv=rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
while True:
try:
if not havegripper:
raise NoGripperException('No gripper')
error = srv(move_pos, move_speed)
print '[Gripper] grasp %d, return:' % move_pos, error
break
except NoGripperException:
print '[Gripper] grasp,', ErrorMessage, command
# publish to joint state publisher for visualization without real hand
jnames = ['wsg_50_gripper_base_joint_gripper_left', 'wsg_50_gripper_base_joint_gripper_right']
js = sensor_msgs.msg.JointState()
js.name = jnames
if grasp_type == 'default':
js.position = [-move_pos / 2.0 / 1000.0, move_pos / 2.0 / 1000.0]
elif grasp_type == 'in':
js.position = [0,0]
elif grasp_type == 'out':
move_pos = 108
js.position = [-move_pos / 2.0 / 1000.0, move_pos / 2.0 / 1000.0]
exec_joint_pub.publish(js)
if not toRetry: break
time.sleep(5)
def nailControl(openNail=True, maxOpen=False):
# openNail: pointing straight forward if true, else fold away
# maxOpen: max opening possible
# TODO: remove or replace next 3 lines once we have an interface with the nails
ack()
command = 'move'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
#time.sleep(0.05)
while True:
try:
error = srv(move_pos, move_speed)
print '[Nails] move, return:', error
break
except:
print '[Nails] move,', ErrorMessage, command
# publish to joint state publisher for visualization without real hand
jnames = ['joint_nail_right', 'joint_nail_left']
js = sensor_msgs.msg.JointState()
js.name = jnames
if openNail:
js.position = [0.0, 0.0]
if maxOpen:
js.position = [-0.17453292519, -0.17453292519]
if not openNail:
js.position = [3.14159265359, 3.14159265359]
exec_joint_pub.publish(js)
if not toRetry: break
time.sleep(0.5)
def release(move_pos=109, move_speed=50):
ack()
command = 'release'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
while True:
try:
if not havegripper:
raise NoGripperException('No gripper')
error = srv(move_pos, move_speed)
print '[Gripper] release, return:', error
break
except NoGripperException:
print '[Gripper] release,', ErrorMessage, command
# publish to joint state publisher for visualization without real hand
jnames = [
'wsg_50_gripper_base_joint_gripper_left',
'wsg_50_gripper_base_joint_gripper_right'
]
js = sensor_msgs.msg.JointState()
js.name = jnames
js.position = [-move_pos / 2.0 / 1000.0, move_pos / 2.0 / 1000.0]
exec_joint_pub.publish(js)
if not toRetry: break
time.sleep(5)
def ack(self):
command = 'ack'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Empty)
try:
error = srv()
except:
pass
def set_force(val=5):
command = 'set_force'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Conf)
try:
error = srv(val)
except:
print '[Gripper] set_force,', ErrorMessage, command
def ack():
command = 'ack'
srv=rospy.ServiceProxy('/wsg_50_driver/%s' % command, Empty)
try:
error = srv()
except:
print '[Gripper] ack,', ErrorMessage, command
def move(move_pos=110, move_speed=50):
#move pos range: 0 - 110 mm
#move speed range: 5- 450 mm/s
ack()
command = 'move'
srv=rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
#time.sleep(0.05)
while True:
try:
error = srv(move_pos, move_speed)
print '[Gripper] move, return:', error
break
except:
print '[Gripper] move,', ErrorMessage, command
# publish to joint state publisher for visualization without real hand
jnames = ['wsg_50_gripper_base_joint_gripper_left', 'wsg_50_gripper_base_joint_gripper_right']
js = sensor_msgs.msg.JointState()
js.name = jnames
js.position = [-move_pos / 2.0 / 1000.0, move_pos / 2.0 / 1000.0]
exec_joint_pub.publish(js)
if not toRetry: break
time.sleep(0.5)
def ack():
command = 'ack'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Empty)
try:
error = srv()
except:
print '[Gripper] ack,', ErrorMessage, command
def homing():
ack()
command = 'homing'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Empty)
try:
error = srv()
except:
print '[Gripper] homing,', ErrorMessage, command
def set_force(self, val=5):
# Range: 1 to 80 (N)
command = 'set_force'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Conf)
try:
error = srv(val)
except:
pass
def homing(self):
self.ack()
command = 'homing'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Empty)
try:
error = srv()
except:
pass
def homing():
ack()
command = 'homing'
srv=rospy.ServiceProxy('/wsg_50_driver/%s' % command, Empty)
try:
error = srv()
except:
print '[Gripper] homing,', ErrorMessage, command
def ack():
command = 'ack'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Empty)
while True:
try:
if not havegripper: raise NoGripperException('No gripper')
error = srv()
break
except NoGripperException:
print '[Gripper] ack,', ErrorMessage, command
if not toRetry: break
time.sleep(0.5)
def set_force(val=5):
command = 'set_force'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Conf)
while True:
try:
error = srv(val)
break
except:
print '[Gripper] set_force,', ErrorMessage, command
if not toRetry: break
time.sleep(0.5)
def grasp(move_pos=10, move_speed=80):
ack()
command = 'grasp'
srv=rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
while True:
try:
error = srv(move_pos, move_speed)
print '[Gripper] grasp, return:', error
break
except:
print '[Gripper] grasp,', ErrorMessage, command
if not toRetry: break
time.sleep(0.5)
def grasp(move_pos=10, move_speed=80):
ack()
command = 'grasp'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
while True:
try:
error = srv(move_pos, move_speed)
print '[Gripper] grasp, return:', error
break
except:
print '[Gripper] grasp,', ErrorMessage, command
if not toRetry: break
time.sleep(0.5)
def release(self, pos=110, speed=80):
if speed is None:
speed = self.speed
self.ack()
command = 'release'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
while True:
try:
error = srv(pos, speed)
# print '[Gripper] release, return:', error
break
except:
pass
# print '[Gripper] release,', ErrorMessage, command
if not toRetry: break
time.sleep(0.5)
def move(move_pos=110, move_speed=50):
move_pos = max(min(move_pos, 110), 0)
#move pos range: 0 - 110 mm
#move speed range: 5- 450 mm/s
ack()
command = 'move'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
#~publish command to impact_time is contact was not detected
if (rospy.get_param('is_contact')
== False) and (rospy.get_param('is_record')
== True) and havegripper:
impact_pub = rospy.Publisher('/impact_time',
Bool,
queue_size=10,
latch=False)
impact_msgs = Bool()
impact_msgs.data = False
impact_pub.publish(impact_msgs)
while True:
try:
if not havegripper:
raise NoGripperException('No gripper')
error = srv(move_pos, move_speed)
print '[Gripper] move, return:', error
break
except NoGripperException:
print '[Gripper] move,', ErrorMessage, command
# publish to joint state publisher for visualization without real hand
jnames = [
'wsg_50_gripper_base_joint_gripper_left',
'wsg_50_gripper_base_joint_gripper_right'
]
js = sensor_msgs.msg.JointState()
js.name = jnames
js.position = [-move_pos / 2.0 / 1000.0, move_pos / 2.0 / 1000.0]
exec_joint_pub.publish(js)
if not toRetry: break
time.sleep(0.5)
def move(self, pos=110, speed=None, toRetry=False):
# move pos range: 0 - 110 mm
# move speed range: 0- 420 mm/s
if speed is None:
speed = self.speed
self.ack()
command = 'move'
srv = rospy.ServiceProxy('/wsg_50_driver/%s' % command, Move)
while True:
try:
error = srv(pos, speed)
# print '[Gripper] move, return:', error
break
except:
pass
# print '[Gripper] move,', ErrorMessage, command
if not toRetry: break
time.sleep(0.5)
