public double getEffectiveDistanceToFramePose(FramePose framePose, double radiusOfRotation) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); transformThis.invert(); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); transformThat.invert(); return TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformThis, transformThat, radiusOfRotation); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testgetTransformDifferenceBetweenTwoTransforms() { Random random = new Random(); int numberOfTests = 1000; for (int i = 0; i < numberOfTests; i++) { DenseMatrix64F matrix = new DenseMatrix64F(4, 4); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA1 = new RigidBodyTransform(matrix); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA2 = new RigidBodyTransform(matrix); double radiusOfRotation = random.nextDouble(); double magnitudeOfTransform1 = TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformFromWorldToA1, transformFromWorldToA2, radiusOfRotation); RigidBodyTransform transformA2toA1 = TransformTools.getTransformFromA2toA1(transformFromWorldToA1, transformFromWorldToA2); double magnitudeOfTransform2 = TransformTools.getSizeOfTransformWithRotationScaled(transformA2toA1, radiusOfRotation); assertEquals(magnitudeOfTransform1, magnitudeOfTransform2, 1e-9); } }