@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSomeMoreSettersAndGetters() {
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());
double time = 3.4;
FrameOrientation orientation =
new FrameOrientation(worldFrame, new Quat4d(0.1, 0.22, 0.34, 0.56));
FrameVector angularVelocity = new FrameVector(worldFrame, 1.7, 8.4, 2.2);
yoFrameSO3TrajectoryPoint.setTime(time);
yoFrameSO3TrajectoryPoint.setOrientation(orientation);
yoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);
PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame));
FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2));
poseFrame.setPositionAndUpdate(poseFramePosition);
FrameOrientation poseOrientation =
new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2));
poseFrame.setOrientationAndUpdate(poseOrientation);
yoFrameSO3TrajectoryPoint.registerReferenceFrame(poseFrame);
yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);
assertFalse(
orientation.epsilonEquals(
yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10));
assertFalse(
angularVelocity.epsilonEquals(
yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10));
orientation.changeFrame(poseFrame);
angularVelocity.changeFrame(poseFrame);
assertTrue(
orientation.epsilonEquals(
yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10));
assertTrue(
angularVelocity.epsilonEquals(
yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10));
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPointTwo =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix + "Two", registry, poseFrame);
yoFrameSO3TrajectoryPointTwo.setTime(time);
yoFrameSO3TrajectoryPointTwo.setOrientation(orientation);
yoFrameSO3TrajectoryPointTwo.setAngularVelocity(angularVelocity);
assertTrue(yoFrameSO3TrajectoryPointTwo.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-10));
}
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));
}
