public void update(Quaternion qMeasured) { if (hasBeenCalled.getBooleanValue()) { qPreviousFiltered.set(this); qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' set(qNewFiltered); } else { set(qMeasured); hasBeenCalled.set(true); } }
/** * Computes the interpolation between the two transforms using the alpha parameter to control the blend. * Note that the transforms must have a proper rotation matrix, meaning it satisfies: R'R = I and det(R) = 1 * @param transform1 * @param transform2 * @param alpha Ranges from [0, 1], where return = (1- alpha) * tansform1 + (alpha) * transform2 * @return return = (1- alpha) * tansform1 + alpha * transform2 */ public void computeInterpolation(RigidBodyTransform transform1, RigidBodyTransform transform2, RigidBodyTransform result, double alpha) { alpha = MathTools.clamp(alpha, 0.0, 1.0); transform1.get(transform1Quaternion, transform1Translation); transform2.get(transform2Quaternion, transform2Translation); interpolatedTranslation.interpolate(transform1Translation, transform2Translation, alpha); interpolatedQuaternion.interpolate(transform1Quaternion, transform2Quaternion, alpha); result.setRotationAndZeroTranslation(interpolatedQuaternion); result.setTranslation(interpolatedTranslation); }
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testInterpolateAgainstQuat4d() throws Exception { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Random random = new Random(6546545L); Quaternion q0 = RandomGeometry.nextQuaternion(random); Quaternion q1 = RandomGeometry.nextQuaternion(random); Quaternion expectedQInterpolated = new Quaternion(); Quaternion actualQInterpolated = new Quaternion(); for (double alpha = 0.0; alpha <= 1.0; alpha += 1.0e-6) { expectedQInterpolated.interpolate(q0, q1, alpha); quaternionCalculus.interpolate(alpha, q0, q1, actualQInterpolated); assertTrue(expectedQInterpolated.epsilonEquals(actualQInterpolated, EPSILON)); } } }
qInterpolated.interpolate(qf, alpha); expected.set(qInterpolated); qInterpolated.interpolate(q0, qf, alpha); expected.set(qInterpolated);
/** * trajectory for current transform to goal transform */ public static Pose3D computeLinearTrajectory(double time, double trajectoryTime, RigidBodyTransform from, RigidBodyTransform to) { double progress = time / trajectoryTime; Point3D fromPoint = new Point3D(from.getTranslationVector()); Point3D toPoint = new Point3D(to.getTranslationVector()); Quaternion fromOrienation = new Quaternion(from.getRotationMatrix()); Quaternion toOrienation = new Quaternion(to.getRotationMatrix()); Point3D point = new Point3D(); Quaternion orientation = new Quaternion(); point.interpolate(fromPoint, toPoint, progress); orientation.interpolate(fromOrienation, toOrienation, progress); return new Pose3D(point, orientation); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAgainstInterpolation() throws Exception { for (int nTest = 0; nTest < 10; nTest++) { double epsilon = 1.0e-15; Random random = new Random(56416456L); Quaternion quat1 = RandomGeometry.nextQuaternion(random); Quaternion quat2 = RandomGeometry.nextQuaternion(random); Quaternion expectedAverageQuat = new Quaternion(); expectedAverageQuat.interpolate(quat1, quat2, 0.5); AverageQuaternionCalculator averageQuaternionCalculator = new AverageQuaternionCalculator(); averageQuaternionCalculator.queueQuaternion(quat1); averageQuaternionCalculator.queueQuaternion(quat2); averageQuaternionCalculator.compute(); Quaternion actualAverageQuat = new Quaternion(); averageQuaternionCalculator.getAverageQuaternion(actualAverageQuat); if (expectedAverageQuat.getS() * actualAverageQuat.getS() < 0.0) expectedAverageQuat.negate(); double[] expecteds = new double[4]; expectedAverageQuat.get(expecteds); double[] actuals = new double[4]; actualAverageQuat.get(actuals); assertArrayEquals(expecteds, actuals, epsilon); } }
qInterpolated.interpolate(q1, q2, alpha); RotationMatrix expectedMatrix = new RotationMatrix(qInterpolated);
quaternionFutureTwo.preMultiply(poseTwo.getOrientation()); quaternionFuture.interpolate(quaternionFutureOne, quaternionFutureTwo, 0.5); difference.difference(pose.getOrientation(), quaternionFuture); angularVelocity.setToZero(this);
interpolatedRotation.interpolate(q0.getQuaternion(), qf.getQuaternion(), alpha);