@Override public void applyTransform(Transform transform) { orientation.applyTransform(transform); angularVelocity.applyTransform(transform); }
@Override public void transform(AdjustFootstepMessage object, RigidBodyTransform rigidBodyTransformToApply) { object.getLocation().applyTransform(rigidBodyTransformToApply); object.getOrientation().applyTransform(rigidBodyTransformToApply); } });
@Test public void testChangeFrame() throws Exception { Random random = new Random(43563); for (int i = 0; i < ITERATIONS; i++) { ReferenceFrame[] referenceFrames = EuclidFrameRandomTools.nextReferenceFrameTree(random); ReferenceFrame initialFrame = referenceFrames[random.nextInt(referenceFrames.length)]; ReferenceFrame anotherFrame = referenceFrames[random.nextInt(referenceFrames.length)]; Quaternion expected = EuclidCoreRandomTools.nextQuaternion(random); FrameQuaternion quaternion = new FrameQuaternion(initialFrame, expected); RigidBodyTransform transform = initialFrame.getTransformToDesiredFrame(anotherFrame); expected.applyTransform(transform); quaternion.changeFrame(anotherFrame); assertTrue(anotherFrame == quaternion.getReferenceFrame()); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expected, quaternion, EPSILON); ReferenceFrame differentRootFrame = ReferenceFrameTools.constructARootFrame("anotherRootFrame"); try { quaternion.changeFrame(differentRootFrame); fail("Should have thrown a RuntimeException"); } catch (RuntimeException e) { // good } } }
@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 = RandomNumbers.nextDouble(random, 0.0, 1000.0); Quaternion expectedOrientation = new Quaternion(RandomGeometry.nextQuaternion(random)); Vector3D expectedAngularVelocity = new Vector3D(RandomGeometry.nextVector3D(random)); SimpleSO3TrajectoryPoint testedSimpleSO3TrajectoryPoint = new SimpleSO3TrajectoryPoint(expectedTime, expectedOrientation, expectedAngularVelocity); for (int i = 0; i < 10000; i++) { expectedFrame = EuclidFrameRandomTools.nextReferenceFrame("randomFrame" + i, random, random.nextBoolean() ? worldFrame : expectedFrame); expectedOrientation.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedAngularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); testedSimpleSO3TrajectoryPoint.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); assertTrajectoryPointContainsExpectedData(expectedTime, expectedOrientation, expectedAngularVelocity, testedSimpleSO3TrajectoryPoint, epsilon); } }
@Override public void transform(FootstepDataMessage message, RigidBodyTransform rigidBodyTransformToApply) { if (message.getLocation() != null) message.getLocation().applyTransform(rigidBodyTransformToApply); if (message.getOrientation() != null) message.getOrientation().applyTransform(rigidBodyTransformToApply); if (message.getCustomPositionWaypoints() != null) { for (int i = 0; i < message.getCustomPositionWaypoints().size(); i++) message.getCustomPositionWaypoints().get(i).applyTransform(rigidBodyTransformToApply); } } });
@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 = RandomNumbers.nextDouble(random, 0.0, 1000.0); Point3D expectedPosition = new Point3D(RandomGeometry.nextPoint3D(random, 10.0, 10.0, 10.0)); Quaternion expectedOrientation = new Quaternion(RandomGeometry.nextQuaternion(random)); Vector3D expectedLinearVelocity = new Vector3D(RandomGeometry.nextVector3D(random)); Vector3D expectedAngularVelocity = new Vector3D(RandomGeometry.nextVector3D(random)); SimpleSE3TrajectoryPoint testedSimpleSE3TrajectoryPoint = new SimpleSE3TrajectoryPoint(expectedTime, expectedPosition, expectedOrientation, expectedLinearVelocity, expectedAngularVelocity); for (int i = 0; i < 10000; i++) { expectedFrame = EuclidFrameRandomTools.nextReferenceFrame("randomFrame" + i, random, random.nextBoolean() ? worldFrame : expectedFrame); expectedPosition.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedOrientation.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedLinearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); expectedAngularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); testedSimpleSE3TrajectoryPoint.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame)); assertTrajectoryPointContainsExpectedData(expectedTime, expectedPosition, expectedOrientation, expectedLinearVelocity, expectedAngularVelocity, testedSimpleSE3TrajectoryPoint, epsilon); } }
desiredPosition2d.applyTransform(fromWorldToMidFeetZUpTransform); Quaternion desiredOrientationCorrected = new Quaternion(desiredOrientation); desiredOrientationCorrected.applyTransform(fromWorldToMidFeetZUpTransform);
desiredPosition2d.applyTransform(fromWorldToMidFeetZUpTransform); Quaternion desiredOrientationCorrected = new Quaternion(desiredOrientation); desiredOrientationCorrected.applyTransform(fromWorldToMidFeetZUpTransform);
orientation.applyTransform(transformToPoseFrame); transformToPoseFrame.transform(angularVelocity);
orientation.applyTransform(transformToPoseFrame); transformToPoseFrame.transform(angularVelocity);
orientation.applyTransform(transformToPoseFrame); transformToPoseFrame.transform(angularVelocity);
orientation.applyTransform(transformToPoseFrame); transformToPoseFrame.transform(linearVelocity); transformToPoseFrame.transform(angularVelocity);
orientation.applyTransform(transformToPoseFrame); transformToPoseFrame.transform(linearVelocity); transformToPoseFrame.transform(angularVelocity);
orientation.applyTransform(transformToPoseFrame); transformToPoseFrame.transform(linearVelocity); transformToPoseFrame.transform(angularVelocity);
assertFalse(angularVelocity.epsilonEquals(simpleSO3TrajectoryPoint.getAngularVelocityCopy(), 1e-10)); orientation.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame)); angularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));
orientation.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame)); linearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame)); angularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));