Example usage for org.apache.commons.math3.geometry.euclidean.threed RotationConvention VECTOR_OPERATOR

Introduction

In this page you can find the example usage for org.apache.commons.math3.geometry.euclidean.threed RotationConvention VECTOR_OPERATOR.

Prototype

RotationConvention VECTOR_OPERATOR

To view the source code for org.apache.commons.math3.geometry.euclidean.threed RotationConvention VECTOR_OPERATOR.

Click Source Link

Document

Constant for rotation that have the semantics of a vector operator.

Usage

From source file:com.diozero.sandpit.imu.invensense.MPU9150DriverTest.java

public void run() {
    try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7,
            MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) {
        mpuInit(mpu);//from www  .  ja v a2 s. com
        System.err.println("Ready.");

        do {
            ImuData imu_data = update(mpu);
            System.out.print("Got IMU data: compass=[" + imu_data.getCompass() + "], temp="
                    + imu_data.getTemperature() + ", gyro=[" + imu_data.getGyro() + "], accel=["
                    + imu_data.getAccel() + "], quat=[" + imu_data.getQuaternion().getQ0() + ", "
                    + imu_data.getQuaternion().getQ1() + ", " + imu_data.getQuaternion().getQ2() + ", "
                    + imu_data.getQuaternion().getQ3() + "], timestamp=" + imu_data.getTimestamp() + ", ");

            Quaternion q = imu_data.getQuaternion();
            //double[] ypr = q.toEuler();
            //double[] ypr = quat.getYawPitchRoll();
            Rotation r = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true);
            double[] ypr = null;
            try {
                ypr = r.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR);
            } catch (CardanEulerSingularityException e) {
                ypr = new double[] { 2 * Math.atan2(q.getQ1(), q.getQ0()), Math.PI / 2, 0 };
                System.out.print("Singularity detected, ");
            }
            Logger.info("ypr=[{}, {}, {}]", Double.valueOf(ypr[0]), Double.valueOf(ypr[1]),
                    Double.valueOf(ypr[2]));
        } while (true);

    } catch (RuntimeIOException ioe) {
        Logger.error(ioe, "Error: {}", ioe);
    }
}

---

From source file:com.diozero.sandpit.imu.invensense.MPU9150DriverMqttPublisher.java

public void run() {
    try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7,
            MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) {
        mpuInit(mpu);/*ww  w . j av a2  s . c  o m*/
        mqttInit();
        System.err.println("Ready.");

        do {
            ImuData imu_data = update(mpu);
            System.out.print("Got IMU data: compass=[" + imu_data.getCompass() + "], temp="
                    + imu_data.getTemperature() + ", gyro=[" + imu_data.getGyro() + "], accel=["
                    + imu_data.getAccel() + "], quat=[" + imu_data.getQuaternion().getQ0() + ", "
                    + imu_data.getQuaternion().getQ1() + ", " + imu_data.getQuaternion().getQ2() + ", "
                    + imu_data.getQuaternion().getQ3() + "], timestamp=" + imu_data.getTimestamp() + ", ");

            Quaternion q = imu_data.getQuaternion();
            //double[] ypr = q.toEuler();
            //double[] ypr = quat.getYawPitchRoll();
            Rotation r = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true);
            double[] ypr = null;
            try {
                ypr = r.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR);
            } catch (CardanEulerSingularityException e) {
                ypr = new double[] { 2 * Math.atan2(q.getQ1(), q.getQ0()), Math.PI / 2, 0 };
                System.out.print("Singularity detected, ");
            }
            Logger.info("ypr=[{}, {}, {}]", Double.valueOf(ypr[0]), Double.valueOf(ypr[1]),
                    Double.valueOf(ypr[2]));

            mqttPublish(imu_data, ypr);
        } while (true);

    } catch (RuntimeIOException ioe) {
        Logger.error(ioe, "Error: {}", ioe);
    } catch (MqttException me) {
        Logger.error(me, "Error: {}", me);
    } finally {
        try {
            mqttClient.disconnect();
        } catch (Exception e) {
        }
    }
}

---