@ContinuousIntegrationTest(estimatedDuration = 0.2)
  @Test(timeout = 30000)
  public void testChangeFrame() throws Exception {
    double epsilon = 1.0e-10;
    Random random = new Random(21651016L);
    ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

    ReferenceFrame expectedFrame = worldFrame;
    double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    FrameOrientation expectedOrientation =
        FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
    FrameVector expectedAngularVelocity =
        FrameVector.generateRandomFrameVector(random, expectedFrame);
    String expectedNamePrefix = "test";
    String expectedNameSuffix = "blop";
    YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(
            expectedNamePrefix,
            expectedNameSuffix,
            new YoVariableRegistry("schnoop"),
            expectedFrame);

    testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);

    ReferenceFrame[] randomFrames = new ReferenceFrame[10];
    randomFrames[0] = worldFrame;
    for (int i = 1; i < 10; i++)
      randomFrames[i] =
          ReferenceFrame.generateRandomReferenceFrame(
              "randomFrame" + i,
              random,
              random.nextBoolean() ? worldFrame : randomFrames[random.nextInt(i)]);

    for (int i = 0; i < 10000; i++) {
      expectedFrame = randomFrames[random.nextInt(10)];
      testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);

      expectedOrientation.changeFrame(expectedFrame);
      expectedAngularVelocity.changeFrame(expectedFrame);
      testedYoFrameSO3TrajectoryPoint.changeFrame(expectedFrame);

      assertWaypointContainsExpectedData(
          expectedNamePrefix,
          expectedNameSuffix,
          expectedFrame,
          expectedTime,
          expectedOrientation,
          expectedAngularVelocity,
          testedYoFrameSO3TrajectoryPoint,
          epsilon);
    }
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSetToNaN() throws Exception {
    Random random = new Random(21651016L);
    ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

    ReferenceFrame expectedFrame = worldFrame;
    double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    FrameOrientation expectedOrientation =
        FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
    FrameVector expectedAngularVelocity =
        FrameVector.generateRandomFrameVector(random, expectedFrame);
    String expectedNamePrefix = "test";
    String expectedNameSuffix = "blop";
    YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(
            expectedNamePrefix,
            expectedNameSuffix,
            new YoVariableRegistry("schnoop"),
            expectedFrame);
    testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);

    testedYoFrameSO3TrajectoryPoint.setToNaN();
    assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
    assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
    assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());

    expectedFrame = ReferenceFrame.generateRandomReferenceFrame("blop", random, worldFrame);
    testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);
    expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, worldFrame);
    expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, worldFrame);
    testedYoFrameSO3TrajectoryPoint.switchCurrentReferenceFrame(worldFrame);
    testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(worldFrame);
    testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);

    testedYoFrameSO3TrajectoryPoint.setToNaN(expectedFrame);

    assertTrue(expectedFrame == testedYoFrameSO3TrajectoryPoint.getReferenceFrame());
    assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
    assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
    assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());
  }