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)