def getReadyPosition():
center_after_pickup_position = [
0.032, -0.573, 0.25, 3.108, 0.069, -1.556
]
send_coords(center_after_pickup_position, ABSOLUTE)
main_fingers(f_opening)
def goToAckPos():
main_fingers(f_opening)
acknowledgementPos = [0.027, -0.472, 0.53, 1.614, 0.0059, 0.171]
send_coords(acknowledgementPos, ABSOLUTE)
main_fingers(f_closing)
main_fingers(f_opening)
main_fingers(f_closing)
main_fingers(f_opening)
def goToAckPos():
main_fingers(f_opening)
acknowledgementPos = [0.035, -0.663, 0.342, 1.596, 0.023, 1.527]
send_coords(acknowledgementPos, ABSOLUTE)
main_fingers(f_closing)
main_fingers(f_opening)
main_fingers(f_closing)
main_fingers(f_opening)
def thumbsUp():
main_fingers(f_opening)
acknowledgementPos = [-0.016, -0.663, 0.342, 1.596, 0.023, 1.527]
send_coords(acknowledgementPos, ABSOLUTE)
main_fingers(f_closing)
main_fingers(f_opening)
main_fingers(f_closing)
def getReadyPosition():
print "Moving to ready position..."
center_after_pickup_position = [
0.016, -0.573, 0.25, 3.108, 0.069, -1.556
]
send_coords(center_after_pickup_position, ABSOLUTE)
main_fingers(f_opening)
def getReadyPosition():
center_after_pickup_position = [
0.032, -0.573, 0.25, 3.108, 0.069, -1.556
]
send_coords(center_after_pickup_position, ABSOLUTE)
#testRotateGripper = [0.0, 0.0, 0.0, 0.0, 0.0, 0.5]
#send_coords(testRotateGripper, RELATIVE)
main_fingers(f_opening)
def handObjecToHuman(old_pos):
send_coords(hoverPos[old_pos], ABSOLUTE)
send_coords(pos_dict[old_pos], ABSOLUTE)
main_fingers(f_closing)
objectToHuman_coords = [-0.037, -0.675, 0.264, 1.436, -0.063, -1.338]
send_coords(objectToHuman_coords, ABSOLUTE)
def endGesture():
objectToHuman_coords = [-0.037, -0.675, 0.264, 1.436, -0.063, -1.338]
send_coords(objectToHuman_coords, ABSOLUTE)
objectToHuman_coords = [-0.037, -0.675, 0.164, 1.436, -0.063, -1.338]
send_coords(objectToHuman_coords, ABSOLUTE)
objectToHuman_coords = [-0.037, -0.675, 0.364, 1.436, -0.063, -1.338]
send_coords(objectToHuman_coords, ABSOLUTE)
objectToHuman_coords = [-0.037, -0.675, 0.264, 1.436, -0.063, -1.338]
send_coords(objectToHuman_coords, ABSOLUTE)
main_fingers(f_closing)
main_fingers(f_opening)
main_fingers(f_closing)
def pickAndPlace(old_pos, new_pos):
print "HERE"
print old_box_pos
print new_box_pos
if old_box_pos != new_box_pos:
print "Should go in here"
# Pick up box at old box position
send_coords(hoverPos[old_pos], ABSOLUTE)
send_coords(pos_dict[old_pos], ABSOLUTE)
main_fingers(f_closing)
# Drop box in new position
send_coords(hoverPos[new_pos], ABSOLUTE)
send_coords(pos_dict[new_pos], ABSOLUTE)
main_fingers(f_opening)
return new_box_pos
else:
return old_box_pos
new_box_pos = getDir(headMovement)
#new_box_pos = raw_input("Write your desired drop point(C,N,S,E,W,NE,NW,SE,SW)...")
print "Desired drop point is " + str(new_box_pos)
if new_box_pos == 'X':
break
elif new_box_pos == 'I':
initialize = 1
elif new_box_pos == old_box_pos:
print "Box is already at position " + str(new_box_pos)
else:
# Pick up box at old box position
#send_coords([0.0, 0.0, 0.10, 0.0, 0.0, 0.0], RELATIVE)
send_coords(hoverPos[old_box_pos], ABSOLUTE)
send_coords(pos_dict[old_box_pos], ABSOLUTE)
main_fingers(f_closing)
#send_coords([0.0, 0.0, 0.10, 0.0, 0.0, 0.0], RELATIVE)
print hoverPos[new_box_pos]
print pos_dict[new_box_pos]
# Drop box in new position
send_coords(hoverPos[new_box_pos], ABSOLUTE)
send_coords(pos_dict[new_box_pos], ABSOLUTE)
main_fingers(f_opening)
send_coords(hoverPos[new_box_pos], ABSOLUTE)
old_box_pos = new_box_pos
# Decide how to grab it (either from the right, or from the top > get it closer > from the right)
method = set_grabbing(handle_direction, JACO_position)
if method == RIGHT: #from the RIGHT
item = set_orientation(x_axis, y_axis, grid_right, item_position)
goal = set_orientation(x_axis, y_axis, grid_right, goal_position)
elif method == TOP: #from the TOP
angle = rotation_angle(handle_direction)
angle_v = [0.0, 0.0, 0.0, 0.0, 0.0, angle]
item = set_orientation(x_axis, y_axis, grid_top, item_position)
item_UP = item
item_UP[2] = item[2] + 0.1
send_coords(item_UP, ABSOLUTE)
send_coords([0.0, 0.0, -0.1, 0.0, 0.0, 0.0], RELATIVE)
f_closing = adjust_fingers_closing_size(0.100) # closing
main_fingers(f_closing)
send_joints(angle_v)
main_fingers([0, 0, 0])
item_right = set_orientation(x_axis, y_axis, grid_right, item_position)
send_coords(item_right, ABSOLUTE)
main_fingers(f_closing)
def homePosition():
print "Go to Home Position"
# Move to home position
send_coords(pos_dict['home'], ABSOLUTE)
main_fingers(f_opening)