@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSomeSetsAngGets() {
String namePrefix = "point";
String nameSuffix = "toTest";
YoVariableRegistry registry = new YoVariableRegistry("myRegistry");
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame);
yoFrameSO3TrajectoryPoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
SimpleSO3TrajectoryPoint simpleTrajectoryPoint = new SimpleSO3TrajectoryPoint();
double time = 3.4;
TransformableQuat4d orientation = new TransformableQuat4d(new Quat4d(0.1, 0.22, 0.34, 0.56));
orientation.normalize();
Vector3d angularVelocity = new Vector3d(1.7, 8.4, 2.2);
simpleTrajectoryPoint.set(time, orientation, angularVelocity);
yoFrameSO3TrajectoryPoint.setIncludingFrame(worldFrame, simpleTrajectoryPoint);
// Check some get calls:
YoFrameQuaternion quaternionForVerification =
new YoFrameQuaternion("quaternionForVerification", worldFrame, registry);
YoFrameVector angularVelocityForVerification =
new YoFrameVector("angularVelocityForVerification", worldFrame, registry);
yoFrameSO3TrajectoryPoint.getOrientation(quaternionForVerification);
yoFrameSO3TrajectoryPoint.getAngularVelocity(angularVelocityForVerification);
assertEquals(time, yoFrameSO3TrajectoryPoint.getTime(), 1e-10);
assertTrue(quaternionForVerification.getFrameOrientation().epsilonEquals(orientation, 1e-10));
assertTrue(
angularVelocityForVerification.getFrameTuple().epsilonEquals(angularVelocity, 1e-10));
// Check NaN calls:
assertFalse(yoFrameSO3TrajectoryPoint.containsNaN());
yoFrameSO3TrajectoryPoint.setOrientationToNaN();
assertTrue(yoFrameSO3TrajectoryPoint.containsNaN());
yoFrameSO3TrajectoryPoint.setOrientationToZero();
assertFalse(yoFrameSO3TrajectoryPoint.containsNaN());
yoFrameSO3TrajectoryPoint.setAngularVelocityToNaN();
assertTrue(yoFrameSO3TrajectoryPoint.containsNaN());
yoFrameSO3TrajectoryPoint.setAngularVelocityToZero();
assertFalse(yoFrameSO3TrajectoryPoint.containsNaN());
yoFrameSO3TrajectoryPoint.getOrientation(orientation);
yoFrameSO3TrajectoryPoint.getAngularVelocity(angularVelocity);
// Make sure they are all equal to zero:
assertTrue(orientation.epsilonEquals(new TransformableQuat4d(), 1e-10));
assertTrue(angularVelocity.epsilonEquals(new Vector3d(), 1e-10));
time = 9.9;
quaternionForVerification.set(0.2, 0.6, 1.1);
angularVelocityForVerification.set(7.1, 2.2, 3.33);
assertFalse(Math.abs(yoFrameSO3TrajectoryPoint.getTime() - time) < 1e-7);
assertFalse(quaternionForVerification.getFrameOrientation().epsilonEquals(orientation, 1e-7));
assertFalse(
angularVelocityForVerification.getFrameTuple().epsilonEquals(angularVelocity, 1e-7));
yoFrameSO3TrajectoryPoint.set(time, quaternionForVerification, angularVelocityForVerification);
yoFrameSO3TrajectoryPoint.getOrientation(orientation);
yoFrameSO3TrajectoryPoint.getAngularVelocity(angularVelocity);
assertEquals(time, yoFrameSO3TrajectoryPoint.getTime(), 1e-10);
assertTrue(quaternionForVerification.getFrameOrientation().epsilonEquals(orientation, 1e-10));
assertTrue(
angularVelocityForVerification.getFrameTuple().epsilonEquals(angularVelocity, 1e-10));
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPointTwo =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix + "Two", registry, worldFrame);
assertFalse(yoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-7));
yoFrameSO3TrajectoryPointTwo.set(yoFrameSO3TrajectoryPoint);
assertTrue(yoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-7));
SimpleSO3TrajectoryPoint simplePoint = new SimpleSO3TrajectoryPoint();
yoFrameSO3TrajectoryPoint.get(simplePoint);
yoFrameSO3TrajectoryPoint.setToNaN();
assertTrue(yoFrameSO3TrajectoryPoint.containsNaN());
assertFalse(yoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-7));
SO3TrajectoryPointInterface<?> trajectoryPointAsInterface = simplePoint;
yoFrameSO3TrajectoryPoint.set(trajectoryPointAsInterface);
assertTrue(yoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-7));
String string = yoFrameSO3TrajectoryPoint.toString();
String expectedString =
"SO3 trajectory point: (time = 9.90, SO3 trajectory point: (time = 9.90, SO3 waypoint: [orientation = ( 0.47, 0.30, -0.07, 0.83), angular velocity = ( 7.10, 2.20, 3.33)].)World)";
assertEquals(expectedString, string);
}
private void assertWaypointContainsExpectedData(
String expectedNamePrefix,
String expectedNameSuffix,
ReferenceFrame expectedFrame,
double expectedTime,
FrameOrientation expectedOrientation,
FrameVector expectedAngularVelocity,
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint,
double epsilon) {
assertTrue(expectedFrame == testedYoFrameSO3TrajectoryPoint.getReferenceFrame());
assertEquals(expectedTime, testedYoFrameSO3TrajectoryPoint.getTime(), epsilon);
assertEquals(expectedNamePrefix, testedYoFrameSO3TrajectoryPoint.getNamePrefix());
assertEquals(expectedNameSuffix, testedYoFrameSO3TrajectoryPoint.getNameSuffix());
assertTrue(
expectedOrientation.epsilonEquals(
testedYoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), epsilon));
assertTrue(
expectedAngularVelocity.epsilonEquals(
testedYoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), epsilon));
FrameSO3TrajectoryPoint actualFrameSO3TrajectoryPoint = new FrameSO3TrajectoryPoint();
testedYoFrameSO3TrajectoryPoint.getIncludingFrame(actualFrameSO3TrajectoryPoint);
FrameSO3TrajectoryPointTest.assertTrajectoryPointContainsExpectedData(
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
actualFrameSO3TrajectoryPoint,
epsilon);
actualFrameSO3TrajectoryPoint = new FrameSO3TrajectoryPoint(expectedFrame);
testedYoFrameSO3TrajectoryPoint.get(actualFrameSO3TrajectoryPoint);
FrameSO3TrajectoryPointTest.assertTrajectoryPointContainsExpectedData(
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
actualFrameSO3TrajectoryPoint,
epsilon);
Quat4d actualOrientation = new Quat4d();
Vector3d actualAngularVelocity = new Vector3d();
testedYoFrameSO3TrajectoryPoint.getOrientation(actualOrientation);
testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualAngularVelocity);
assertTrue(expectedOrientation.epsilonEquals(actualOrientation, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon));
FrameOrientation actualFrameOrientation = new FrameOrientation(expectedFrame);
FrameVector actualFrameAngularVelocity = new FrameVector(expectedFrame);
testedYoFrameSO3TrajectoryPoint.getOrientation(actualFrameOrientation);
testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity);
assertTrue(expectedOrientation.epsilonEquals(actualFrameOrientation, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
actualFrameOrientation = new FrameOrientation();
actualFrameAngularVelocity = new FrameVector();
testedYoFrameSO3TrajectoryPoint.getOrientationIncludingFrame(actualFrameOrientation);
testedYoFrameSO3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity);
assertTrue(expectedOrientation.epsilonEquals(actualFrameOrientation, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
}