@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));
  }