def get_user_hand_adj(grasp, command_pub):
raw_input("Press enter to grasp.")
cur_hand_jts = [
0, grasp["Finger 1(rads)"], grasp["Finger 2(rads)"],
grasp["Finger 3(rads)"], 0, 0, grasp["Spread (rads)"], 0
]
grasp_inc = "0"
while grasp_inc != "q" and grasp_inc != "Q":
grasp_inc = float(grasp_inc)
cur_hand_jts[1] += grasp_inc
cur_hand_jts[2] += grasp_inc
cur_hand_jts[3] += grasp_inc
send_hand_position(command_pub, cur_hand_jts)
#Make sure input is correct and allow them to re-enter or quit
while True:
try:
grasp_inc = raw_input(
"Enter how much to increase closure by (change in radians, or q to quit):"
)
if grasp_inc == "q" or grasp_inc == "Q":
break
else:
float(grasp_inc)
break
except ValueError:
print "Invalid Entry, try again"
return cur_hand_jts
def reset_arm_hand(grasp): send_hand_position(command_pub, [0,0,0,0,0,0,0,0]) adept = [] adept = [-.5,-1, grasp["J position3"], grasp["J position4"], grasp["J position5"], grasp["J position6"] ] set_speed(300) send_adept_joints(adept) set_speed(100) pi_start_up()
def reset_arm_hand(grasp):
send_hand_position(command_pub, [0, 0, 0, 0, 0, 0, 0, 0])
adept = []
adept = [
-.5, -1, grasp["J position3"], grasp["J position4"],
grasp["J position5"], grasp["J position6"]
]
set_speed(300)
send_adept_joints(adept)
set_speed(100)
pi_start_up()
def get_user_hand_adj(grasp, command_pub):
raw_input("Press enter to grasp.")
cur_hand_jts = [0, grasp["Finger 1(rads)"], grasp["Finger 2(rads)"], grasp["Finger 3(rads)"], 0, 0, grasp["Spread (rads)"], 0 ]
grasp_inc = "0"
while grasp_inc != "q" and grasp_inc != "Q":
grasp_inc = float(grasp_inc)
cur_hand_jts[1] += grasp_inc
cur_hand_jts[2] += grasp_inc
cur_hand_jts[3] += grasp_inc
send_hand_position(command_pub, cur_hand_jts)
#Make sure input is correct and allow them to re-enter or quit
while True:
try:
grasp_inc = raw_input("Enter how much to increase closure by (change in radians, or q to quit):")
if grasp_inc == "q" or grasp_inc == "Q":
break
else:
float(grasp_inc)
break
except ValueError:
print "Invalid Entry, try again"
return cur_hand_jts
def get_hand_adj(grasp, command_pub, the_joint_data):
time.sleep(15)
cur_hand_jts = [
0, grasp["Finger 1(rads)"], grasp["Finger 2(rads)"],
grasp["Finger 3(rads)"], 0, 0, grasp["Spread (rads)"], 0
]
send_hand_position(command_pub, cur_hand_jts)
time.sleep(0.3)
# Find which fingers we're supposed to close
close_list = []
close_threshold = 0.05
if grasp["Finger 1(rads)"] > close_threshold:
close_list.append((the_joint_data.get_velocity1, 0))
if grasp["Finger 2(rads)"] > close_threshold:
close_list.append((the_joint_data.get_velocity2, 1))
if grasp["Finger 3(rads)"] > close_threshold:
close_list.append((the_joint_data.get_velocity3, 2))
# Iterate over each finger closing them until each has hit an effort max
grasp_inc = [0, 0, 0]
while len(close_list) > 0:
for f_vel, f_idx in close_list[:]:
print "Current max for finger ", (f_idx + 1), ": ", f_vel()
if f_vel() < .01:
close_list.remove((f_vel, f_idx))
grasp_inc[f_idx] = 0
else:
# The finger needs more closing
grasp_inc[f_idx] = 0.02
# Publish updated finger positions
cur_hand_jts[1] += grasp_inc[0]
cur_hand_jts[2] += grasp_inc[1]
cur_hand_jts[3] += grasp_inc[2]
send_hand_position(command_pub, cur_hand_jts)
time.sleep(.1)
cur_hand_jts[1] += .07
cur_hand_jts[2] += .07
cur_hand_jts[3] += .07
send_hand_position(command_pub, cur_hand_jts)
return cur_hand_jts
def get_hand_adj(grasp, command_pub, the_joint_data): time.sleep(15) cur_hand_jts = [0, grasp["Finger 1(rads)"], grasp["Finger 2(rads)"], grasp["Finger 3(rads)"], 0, 0, grasp["Spread (rads)"], 0 ] send_hand_position(command_pub, cur_hand_jts) time.sleep(0.3) # Find which fingers we're supposed to close close_list = [] close_threshold = 0.05 if grasp["Finger 1(rads)"] > close_threshold: close_list.append( (the_joint_data.get_velocity1, 0) ) if grasp["Finger 2(rads)"] > close_threshold: close_list.append( (the_joint_data.get_velocity2, 1) ) if grasp["Finger 3(rads)"] > close_threshold: close_list.append( (the_joint_data.get_velocity3, 2) ) # Iterate over each finger closing them until each has hit an effort max grasp_inc = [0,0,0] while len(close_list) > 0: for f_vel, f_idx in close_list[:]: print "Current max for finger ", (f_idx + 1), ": ", f_vel() if f_vel() < .01 : close_list.remove( (f_vel, f_idx) ) grasp_inc[f_idx] = 0 else: # The finger needs more closing grasp_inc[f_idx] = 0.02 # Publish updated finger positions cur_hand_jts[1] += grasp_inc[0] cur_hand_jts[2] += grasp_inc[1] cur_hand_jts[3] += grasp_inc[2] send_hand_position(command_pub, cur_hand_jts) time.sleep(.1) cur_hand_jts[1] += .07 cur_hand_jts[2] += .07 cur_hand_jts[3] += .07 send_hand_position(command_pub, cur_hand_jts) return cur_hand_jts
def automatic_hand_close(command_pub, hand_adj):
time.sleep(
15) #No feed back to know if arm is in postion so we wait to grasp
send_hand_position(command_pub, hand_adj)
time.sleep(1.2)
def automatic_hand_close(command_pub, hand_adj):
time.sleep(15) #No feed back to know if arm is in postion so we wait to grasp
send_hand_position(command_pub, hand_adj )
time.sleep(1.2)
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
import hand_control
if __name__ == "__main__":
rospy.init_node("open_barrett_hand")
command_pub = rospy.Publisher("bhand_node/command",
JointState,
queue_size=100,
latch=True)
while True:
command = raw_input("Pleae enter a command (o - open, q - quit): ")
if command.lower() == "o":
hand_control.send_hand_position(command_pub,
[0, 0, 0, 0, 0, 0, 0, 0])
elif command.lower() == "q":
break
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
import hand_control
if __name__ == "__main__":
rospy.init_node("open_barrett_hand")
command_pub = rospy.Publisher("bhand_node/command", JointState, queue_size=100, latch=True)
while True:
command = raw_input("Pleae enter a command (o - open, q - quit): ")
if command.lower() == "o":
hand_control.send_hand_position(command_pub, [0,0,0,0,0,0,0,0])
elif command.lower() == "q":
break
