public static double getSizeOfTransformWithRotationScaled(RigidBodyTransform rigidBodyTransform, double radiusForScalingRotation) { return getMagnitudeOfTranslation(rigidBodyTransform) + radiusForScalingRotation * getMagnitudeOfAngleOfRotation(rigidBodyTransform); }
public static double getSizeOfTransformWithRotationScaled(RigidBodyTransform rigidBodyTransform, double radiusForScalingRotation) { return getMagnitudeOfTranslation(rigidBodyTransform) + radiusForScalingRotation * getMagnitudeOfAngleOfRotation(rigidBodyTransform); }
/** 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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testgetTransformDifference() { RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(); Random random = new Random(); int numberOfTests = 1000; for (int i = 0; i < numberOfTests; i++) { double angle = -Math.PI + 2 * Math.PI * random.nextDouble(); Vector3D vector3d = new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); vector3d.normalize(); AxisAngle axisAngle = new AxisAngle(vector3d, angle); double vectorScale = 10.0; Vector3D vector3dTranlation = new Vector3D(-vectorScale + 2.0 * vectorScale * random.nextDouble(), -vectorScale + 2.0 * vectorScale * random.nextDouble(), -vectorScale + 2.0 * vectorScale * random.nextDouble()); rigidBodyTransform.set(axisAngle, vector3dTranlation); double angleFromTransform = TransformTools.getMagnitudeOfAngleOfRotation(rigidBodyTransform); assertEquals(Math.abs(angle), angleFromTransform, 1e-9); double translation = TransformTools.getMagnitudeOfTranslation(rigidBodyTransform); assertEquals(vector3dTranlation.length(), translation, 1e-9); double radiusOfRotation = random.nextDouble(); double magnitudeOfTransform = TransformTools.getSizeOfTransformWithRotationScaled(rigidBodyTransform, radiusOfRotation); assertEquals(vector3dTranlation.length() + Math.abs(angle) * radiusOfRotation, magnitudeOfTransform, 1e-9); } }
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)); }