def main(): rospy.init_node("Surf_2014_Final") lhObj = LeapHelp() initial_num_tools = int(raw_input("Enter number of tools on table:")) left_untuck = {"left_w0":-0.08, "left_w1":-1.0, "left_w2":-1.19, "left_e0":1.94, "left_e1":0.67, "left_s0":1.03, "left_s1":-0.50} fo = open('left_del_cam_pos.dat', "r") del_limb = cPickle.load(fo) #del_limb = 'left' left_del_pos = cPickle.load(fo) cam_right_pos = cPickle.load(fo) fo.close() fo = open('left_high_pos.dat', "r") del_limb = cPickle.load(fo) left_high_pos = cPickle.load(fo) fo.close() fo = open('left_ret_pos.dat', "r") ret_limb = cPickle.load(fo) left_ret_del_pos = cPickle.load(fo) fo.close() posFile = open("BaxterCamArmJointPosFile.dat", "r") cam_arm_joint_pos = cPickle.load(posFile) posFile.close() posFile = open("BaxterRetArmJointPosFile.dat", "r") ret_arm_joint_pos = cPickle.load(posFile) # Ontable, moveCamArm above table, left_ret_del_pos:retrieval dleivery position posFile.close() ret_back_img = cv2.imread("RetrieveImage.png") ret_arm_cam = moveGetCamArmImage(limb = 'left', file_name = "RetrieveBaxterCamArmJointPosFile.dat") mvObj = moveGetCamArmImage('right') mvObj.moveCamArm() img = mvObj.getCamArmImage() ppObj = PixelToBaxterPosTransformer() ppObj.setOffsets(0.0, 0.019, -0.025) # Figuring out smin smin = getSMin(img.copy(), initial_num_tools, 10) timgObj = ToolImageProcessHelp() timgObj.setImage(img, 'Original') timgObj.setSMin(smin) #timgObj.processImage_BackProject() timgObj.processImage() num = timgObj.getNumTools() h = timgObj.getHuMomentsList() c = timgObj.getCentroidsList() baxter_img = timgObj.getContourImage() baxter_img = cv2.resize(baxter_img, (1024, 600)) send_image(baxter_img) print "SMIN:%s" % (smin) ToolList = [] for i in range(num): tool = ToolIdentify() tool.setHuMoments([ h[0,i], h[1,i], h[2,i], h[3,i], h[4,i], h[5,i], h[6,i], h[7,i] ]) tool.setCentroid(c[0, i], c[1, i]) tool.identifyTool() ToolList.append(tool) #print "Tool #%s: %s" % (i+1, tool.getToolName()) last_ret_time = time.time() key_pos = 0 just_delivered = False while(num > 0): just_delivered = False for i in range(num): print "Tool #%s: %s" % (i+1, ToolList[i].getToolName()) ##############LEAP STUFF######################################## index = -1 lhObj.processFrame() print "Fingers: %s" % (lhObj.getNumFingers()) if(lhObj.getKeyTapDetected() == True): print "Extend Fingers:" key_pos = key_pos + 1 key_pos = key_pos % 2 time.sleep(2) lhObj.processFrame() finger_count = lhObj.getNumFingers() if (finger_count) in range(1, num+1): print "Tool Available" print finger_count index = int(finger_count) - 1 else: print"Selected tool unavaialable" time.sleep(1) ################################################################ #index = raw_input("Enter index of tool to deliver:") #index = int(index) - 1 # Because tools are printed from 1 onwards but indexing starts from 0 if(index != -1): ppObj.calcBaxterPos(ToolList[index]._centroidX, ToolList[index]._centroidY) (joints_loc, inter_joints_loc1, inter_joints_loc2) = ppObj.getBaxterJointSpace(del_limb) print "Delivering %s..." % (ToolList[index].getToolName()) just_delivered = True del(ToolList[index]) num = num - 1 #############Chaneg right to left in cam_arm_joint_pos################ #dummy = [] #for i in cam_arm_joint_pos.keys(): # new_key = i.replace('right', 'left') # dummy.append(new_key) #cam_right_pos = dict(zip(dummy, cam_arm_joint_pos.values())) ######################################################################## #Move Delivery Arm control_arm = baxter_interface.limb.Limb(del_limb) control_arm.set_joint_position_speed(0.5) # Joint movement speed control_arm.move_to_joint_positions(left_high_pos, 7.0) control_arm.move_to_joint_positions(inter_joints_loc1, 7.0) time.sleep(1) control_arm.move_to_joint_positions(joints_loc, 7.0) time.sleep(1) control_arm.move_to_joint_positions(inter_joints_loc2, 7.0) time.sleep(1) control_arm.move_to_joint_positions(left_del_pos, 7.0) time.sleep(3) control_arm.move_to_neutral(timeout = 7) ret_arm_cam.moveCamArm() ############################################################################################ tool_detected = False if(time.time() > 15 + last_ret_time) and (just_delivered == False): print "Checking retrieval tray" ret_arm_cam.moveCamArm() icObj = ImageClipper('RetrieveImageClipPosFile.dat') per_white = 100.0 count = 0 while (per_white >= 18.0 or per_white <= 2.0) and count <= 3: img = ret_arm_cam.getCamArmImage() img = icObj.clipImage(img) per_white, binary_img = getForeBack(ret_back_img, img) print "Waiting for next iteration" print count += 1 if per_white >= 2.0 and per_white <= 15.0: print "TOOL PRESENT" tool_detected = True else: print "NO TOOL" tool_detected = False last_ret_time = time.time() ########## ########### if tool_detected == True: # Move Retrieval Arm control_arm = baxter_interface.limb.Limb(ret_limb) control_arm.set_joint_position_speed(0.5) # Joint movement speed ret_arm_cam.moveCamArm() time.sleep(1) control_arm.move_to_joint_positions(ret_arm_joint_pos, 7.0) time.sleep(1) ret_arm_cam.moveCamArm() time.sleep(1) control_arm.move_to_joint_positions(left_ret_del_pos, 7.0) time.sleep(1) ret_arm_cam.moveCamArm() ############################################################################################ return 0
def main(): rospy.init_node("Demo_Jul_22") color_num = {'1':'blue', '2':'green', '3':'yellow', '4':'red'} color = 'black' place_num = 0 used_nums = [0, 5] # Fingers not to be processed posFile = open('RestPosFile.dat', "r") dummy_load = cPickle.load(posFile) rest_pos = cPickle.load(posFile) posFile.close() print "Initial Calibration" ll = baxter_interface.limb.Limb("left") rl = baxter_interface.limb.Limb("right") lg = baxter_interface.gripper.Gripper("left") rg = baxter_interface.gripper.Gripper("right") ll.set_joint_position_speed(0.9) rl.set_joint_position_speed(0.9) lg.calibrate() rg.calibrate() lg.open() rg.open() ll.move_to_neutral(5) rl.move_to_neutral(5) key_pos = 0 lhObj = LeapHelp() color = 'black' place_num = 0 send_image('DemoJuly22Images/DemoJul22Registering.png') while(place_num <= 3): lhObj.processFrame() print "Fingers: %s" % (lhObj.getNumFingers()) if(lhObj.getKeyTapDetected() == True): send_image('DemoJuly22Images/DemoJul22ColorSelect.png') print "Extend Fingers:" key_pos = key_pos + 1 key_pos = key_pos % 2 time.sleep(2) lhObj.processFrame() finger_count = lhObj.getNumFingers() if finger_count not in used_nums: used_nums.append(finger_count) color = color_num[str(finger_count)] image_name = 'DemoJuly22Images/DemoJul22' + color + '.png' send_image(image_name) print "FingerCount:" print finger_count print "Color:" print color place_num += 1 print "Place Num:" print place_num else: send_image('DemoJuly22Images/DemoJul22AnotherColor.png') print"Selected color has alrady been assembled" time.sleep(1) if color == 'black': continue ll = baxter_interface.limb.Limb("left") rl = baxter_interface.limb.Limb("right") lg = baxter_interface.gripper.Gripper("left") rg = baxter_interface.gripper.Gripper("right") ll.set_joint_position_speed(0.9) rl.set_joint_position_speed(0.9) lg.calibrate() rg.calibrate() lg.open() rg.open() ll.move_to_neutral(5) rl.move_to_neutral(5) send_image('DemoJuly22Images/DemoJul22PickingParts.png') print "Moving Left Arm" (j1, j2) = get_positions(color, 'left') ll.set_joint_position_speed(0.7) ll.move_to_neutral(timeout = 5.0) ll.move_to_joint_positions(j1, 5.0) ll.set_joint_position_speed(0.5) ll.move_to_joint_positions(j2, 3.0) lg.close() ll.move_to_joint_positions(j1, 3.0) ll.set_joint_position_speed(0.7) ll.move_to_neutral(timeout = 5.0) print "Moving Right Arm" (j1, j2) = get_positions(color, 'right') rl.set_joint_position_speed(0.5) rl.move_to_neutral(timeout = 5.0) rl.move_to_joint_positions(j1, 5.0) rl.set_joint_position_speed(0.3) rl.move_to_joint_positions(j2, 3.0) rg.close() rl.move_to_joint_positions(j1, 3.0) rl.set_joint_position_speed(0.5) rl.move_to_neutral(timeout = 5.0) send_image('DemoJuly22Images/DemoJul22Assembly.png') #time.sleep(1) os.system("rosrun baxter_examples joint_position_file_playback.py -f Assemble ") time.sleep(1) rg.open() time.sleep(1) rl.move_to_neutral(timeout = 5.0) send_image('DemoJuly22Images/DemoJul22PlacingProduct.png') #time.sleep(1) command = "rosrun baxter_examples joint_position_file_playback.py -f Place" + str(place_num) os.system(command) lg.open() time.sleep(1) ll.set_joint_position_speed(0.5) ll.move_to_joint_positions(rest_pos, 5.0) ll.set_joint_position_speed(0.7) ll.move_to_neutral(timeout = 5.0) color = 'black' if key_pos == 1: color = 'black' send_image('DemoJuly22Images/DemoJul22Registering.png') key_pos = 0 rate = rospy.Rate(10) print "Demo Complete" send_image('DemoJuly22Images/DemoJul22Welcome.png') return 0