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 __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 __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)
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 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 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"
