class Demo26Master: def __init__(self, limb, name='Demo_26_Master_Leap'): rospy.init_node(name, anonymous=True) self._limb = limb self._knhObj = KinectNiteHelp() self._baeObj = BaxterArmEndpoint(self._limb) self._bbhObj = BaxterButtonHelp(self._limb) self._dmObj = Demo26Help() self._baeObj.startSubscriber() self._lhObj = LeapHelp() self._handCoordinates = [] self._baxterCoordinates = [] self._posCount = 0 rtMatFile = open("RTMatFile.dat", "r") self._rotMat = cPickle.load(rtMatFile) self._transMat = cPickle.load(rtMatFile) self._gCount = 0 self._gOn = 0 self._gPointCount = 0 self._gPoints = [] rtMatFile.close() self._control_arm = baxter_interface.limb.Limb(ARM) self._control_arm.set_joint_position_speed(0.9) def runMaster(self): # Get and store kinect coordinates try: hand_endpoint = self._knhObj.getLeftHandPos() #print hand_endpoint self._handCoordinates = array([[hand_endpoint[0]], [hand_endpoint[1]], [hand_endpoint[2]]]) (rows, cols) = self._rotMat.shape print #print self._rotMat #print self._handCoordinates #print self._transMat self._baxterCoordinates = self._rotMat * mat(self._handCoordinates) + self._transMat print "Hand Position: %s" %(self._baxterCoordinates) #print "Gold Man" except: print "Error in kinect_nite_help part." ## Cd eto set left wo and w1 value try: self._dmObj.setHandPoint(self._baxterCoordinates[0], self._baxterCoordinates[1], self._baxterCoordinates[2]) self._dmObj.calcAngleOrientation() angles = self._dmObj.getAngleOrientation('r') joint_angles = self._control_arm.joint_angles() joint_angles["left_w0"] = angles[0] joint_angles["left_w1"] = angles[1] self._lhObj.processFrame() handEmpty = self._lhObj.getIsHandEmpty() print "Hand Empty: %s" %(handEmpty) if handEmpty == True: fCount = 0 else: fCount = self._lhObj.getNumFingers() print "Finger Count: %s" %(fCount) except: print "Error in joint angle set part" try: # Move arm to space bro #print "Gonna Move" #print joint_angles self._control_arm.move_to_joint_positions(joint_angles, 1.0) except: print "Error in baxter move part"
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
class Demo26Master: def __init__(self, limb, name='Demo_26_Master_Point'): rospy.init_node(name, anonymous=True) self._limb = limb self._knhObj = KinectNiteHelp() self._baeObj = BaxterArmEndpoint(self._limb) self._dmObj = Demo26Help() self._lhObj = LeapHelp() self._handCoordinates = [] self._baxterCoordinates = [] self._posCount = 0 rtMatFile = open("RTMatFile.dat", "r") self._rotMat = cPickle.load(rtMatFile) self._transMat = cPickle.load(rtMatFile) self._gCount = 0 self._gOn = 0 self._gPointCount = 0 self._gPoints = [] rtMatFile.close() def runMaster(self): # Get and store kinect coordinates try: hand_endpoint = self._knhObj.getLeftHandPos() #print hand_endpoint self._handCoordinates = array([[hand_endpoint[0]], [hand_endpoint[1]], [hand_endpoint[2]]]) (rows, cols) = self._rotMat.shape print #print self._rotMat #print self._handCoordinates #print self._transMat self._baxterCoordinates = self._rotMat * mat(self._handCoordinates) + self._transMat print "Hand Position: %s" %(self._baxterCoordinates) #print "Gold Man" except: print "Error in kinect_nite_help part." ## Cd eto set left wo and w1 value try: self._dmObj.setHandPoint(self._baxterCoordinates[0], self._baxterCoordinates[1], self._baxterCoordinates[2]) self._dmObj.calcAngleOrientation() angles = self._dmObj.getAngleOrientation('r') self._lhObj.processFrame() handEmpty = self._lhObj.getIsHandEmpty() print "Hand Empty: %s" %(handEmpty) if handEmpty == True: fCount = 0 else: fCount = self._lhObj.getNumFingers() print "Finger Count: %s" %(fCount) except: print "Error in joint angle set part" try: if (sf.Keyboard.is_key_pressed(sf.Keyboard.L_SHIFT)): if(self._gOn == 0): self._gCount += 1 self._gOn = 1 if (fCount == 0) or (fCount == 2) or (fCount == 4): fCount = fCount + 1 handPoint = [self._baxterCoordinates[0, 0], self._baxterCoordinates[1, 0], self._baxterCoordinates[2, 0], fCount] print "Point Appended" self._gPoints.append(handPoint) print self._gPoints print self._gOn self._gPointCount += 1 else: print "Else" if(self._gOn == 1): self._gPointCount = 0 self._gOn = 0 print "recorded" print "Bitch!" str = "PointData.csv" with open(str, "wb") as f: writer = csv.writer(f) writer.writerows(self._gPoints) #a = numpy.mat(self._gPoints) #str = "PointData" + self._gCount + ".txt" #numpy.savetxt(str, a) self._gPoints = [] except: print "Error in baxter move part"
class Demo26Master: def _callback(self, data): self._startGesture = data.data def __init__(self, limb, name='Demo_26_Final'): rospy.init_node(name, anonymous=True) self._startGesture = '0' self._limb = limb self._knhObj = KinectNiteHelp() self._baeObj = BaxterArmEndpoint(self._limb) self._dmObj = Demo26Help() self._lhObj = LeapHelp() self._handCoordinates = [] self._baxterCoordinates = [] self._posCount = 0 rtMatFile = open("RTMatFile.dat", "r") self._rotMat = cPickle.load(rtMatFile) self._transMat = cPickle.load(rtMatFile) self._gCount = 0 self._gOn = 0 self._gPointCount = 0 self._gPoints = [] self._hmmObjG1 = HMM('G1.hmm') self._hmmObjG2 = HMM('G2.hmm') self._hmmObjG3 = HMM('G3.hmm') self._hmmObjG4 = HMM('G4.hmm') self._flag = '0' self._pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) #self._flagPub = rospy.Publisher('flag_topic', String) self._sub = rospy.Subscriber('/key_tap_topic', String, self._callback) rtMatFile.close() img = cv.LoadImage('Welcome.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(3) def runMaster(self): # Get and store kinect coordinates try: hand_endpoint = self._knhObj.getLeftHandPos() #print hand_endpoint self._handCoordinates = array([[hand_endpoint[0]], [hand_endpoint[1]], [hand_endpoint[2]]]) (rows, cols) = self._rotMat.shape print #print self._rotMat #print self._handCoordinates #print self._transMat self._baxterCoordinates = self._rotMat * mat(self._handCoordinates) + self._transMat print "Hand Position: %s" %(self._baxterCoordinates) #print "Gold Man" except: print "Error in kinect_nite_help part." ## Cd eto set left wo and w1 value try: self._dmObj.setHandPoint(self._baxterCoordinates[0], self._baxterCoordinates[1], self._baxterCoordinates[2]) self._dmObj.calcAngleOrientation() angles = self._dmObj.getAngleOrientation('r') self._lhObj.processFrame() handEmpty = self._lhObj.getIsHandEmpty() print "Hand Empty: %s" %(handEmpty) if handEmpty == True: fCount = 0 else: fCount = self._lhObj.getNumFingers() print "Finger Count: %s" %(fCount) print "GestureState: %s" %(self._startGesture) except: print "Error in joint angle set part" try: if (self._startGesture == '1'): if(self._gOn == 0): self._gCount += 1 self._gOn = 1 img = cv.LoadImage('GestureRecord.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(2) if (fCount == 0) or (fCount == 2) or (fCount == 4): fCount = fCount + 1 handPoint = [self._baxterCoordinates[0, 0], self._baxterCoordinates[1, 0], self._baxterCoordinates[2, 0], fCount] print "Point Appended" self._gPoints.append(handPoint) print self._gPoints print self._gOn self._gPointCount += 1 elif(self._startGesture == '0'): print "Else" if(self._gOn == 1): self._gPointCount = 0 self._gOn = 0 img = cv.LoadImage('GestureAnalyze.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(3) print "recorded" # Process points print "GPoINTS" print self._gPoints symbols = getsym(numpy.matrix(self._gPoints)) log_prob = [self._hmmObjG1.viterbi(symbols), self._hmmObjG2.viterbi(symbols), self._hmmObjG3.viterbi(symbols), self._hmmObjG4.viterbi(symbols)] print log_prob max_value = max(log_prob) max_index = log_prob.index(max_value) self._gPoints = [] # Do gesture recognised processing self._flag = str(max_index + 1) gflag = max_index + 1 if gflag == 1: #while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)): # print max_index + 1 # print log_prob img = cv.LoadImage('Gesture1.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(3) elif gflag == 2: #while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)): # print max_index + 1 # print log_prob img = cv.LoadImage('Gesture2.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(3) elif gflag == 3: #while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)): # print max_index + 1 # print log_prob img = cv.LoadImage('Gesture3.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(3) elif gflag == 4: #while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)): # print max_index + 1 # print log_prob img = cv.LoadImage('Gesture4.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(3) else: #while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)): # print "You are welcome. No gesture detected" # print log_prob img = cv.LoadImage('Welcome.png') msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8") self._pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(3) except Exception,e: print str(e) print "Error in baxter move part"
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