@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testCommonUsageExample() {
String namePrefix = "point";
String nameSuffix = "toTest";
YoVariableRegistry registry = new YoVariableRegistry("myRegistry");
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
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 yoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame, poseFrame);
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);
yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);
// Do some checks:
RigidBodyTransform transformToPoseFrame = worldFrame.getTransformToDesiredFrame(poseFrame);
orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(angularVelocity);
namePrefix = "point";
nameSuffix = "toVerify";
YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, poseFrame);
expectedYoFrameSO3TrajectoryPoint.setTime(time);
expectedYoFrameSO3TrajectoryPoint.setOrientation(orientation);
expectedYoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);
assertEquals(3.4, yoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertEquals(3.4, expectedYoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertTrue(expectedYoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPoint, 1e-10));
}
@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);
}
