def test():
rospy.init_node('test',anonymous=True)
rp = RavenPlanner(MyConstants.Arm.Both)
rospy.sleep(4)
rp.updateOpenraveJoints(MyConstants.Arm.Left)
poseMatInRef = np.eye(4)
refLinkName = MyConstants.Frames.LeftTool
targLinkName = MyConstants.Frames.Link0
# ref -> world
refFromWorld = rp.robot.GetLink(refLinkName).GetTransform()
# target -> world
targFromWorld = rp.robot.GetLink(targLinkName).GetTransform()
# target -> ref
targFromRef = np.dot(np.linalg.inv(targFromWorld), refFromWorld)
poseMatInTarg = np.dot(targFromRef, poseMatInRef)
pose = tfx.pose(poseMatInTarg, frame=targLinkName)
rospy.loginfo('publishing')
pub = rospy.Publisher('left_tool_rave',PoseStamped)
while not rospy.is_shutdown():
pub.publish(pose.msg.PoseStamped())
rospy.sleep(.5)
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()
def testFK():
rospy.init_node('test_IK',anonymous=True)
rp = RavenPlanner(MyConstants.Arm.Both)
rp.env.SetViewer('qtcoin')
rospy.sleep(2)
leftPose = rp.getCurrentPose(MyConstants.Arm.Left)
rightPose = rp.getCurrentPose(MyConstants.Arm.Right)
leftGrasp = rp.getCurrentGrasp(MyConstants.Arm.Left)
rightGrasp = rp.getCurrentGrasp(MyConstants.Arm.Right)
leftJoints = invArmKin(MyConstants.Arm.Left, leftPose, leftGrasp)
rightJoints = invArmKin(MyConstants.Arm.Right, rightPose, rightGrasp)
rp.updateOpenraveJoints(MyConstants.Arm.Left, joints1=leftJoints, grasp=leftGrasp)
rp.updateOpenraveJoints(MyConstants.Arm.Right, joints1=rightJoints, grasp=rightGrasp)
raveLeftMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, MyConstants.Frames.Link0)
raveRightMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, MyConstants.Frames.Link0)
raveLeftPose = tfx.pose(raveLeftMatrix,frame=leftPose.frame)
raveRightPose = tfx.pose(raveRightMatrix,frame=rightPose.frame)
deltaLeft = Util.deltaPose(leftPose, raveLeftPose)
deltaRight = Util.deltaPose(rightPose, raveRightPose)
g = []
#g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, 'world'))
#g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, 'world'))
g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, leftPose.matrix, leftPose.frame[1:], 'world'))
g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, rightPose.matrix, rightPose.frame[1:], 'world'))
IPython.embed()
