@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)); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConversionQDotToAngularVelocityBackAndForth() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); double length = RandomNumbers.nextDouble(random, 0.0, 10.0); Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random, length); if (random.nextBoolean()) expectedAngularVelocity.negate(); Vector3D actualAngularVelocity = new Vector3D(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDot(q, expectedAngularVelocity, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, EPSILON)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLogAndExpAlgebra() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); Vector4D qLog = new Vector4D(); Quaternion vExp = new Quaternion(); quaternionCalculus.log(q, qLog); Vector3D v = new Vector3D(qLog.getX(),qLog.getY(),qLog.getZ()); quaternionCalculus.exp(v, vExp); assertTrue(Math.abs(q.getX() - vExp.getX()) < 10e-10); assertTrue(Math.abs(q.getY() - vExp.getY()) < 10e-10); assertTrue(Math.abs(q.getZ() - vExp.getZ()) < 10e-10); assertTrue(Math.abs(q.getS() - vExp.getS()) < 10e-10); } }
public static FrameVector3D computeAngularVelocityByFiniteDifference(double dt, ReferenceFrame bodyFrame, ReferenceFrame bodyFrameInFuture) { FrameQuaternion bodyOrientation = new FrameQuaternion(bodyFrame); bodyOrientation.changeFrame(worldFrame); FrameQuaternion bodyOrientationInFuture = new FrameQuaternion(bodyFrameInFuture); bodyOrientationInFuture.changeFrame(worldFrame); FrameVector3D bodyAngularVelocity = new FrameVector3D(worldFrame); QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDotByFiniteDifferenceCentral(bodyOrientation, bodyOrientationInFuture, 0.5 * dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(bodyOrientation, qDot, bodyAngularVelocity); bodyAngularVelocity.changeFrame(bodyFrame); return bodyAngularVelocity; }
public void testContinuityForSlowTrajectory() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); Random random = new Random(5165165161L);
public void testContinuityForFastishTrajectory() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); Random random = new Random(5165165161L);
QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); double length = RandomNumbers.nextDouble(random, 0.0, 10.0);
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testIntegrateToQuaternion() throws Exception { for (int i = 0; i < 100; i++) { Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random); Vector3D actualAngularVelocity = new Vector3D(); Quaternion integrationResultPrevious = new Quaternion(); Quaternion integrationResultCurrent = new Quaternion(); Quaternion integrationResultNext = new Quaternion(); Vector4D qDot = new Vector4D(); QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); double dt = 1.0e-4; for (double t = dt; t <= 1.0; t += dt) { RotationTools.integrateAngularVelocity(expectedAngularVelocity, t - dt, integrationResultPrevious); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t, integrationResultCurrent); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t + dt, integrationResultNext); quaternionCalculus.computeQDotByFiniteDifferenceCentral(integrationResultPrevious, integrationResultNext, dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(integrationResultCurrent, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, 1.0e-7)); } } }
public void testFDSimpleCase() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Random random = new Random(65265L); double integrationTime = 1.0;
public void testContinuityForSlowTrajectory() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); Random random = new Random(5165165161L);
public void testContinuityForFastishTrajectory() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); Random random = new Random(5165165161L);
public void testVelocityFromFDAgainstTrajectory() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); SimpleOrientationTrajectoryGenerator traj = new SimpleOrientationTrajectoryGenerator("traj", ReferenceFrame.getWorldFrame(), new YoVariableRegistry("null")); double trajectoryTime = 1.0;
public void testAccelerationFromFDAgainstTrajectory() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); SimpleOrientationTrajectoryGenerator traj = new SimpleOrientationTrajectoryGenerator("traj", ReferenceFrame.getWorldFrame(), new YoVariableRegistry("null")); double trajectoryTime = 1.0;