public static Quaternion nextQuaternion(Random random, double minMaxAngleRange) { AxisAngle orientation = nextAxisAngle(random, minMaxAngleRange); Quaternion quat = new Quaternion(); quat.set(orientation); return quat; }
public MocapRigidBody(int id, Vector3D position, Quaternion orientation, ArrayList<MocapMarker> listOfAssociatedMarkers, boolean isTracked) { this.id = id; this.xPosition = (float) position.getX(); this.yPosition = (float) position.getY(); this.zPosition = (float) position.getZ(); this.qw = (float) orientation.getS(); this.qx = (float) orientation.getX(); this.qy = (float) orientation.getY(); this.qz = (float) orientation.getZ(); this.listOfAssociatedMarkers = listOfAssociatedMarkers; this.dataValid = isTracked; }
private void setYawPitchRoll() { Quaternion q = new Quaternion(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). q.setYawPitchRoll(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue()); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.multiply(qprev); q_qs.set(q.getS()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
private void corruptOrientation(Quaternion orientation) { Vector3D axis = RandomGeometry.nextVector3D(random); double angle = RandomNumbers.nextDouble(random, -maxRotationCorruption, maxRotationCorruption); AxisAngle axisAngle4d = new AxisAngle(); axisAngle4d.set(axis, angle); Quaternion corruption = new Quaternion(); corruption.set(axisAngle4d); orientation.multiply(corruption); }
private RigidBodyTransform[] createYawOnlyCorrectionTargets(Random random, int numTargets) { RigidBodyTransform[] targets = new RigidBodyTransform[numTargets]; Quaternion rot = new Quaternion(); for (int i = 0; i < 10; i++) { targets[i] = new RigidBodyTransform(); rot.setYawPitchRoll((random.nextDouble() * Math.PI * 2) - Math.PI, 0, 0); targets[i].setRotation(rot); } return targets; }
@Override public Quaternion initialValue() { return new Quaternion(); } };
@Override protected void copy(Quaternion src, Quaternion dest) { dest.set(src); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testSetQuaternionBasedOnMatrix3d() { Random random = new Random(1776L); Quaternion unitQuaternion = new Quaternion(0.0, 0.0, 0.0, 1.0); int numberOfTests = 100000; for (int i = 0; i < numberOfTests; i++) { Quaternion randomQuaternion = RandomGeometry.nextQuaternion(random); RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.set(randomQuaternion); Quaternion quaternionToPack = new Quaternion(rotationMatrix); quaternionToPack.multiplyConjugateOther(randomQuaternion); if (quaternionToPack.getS() < 0.0) quaternionToPack.negate(); boolean quaternionsAreEpsilonEquals = unitQuaternion.epsilonEquals(quaternionToPack, 1e-7); assertTrue(quaternionsAreEpsilonEquals); } }
public FootstepPlanState(double x, double y, double theta, RobotSide side) { footstepData.getLocation().set(new Point3D(x, y, 0)); Quaternion orientation = new Quaternion(); orientation.setToYawQuaternion(theta); footstepData.getOrientation().set(orientation); footstepData.setRobotSide(side.toByte()); this.theta = theta; }
private Quaternion computeQuat1Quat2Quat1Conjugate(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion1Inverse = new Quaternion(quaternion1); quaternion1Inverse.inverse(); Quaternion quaternion1TimesQuaternion2 = new Quaternion(); quaternion1TimesQuaternion2.multiply(quaternion1, quaternion2); Quaternion quaternion1TimesQuaternion2TimesQuaternion1Inverse = new Quaternion(); quaternion1TimesQuaternion2TimesQuaternion1Inverse.multiply(quaternion1TimesQuaternion2, quaternion1Inverse); return quaternion1TimesQuaternion2TimesQuaternion1Inverse; }
@Test public void testSingularitiesYawPitchRoll() throws Exception { Quaternion quaternion = new Quaternion(); quaternion.setYawPitchRoll(0.5 * Math.PI, 0.0, 1e-9 - 0.5 * Math.PI); double[] yawPitchRoll = new double[3]; quaternion.getYawPitchRoll(yawPitchRoll); } }
private void computeAngularVelocity(Vector3D angularVelocityToPack, Quaternion startRotationQuaternion, Quaternion endRotationQuaternion, double alphaDot) { if (startRotationQuaternion.dot(endRotationQuaternion) < 0.0) endRotationQuaternion.negate(); // compute relative orientation: orientation of interpolated frame w.r.t. start frame relativeRotationQuaternion.set(startRotationQuaternion); // R_W_S: orientation of start w.r.t. world relativeRotationQuaternion.conjugate(); // R_S_W: orientation of world w.r.t. start relativeRotationQuaternion.multiply(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); endRotationQuaternion.transform(angularVelocityToPack); } }
public void initialize(DenseMatrix64F accel, DenseMatrix64F pqr, double heading) { setMatrix(bias, pqr); // euler = accel2euler(accel, heading); accel2euler(accel, heading); Quaternion quaternion = new Quaternion(); quaternion.setYawPitchRoll(eulerAngles); quaternion.get(q); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate() throws Exception { for (int i = 0; i < 1000; i++) { double yaw = RandomNumbers.nextDouble(random, Math.PI); double pitch = RandomNumbers.nextDouble(random, Math.PI / 2.0); double roll = RandomNumbers.nextDouble(random, Math.PI); double yawRate = RandomNumbers.nextDouble(random, 1.0); double pitchRate = RandomNumbers.nextDouble(random, 1.0); double rollRate = RandomNumbers.nextDouble(random, 1.0); Quaternion rotation = new Quaternion(); rotation.setYawPitchRoll(yaw, pitch, roll); Vector3D expectedAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, expectedAngularVelocity); rotation.transform(expectedAngularVelocity); Vector3D actualAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInWorldFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, actualAngularVelocity); EuclidCoreTestTools.assertTuple3DEquals(expectedAngularVelocity, actualAngularVelocity, EPSILON); } }
@Override public FrameQuaternion createFrameTuple(ReferenceFrame referenceFrame, double x, double y, double z, double s) { Quaternion quaternion = new Quaternion(); quaternion.setUnsafe(x, y, z, s); return new FrameQuaternion(referenceFrame, quaternion); }
private static double getAngleDifference(Quaternion q1, Quaternion q2) { Quaternion qDifference = new Quaternion(q1); qDifference.multiplyConjugateOther(q2); AxisAngle axisAngle = new AxisAngle(); axisAngle.set(qDifference); return axisAngle.getAngle(); } }