def getBaxterJointSpace(self, limb = 'right'): # All orientation calclulations voObj = VectorOrientation() voObj.setVectorChange(0.00, 0.00, -1.0) # X, Y and Z Vector Axis Orientation #voObj.setAngleOrientation(-20, 0, 0, 'd') voObj.calcVectorOrientation() # Set gamma in degrees voObj.setGamma(180, 'd') # Trick for baxter orientation change for rigth arm: voObj.setAngleOrientation(-voObj._theta, voObj._phi, voObj._gamma, 'r') # Calculate quaternion quat = voObj.getQuaternionOrientation() # Inverse Kinematics arm = IKSolveBaxter(limb) arm.setArmEndPos(self._del_arm_x, self._del_arm_y, self._del_arm_z, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); self._joints_loc = arm.getBaxterJointSpace() arm.setArmEndPos(self._del_arm_x, self._del_arm_y, self._del_arm_z + 0.10, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); self._inter_joints_loc1 = arm.getBaxterJointSpace() arm.setArmEndPos(self._del_arm_x, self._del_arm_y, self._del_arm_z + 0.15, quat[0], quat[1], quat[2], quat[3]) arm.ik_solve(); self._inter_joints_loc2 = arm.getBaxterJointSpace() return self._joints_loc, self._inter_joints_loc1, self._inter_joints_loc2
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"