Esempio n. 1
0
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()
Esempio n. 3
0
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 
Esempio n. 5
0
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
Esempio n. 7
0
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