Java OpenGLMatrix.getTranslation方法代码示例

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入方法依赖的package包/类
public VectorF getVuMarkPosition()
{
    VectorF targetPos = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetPos = pose.getTranslation();
            robot.tracer.traceInfo("TargetPos", "%s: x=%6.2f, y=%6.2f, z=%6.2f",
                                   vuMark.toString(),
                                   targetPos.get(0)/RobotInfo.MM_PER_INCH,
                                   targetPos.get(1)/RobotInfo.MM_PER_INCH,
                                   -targetPos.get(2)/RobotInfo.MM_PER_INCH);
        }
    }

    return targetPos;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入方法依赖的package包/类
public void updateLocation(){
    OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.getListener()).getPose();
    telemetry.addData("Pose", format(pose));

    /* We further illustrate how to decompose the pose into useful rotational and
    * translational components */
    if (pose != null) {
        VectorF trans = pose.getTranslation();
        Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);

        // Extract the X, Y, and Z components of the offset of the target relative to the robot
        tX = trans.get(0);
        tY = trans.get(1);
        tZ = trans.get(2);

        // Extract the rotational components of the target relative to the robot
        rX = rot.firstAngle;
        rY = rot.secondAngle;
        rZ = rot.thirdAngle;
        //Z is forward-backward
        //x is sideways
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入方法依赖的package包/类
private VectorF getVuMarkPosition()
{
    VectorF targetPos = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetPos = pose.getTranslation();
        }
    }

    return targetPos;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入方法依赖的package包/类
@Override
    public void runPeriodic(double elapsedTime)
    {
        double left = gamepad.getLeftStickY(true);
        double right = gamepad.getRightStickY(true);
        driveBase.tankDrive(left, right);

        final int LABEL_WIDTH = 180;
        dashboard.displayPrintf(1, LABEL_WIDTH, "Power(L/R) = ", "%.2f/%.2f", left, right);
        dashboard.displayPrintf(2, LABEL_WIDTH, "GyroHeading = ", "%.2f", gyro.getZHeading().value);
        dashboard.displayPrintf(3, LABEL_WIDTH, "SoundEnvelope = ", "%s", envelopeToggle.getState()? "ON": "OFF");
        dashboard.displayPrintf(4, LABEL_WIDTH, "ToneDevice = ", "%s",
                                analogToneToggle.getState()? "AnalogOut": "Android");
        dashboard.displayPrintf(5, LABEL_WIDTH, "Vision = ", "%s (%d)", visionEnabled, targetIndex);

        if (isVisionEnabled())
        {
            boolean followTarget = followTargetToggle.getState();

            for (int i = 0; i < targets.length; i++)
            {
                VuforiaTrackable target = vuforia.getTarget(i);
                boolean visible = vuforia.isTargetVisible(target);

                if (USE_SPEECH)
                {
                    if (visible != targetsFound[i])
                    {
                        targetsFound[i] = visible;
                        String sentence = String.format(
                                "%s is %s.", target.getName(), visible ? "in view" : "out of view");
                        textToSpeech.speak(sentence, TextToSpeech.QUEUE_FLUSH, null);
                    }
                }

                if (visible)
                {
                    String label = String.format(i == targetIndex ? "<%s> = " : "%s = ", target.getName());
                    OpenGLMatrix pose = vuforia.getTargetPose(target);
                    if (pose != null)
                    {
                        VectorF translation = pose.getTranslation();
                        float targetX = translation.get(0) / MM_PER_INCH;
                        float targetY = translation.get(1) / MM_PER_INCH;
                        float targetZ = -translation.get(2) / MM_PER_INCH;

                        dashboard.displayPrintf(
                                i + 5, LABEL_WIDTH, label, "%6.2f,%6.2f,%6.2f", targetX, targetY, targetZ);

                        if (followTarget && i == targetIndex)
                        {
//                            targetDistance = targetZ;
                            targetDistance = 24.0;
                            // TODO: doesn't work! Event won't fire until distance is satisfied.
                            if (visionEvent.isSignaled())
                            {
                                targetHeading = Math.toDegrees(Math.atan2(targetX, targetZ));
                                visionEvent.set(false);
                                visionPidDrive.setTarget(24.0, targetHeading, false, visionEvent);
                            }
                        }
                    }

                    OpenGLMatrix robotLocation = vuforia.getRobotLocation(target);
                    if (robotLocation != null)
                    {
                        lastKnownRobotLocation = robotLocation;
                    }
                }
            }

            if (lastKnownRobotLocation != null)
            {
                dashboard.displayPrintf(9, LABEL_WIDTH, "RobotLoc = ", lastKnownRobotLocation.formatAsTransform());
            }
        }
    }
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入方法依赖的package包/类
@Override
public void runPeriodic(double elapsedTime)
{
    final int LABEL_WIDTH = 120;

    for (int i = 0; i < targets.length; i++)
    {
        VuforiaTrackable target = vuforia.getTarget(i);
        boolean visible = vuforia.isTargetVisible(target);

        if (SPEECH_ENABLED)
        {
            if (visible != targetsFound[i])
            {
                targetsFound[i] = visible;
                String sentence = String.format(
                        "%s is %s.", target.getName(), visible? "in view": "out of view");
                textToSpeech.speak(sentence, TextToSpeech.QUEUE_FLUSH, null);
            }
        }

        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            VectorF translation = pose.getTranslation();
            dashboard.displayPrintf(
                    i + 1, LABEL_WIDTH, target.getName() + " = ", "%6.2f,%6.2f,%6.2f",
                    translation.get(0)/MM_PER_INCH,
                    translation.get(1)/MM_PER_INCH,
                    -translation.get(2)/MM_PER_INCH);
        }

        OpenGLMatrix robotLocation = vuforia.getRobotLocation(target);
        if (robotLocation != null)
        {
            lastKnownRobotLocation = robotLocation;
        }
    }

    if (lastKnownRobotLocation != null)
    {
        dashboard.displayPrintf(5, LABEL_WIDTH, "RobotLoc = ", lastKnownRobotLocation.formatAsTransform());
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入方法依赖的package包/类
public void updateLocation(){

        OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.getListener()).getPose();

        telemetry.addData("Pose", format(pose));



                    /* We further illustrate how to decompose the pose into useful rotational and

                 * translational components */

        if (pose != null) {

            VectorF trans = pose.getTranslation();

            Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);



            // Extract the X, Y, and Z components of the offset of the target relative to the robot

            tX = trans.get(0);

            tY = trans.get(1);

            tZ = trans.get(2);



            // Extract the rotational components of the target relative to the robot

            rX = rot.firstAngle;

            rY = rot.secondAngle;

            rZ = rot.thirdAngle;

            // Z is forward-backward
            //x is sideways
        }

    }