def testPoseDiff(arm=Constants.Arm.Left):
rospy.init_node('testPoseDiff',anonymous=True)
t = Test(arm)
ravenArm = RavenArm(arm)
ravenPlanner = RavenPlanner(arm)
rospy.loginfo('Press enter to start recording')
raw_input()
t.startRecording()
rospy.loginfo('Press enter to stop recording')
raw_input()
t.stopRecording()
rospy.loginfo('Press enter to print poses')
raw_input()
t.printPoses()
rospy.loginfo('Press enter to compare joints')
raw_input()
tapePose = t.tapePoses[-1]
ravenJoints = ravenArm.getCurrentJoints()
ikJoints = ravenPlanner.getJointsFromPose(tapePose)
for jointType in ravenPlanner.rosJointTypes:
ravenJoint = (180.0/math.pi)*ravenJoints[jointType]
ikJoint = (180.0/math.pi)*ikJoints[jointType]
print('JointType = {0}'.format(jointType))
print('ravenJoint = {0}'.format(ravenJoint))
print('ikJoint = {0}'.format(ikJoint))
print('Joint difference = {0}\n'.format(abs(ravenJoint-ikJoint)))
rospy.loginfo('Press enter to exit')
raw_input()
