public void applyTransform(RigidBodyTransform transform) { this.transform.preMultiply(transform); }
@Test public void testPreMultiplyInvertOtherWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform inverseOfMultipliedWithRigidBody = new RigidBodyTransform(multipliedWithRigidBody); inverseOfMultipliedWithRigidBody.invert(); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.preMultiply(inverseOfMultipliedWithRigidBody); actual.set(original); actual.preMultiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.preMultiplyInvertOther(new QuaternionBasedTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertThisWithAffineTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = new AffineTransform(multipliedWithRigidBody); multipliedWith.setScale(EuclidCoreRandomTools.nextVector3D(random, 0.0, 10.0)); expected.set(original); expected.invert(); expected.preMultiply(multipliedWithRigidBody); actual.set(original); actual.preMultiplyInvertThis(multipliedWith); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.preMultiplyInvertThis(new AffineTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyWithRigidBodyTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { QuaternionBasedTransform original = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); QuaternionBasedTransform expected = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); QuaternionBasedTransform actual = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWith = EuclidCoreRandomTools.nextRigidBodyTransform(random); expectedRigidBody.set(original); expectedRigidBody.preMultiply(multipliedWith); expected.set(expectedRigidBody); actual.set(original); actual.preMultiply(multipliedWith); EuclidCoreTestTools.assertQuaternionBasedTransformGeometricallyEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertOtherWithAffineTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform inverseOfMultipliedWithRigidBody = new RigidBodyTransform(multipliedWithRigidBody); inverseOfMultipliedWithRigidBody.invert(); AffineTransform multipliedWith = new AffineTransform(multipliedWithRigidBody); multipliedWith.setScale(EuclidCoreRandomTools.nextVector3D(random, 0.0, 10.0)); expected.set(original); expected.preMultiply(inverseOfMultipliedWithRigidBody); actual.set(original); actual.preMultiplyInvertOther(multipliedWith); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyInvertThisWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.invert(); expected.preMultiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiplyInvertThis(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.preMultiplyInvertThis(new QuaternionBasedTransform(actual)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiply() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { QuaternionBasedTransform original = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); QuaternionBasedTransform expected = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); QuaternionBasedTransform actual = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); expectedRigidBody.set(original); expectedRigidBody.preMultiply(multipliedWith); expected.set(expectedRigidBody); actual.set(original); actual.preMultiply(multipliedWith); EuclidCoreTestTools.assertQuaternionBasedTransformGeometricallyEquals(expected, actual, EPS); } }
expected.preMultiply(inverseOfMultipliedWith); actual.set(original); assertTrue(actual.hasRotation());
expected.preMultiply(multipliedWith); actual.set(original); assertTrue(actual.hasRotation());
@Test public void testPreMultiplyWithAffineTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { QuaternionBasedTransform original = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); QuaternionBasedTransform expected = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); QuaternionBasedTransform actual = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); expectedRigidBody.set(original); expectedRigidBody.preMultiply(multipliedWith); expected.set(expectedRigidBody); actual.set(original); actual.preMultiply(multipliedWith); EuclidCoreTestTools.assertQuaternionBasedTransformGeometricallyEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.preMultiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); multipliedWith.set(actual); multipliedWith.invert(); actual.preMultiply(multipliedWith); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiply() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); AffineTransform multipliedWith = EuclidCoreRandomTools.nextAffineTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiply(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiply(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
@Test public void testPreMultiplyWithQuaternionBasedTransform() throws Exception { Random random = new Random(2342L); // Test against RigidBodyTransform.multiply() for (int i = 0; i < ITERATIONS; i++) { AffineTransform original = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform expected = EuclidCoreRandomTools.nextAffineTransform(random); AffineTransform actual = EuclidCoreRandomTools.nextAffineTransform(random); RigidBodyTransform expectedRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); original.getRigidBodyTransform(expectedRigidBody); expectedRigidBody.preMultiply(multipliedWith); expected.set(expectedRigidBody); expected.setScale(original.getScale()); actual.set(original); actual.preMultiply(multipliedWith); EuclidCoreTestTools.assertAffineTransformEquals(expected, actual, EPS); } }
expected.preMultiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(new AffineTransform(multipliedWithRigidBody)); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation());
transform.preMultiply(inverse); assertFalse(transform.hasRotation()); assertFalse(transform.hasTranslation()); RigidBodyTransform t2 = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform t3 = new RigidBodyTransform(t2); t3.preMultiply(t1); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(t2); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); assertFalse(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(t2); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(t2); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); assertTrue(actual.hasRotation()); assertFalse(actual.hasTranslation()); actual.preMultiply(t2);
RigidBodyTransform transformFromRootJointToWorldFrame = rootJointFrame.getTransformToDesiredFrame(supportFootFrame); RigidBodyTransform initialSupportFootTransform = initialFullRobotModel.getFoot(supportFootSide).getBodyFixedFrame().getTransformToWorldFrame(); transformFromRootJointToWorldFrame.preMultiply(initialSupportFootTransform);
transformToPack.preMultiply(parentFrame.transformToParent);
expected.set(original); assertTrue(expected.hasTranslation()); expected.preMultiply(translationTransform); assertTrue(expected.hasTranslation()); expected.set(original); assertTrue(expected.hasTranslation()); expected.preMultiply(translationTransform); assertTrue(expected.hasTranslation());
expected.set(original); assertTrue(expected.hasRotation()); expected.preMultiply(yawTransform); assertTrue(expected.hasRotation()); pitchTransform.setRotationPitch(pitch); expected.set(original); expected.preMultiply(pitchTransform); assertTrue(expected.hasRotation()); expected.set(original); assertTrue(expected.hasRotation()); expected.preMultiply(rollTransform); assertTrue(expected.hasRotation());