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"
