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"
class CalibBaxterKinect: def __init__(self, limb): # self._limb = limb self._knObj = KinectNiteHelp() self._baeObj = BaxterArmEndpoint(self._limb) self._bbhObj = BaxterButtonHelp(self._limb) self._baeObj.startSubscriber() self._posCount = 0 self._rotMat = [] self._transMat = [] self._handCoordinates = [0, 0, 0] self._baxterCoordinates = [0, 0, 0] def openingStatement(self): # Display opening statement with instructions for caliberation print "Baxter Kinect Calibration Program" print "Press circle button to start calibration when Baxter arm is in desired position." print "Press long button hen user hand is in desired position" print "Press big button on Baxter arm to calculate rotation and translation matrix" print def calibrate(self): buttonStates = self._bbhObj.getButtonStates() if(buttonStates["cState"]): print "Registered Baxter Hand Pos." print "Move arm to desired pos and press long button on cuff." buttonStates = self._bbhObj.getButtonStates() while(buttonStates["lState"] == False): buttonStates = self._bbhObj.getButtonStates() continue # Get and store baxter coordinates baxter_endpoint = self._baeObj.getArmEndPos() baxterTempPos = [baxter_endpoint["x"], baxter_endpoint["y"], baxter_endpoint["z"]] self._baxterCoordinates = vstack((self._baxterCoordinates, baxterTempPos)) # Get and store kinect coordinates try: hand_endpoint = self._knObj.getLeftHandPos() handTempPos = [hand_endpoint[0], hand_endpoint[1], hand_endpoint[2]] self._handCoordinates = vstack((self._handCoordinates, handTempPos)) except: print "Error in kinect_nite_help part." print "Baxter and Kinect Pos %d added." % (self._posCount + 1) print "Current Baxter Matrix:" print self._baxterCoordinates print "Current Hand Matrix" print self._handCoordinates self._posCount = self._posCount + 1 elif(buttonStates["bState"]): if(self._posCount >= 4): tempBaxterMat = mat(self._baxterCoordinates[1:]) tempHandMat = mat(self._handCoordinates[1:]) print tempBaxterMat print tempHandMat print "Calculate rotation and transalation matrix" self._rotMat, self._transMat = svd_rt(tempHandMat, tempBaxterMat) print "Rotation matrix:" print self._rotMat print "Translation matrix:" print self._transMat rtMatFile = open("RTMatFile.dat", "w"); cPickle.dump(self._rotMat, rtMatFile) cPickle.dump(self._transMat, rtMatFile); rtMatFile.close() print "Rotation and Translation Matrices Stored To File" else: print "Not Enough Points to perform SVD. You need %d more points" % (3 - self._posCount)
class MasterBaxterProgram: def __init__(self, limb, name='master_baxter_program'): rospy.init_node(name, anonymous=True) self._limb = limb self._knhObj = KinectNiteHelp() self._baeObj = BaxterArmEndpoint(self._limb) self._bbhObj = BaxterButtonHelp(self._limb) self._iksbObj = IKSolveBaxter(self._limb) self._baeObj.startSubscriber() self._handCoordinates = [] self._baxterCoordinates = [] self._posCount = 0 rtMatFile = open("RTMatFile.dat", "r") self._rotMat = cPickle.load(rtMatFile) self._transMat = cPickle.load(rtMatFile) 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 self._baxterCoordinates print "Gold Man" except: print "Error in kinect_nite_help part." try: self._iksbObj.setArmEndPos(self._baxterCoordinates[0, 0], self._baxterCoordinates[1, 0], self._baxterCoordinates[2, 0], -0.140368694896, 0.9899737577306, -0.0098400631999, 0.0248863560409) #self._iksbObj.setArmEndPos(0.5, 0, 0, -0.140368694896, 0.9899737577306, -0.0098400631999, 0.0248863560409) print self._iksbObj._armEndPos print "ArmPosSet" except: print "Error in ik Set part" try: print joints = {} self._iksbObj._validJointSolutionFound = False self._iksbObj.ik_solve(); joints = self._iksbObj.getBaxterJointSpace() print joints except: print "Error in ik Solve part" try: # Move arm to space bro self._control_arm.move_to_joint_positions(joints, 1.0) except: print "Error in Baxter Movement Part"