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 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()