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