def test_moveGripperDeltaAR():
    rospy.init_node('gripper_control',anonymous=True)
    listener = tf.TransformListener()
    gripperControl = GripperControlClass(arm, listener)
    imageDetector = ARImageDetectionClass()
    rospy.sleep(4)
    

    currPose = imageDetector.getGripperPose(arm)
    desPose = imageDetector.getObjectPose()

    
    
    deltaPose = Util.deltaPose(currPose, desPose, MyConstants.Frames.Link0, MyConstants.Frames.LeftTool)
    deltaPose.position.z += .03

    #code.interact(local=locals())
    #return
    

    #gripperControl.start()
    
    
    rospy.loginfo('Press enter')
    raw_input()

    
    gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(MyConstants.Frames.Link0), deltaPose)

    rospy.loginfo('exit')
    raw_input()
def test_rotation():
    rospy.init_node('gripper_control',anonymous=True)
    listener = tf.TransformListener()
    tf_br = tf.TransformBroadcaster()
    gripperControl = GripperControlClass(arm, listener)
    imageDetector = ARImageDetectionClass()

    if arm == MyConstants.Arm.Left:
        frame = MyConstants.Frames.Link0
        toolframe = MyConstants.Frames.LeftTool
    else:
        frame = MyConstants.Frames.Link0
        toolframe = MyConstants.Frames.RightTool

    rospy.sleep(3)

    while not (imageDetector.hasFoundGripper(arm) and imageDetector.hasFoundObject()) and not rospy.is_shutdown():
        print imageDetector.hasFoundGripper(arm), imageDetector.hasFoundObject()
        print 'searching'
        rospy.sleep(1)

    print imageDetector.hasFoundGripper(arm), imageDetector.hasFoundObject()

    currPose = imageDetector.getGripperPose(arm)
    desPose = imageDetector.getObjectPose(toolframe)

    code.interact(local=locals())
    """
    desPose = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped()

    
    while not listener.canTransform('tool_L', '0_link', rospy.Time()) and not rospy.is_shutdown():
        rospy.sleep(1)

    time = listener.getLatestCommonTime('tool_L', '0_link')

    print currPose.header.frame_id
    currPose.header.stamp = time
    desPose.header.stamp = time
    desPose.header.frame_id = '0_link'
    currPose = listener.transformPose('tool_L', currPose)
    desPose = listener.transformPose('tool_L', desPose) 
    """

    deltaPose = Util.deltaPose(currPose, desPose)
    deltaPose = tfx.pose([0,0,0],deltaPose.orientation).msg.PoseStamped()
    deltaPoseTb = tfx.tb_angles(deltaPose.pose.orientation)
    #deltaPose = tfx.pose([0,0,0],deltaPose.orientation).msg.Pose()
    print 'deltaPoseTb', deltaPoseTb
    
    """
    gripper_tb = tfx.tb_angles(currPose.pose.orientation)
    obj_tb = tfx.tb_angles(desPose.pose.orientation)
    print 'actual', gripper_tb
    print obj_tb
    deltaPoseTb = tfx.pose(Util.deltaPose(currPose, desPose)).orientation
    print 'deltaPose', deltaPoseTb
    endQuatMat = gripper_tb.matrix * deltaPoseTb
    print 'desOri', tfx.tb_angles(endQuatMat)

    time = listener.getLatestCommonTime('tool_L', '0_link')
    deltaPose.header.frame_id = '0_link'
    deltaPose.header.stamp = time
    deltaPose = listener.transformPose('tool_L', deltaPose)
    #deltaPose = tfx.pose([0,0,0], deltaPose.orientation).msg.Pose()
    deltaPoseTb = tfx.tb_angles(deltaPose.pose.orientation)
    print 'deltaPoseTb', deltaPoseTb
    currPose = tfx.pose(currPose)
    endQuatMat = currPose.orientation.matrix * deltaPoseTb.matrix
    print 'desOri', tfx.tb_angles(endQuatMat)
    """

    yaw = tfx.pose([0,0,0], tfx.tb_angles(deltaPoseTb.yaw_deg,0,0))
    pitch = tfx.pose([0,0,0], tfx.tb_angles(0,deltaPoseTb.pitch_deg,0))
    roll = tfx.pose([0,0,0], tfx.tb_angles(0,0,deltaPoseTb.roll_deg))

    rate = rospy.Rate(1)

    print "press enter"
    raw_input()

    """
    startPose = tfx.pose(gripperControl.getGripperPose(MyConstants.Frames.Link0))
    print 'start', startPose
    endQuatMat = startPose.orientation.matrix * tfx.tb_angles(deltaPose.orientation).matrix
    print tfx.tb_angles(endQuatMat)
    """

    
    gripperControl.start()
    gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), deltaPose, duration=4)
    
    """
    #gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(MyConstants.Frames.Link0), deltaPose, duration=4)
    print 'yaw', yaw
    raw_input()
    gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), yaw, duration=4)

    print 'pitch', pitch
    gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), pitch, duration=4)

    print 'roll', roll
    gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), roll, duration=4)
    """

    while not rospy.is_shutdown():
        rospy.loginfo('loop')
        currPose = tfx.pose(gripperControl.getGripperPose(frame))
        rate.sleep()