/** This will get the magnitude of the transform between two transforms that are relative to the same frame. * @param rigidBodyTransform1 with respect to frame B * @param rigidBodyTransform2 with respect to frame B * @param radiusForScalingRotation * @return */ public static double getSizeOfTransformBetweenTwoWithRotationScaled(RigidBodyTransform rigidBodyTransform1, RigidBodyTransform rigidBodyTransform2, double radiusForScalingRotation) { RigidBodyTransform temp = getTransformFromA2toA1(rigidBodyTransform1, rigidBodyTransform2); return getMagnitudeOfTranslation(temp) + radiusForScalingRotation * getMagnitudeOfAngleOfRotation(temp); } }
/** This will get the magnitude of the transform between two transforms that are relative to the same frame. * @param rigidBodyTransform1 with respect to frame B * @param rigidBodyTransform2 with respect to frame B * @param radiusForScalingRotation * @return */ public static double getSizeOfTransformBetweenTwoWithRotationScaled(RigidBodyTransform rigidBodyTransform1, RigidBodyTransform rigidBodyTransform2, double radiusForScalingRotation) { RigidBodyTransform temp = getTransformFromA2toA1(rigidBodyTransform1, rigidBodyTransform2); return getMagnitudeOfTranslation(temp) + radiusForScalingRotation * getMagnitudeOfAngleOfRotation(temp); } }
public RigidBodyTransform getTransformFromThisToThat(FramePose thatPose) { checkReferenceFrameMatch(thatPose); RigidBodyTransform transformToThis = new RigidBodyTransform(); this.getPose(transformToThis); RigidBodyTransform transformToThat = new RigidBodyTransform(); thatPose.getPose(transformToThat); return TransformTools.getTransformFromA2toA1(transformToThat, transformToThis); }
public double getOrientationDistance(FramePose2d framePose) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); return TransformTools.getMagnitudeOfAngleOfRotation(TransformTools.getTransformFromA2toA1(transformThis, transformThat)); }
@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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetTransformFromA1toA2Random() { DenseMatrix64F matrix = new DenseMatrix64F(4, 4); Random random = new Random(111L); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA1 = new RigidBodyTransform(matrix); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA2 = new RigidBodyTransform(matrix); ReferenceFrame a1 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a1", ReferenceFrame.getWorldFrame(), transformFromWorldToA1); ReferenceFrame a2 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a2", ReferenceFrame.getWorldFrame(), transformFromWorldToA2); RigidBodyTransform transformA2toA1 = TransformTools.getTransformFromA2toA1(transformFromWorldToA1, transformFromWorldToA2); Point3D a2Origin = new Point3D(); transformA2toA1.transform(a2Origin); // System.out.println("a2Origin after transform" + a2Origin); FramePoint3D a2OriginFramePoint = new FramePoint3D(a2); a2OriginFramePoint.changeFrame(a1); // System.out.println("a2OriginFramePoint = " + a2OriginFramePoint); a2Origin.epsilonEquals(a2OriginFramePoint, 1e-9); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetTransformFromA1toA2Simple() { Vector3D vectorA1 = new Vector3D(-1.0, -2.0, -3.0); RigidBodyTransform transformFromWorldToA1 = new RigidBodyTransform(); transformFromWorldToA1.setTranslation(vectorA1); Vector3D vectorA2 = new Vector3D(vectorA1); vectorA2.negate(); RigidBodyTransform transformFromWorldToA2 = new RigidBodyTransform(); transformFromWorldToA2.setTranslation(vectorA2); ReferenceFrame a1 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a1", ReferenceFrame.getWorldFrame(), transformFromWorldToA1); ReferenceFrame a2 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a2", ReferenceFrame.getWorldFrame(), transformFromWorldToA2); RigidBodyTransform transformA2toA1 = TransformTools.getTransformFromA2toA1(transformFromWorldToA1, transformFromWorldToA2); Point3D a2Origin = new Point3D(); transformA2toA1.transform(a2Origin); // System.out.println("a2Origin after transform" + a2Origin); FramePoint3D a2OriginFramePoint = new FramePoint3D(a2); a2OriginFramePoint.changeFrame(a1); // System.out.println("a2OriginFramePoint = " + a2OriginFramePoint); a2Origin.epsilonEquals(a2OriginFramePoint, 1e-9); }