/** * Asserts on a per component basis that the two frame tuples are equal to an {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param expected the expected frame tuple. Not modified. * @param actual the actual frame tuple. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame tuples are not equal or not expressed in the reference * frame. If only one of the arguments is equal to {@code null}. */ public static void assertFrameTuple3DEquals(FrameTuple3DReadOnly expected, FrameTuple3DReadOnly actual, double epsilon) { assertFrameTuple3DEquals(null, expected, actual, epsilon); }
/** * Asserts on a per component basis that the two frame tuples are equal to an {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param messagePrefix prefix to add to the error message. * @param expected the expected frame tuple. Not modified. * @param actual the actual frame tuple. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame tuples are not equal or not expressed in the reference * frame. If only one of the arguments is equal to {@code null}. */ public static void assertFrameTuple3DEquals(String messagePrefix, FrameTuple3DReadOnly expected, FrameTuple3DReadOnly actual, double epsilon) { assertFrameTuple3DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
private static void assertFramePointListsAreEqual(List<? extends FramePoint3DReadOnly> waypointList1, List<? extends FramePoint3DReadOnly> waypointList2, double epsilon) { for (int i = 0; i < waypointList1.size(); i++) { FramePoint3DReadOnly icpWaypoint1 = waypointList1.get(i); FramePoint3DReadOnly icpWaypoint2 = waypointList2.get(i); EuclidFrameTestTools.assertFrameTuple3DEquals(icpWaypoint1, icpWaypoint2, epsilon); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testInitialAndFinalPositionFullCircle() { Random random = new Random(125125L); ReferenceFrame referenceFrame = ReferenceFrame.getWorldFrame(); FramePoint3D offset = new FramePoint3D(referenceFrame, RandomGeometry.nextVector3D(random)); double rotationAngle = 2.0 * Math.PI; double trajectoryTime = random.nextDouble(); CirclePositionTrajectoryGenerator circleTrajectory = createTrajectory(referenceFrame, offset, rotationAngle, trajectoryTime); circleTrajectory.initialize(); FramePoint3D position = new FramePoint3D(); FrameVector3D velocity = new FrameVector3D(); FrameVector3D acceleration = new FrameVector3D(); FrameVector3D zero = new FrameVector3D(referenceFrame); circleTrajectory.compute(0.0); circleTrajectory.getPosition(position); circleTrajectory.getVelocity(velocity); EuclidFrameTestTools.assertFrameTuple3DEquals(offset, position, 1e-12); EuclidFrameTestTools.assertFrameTuple3DEquals(zero, velocity, 1e-12); circleTrajectory.compute(trajectoryTime); circleTrajectory.getPosition(position); circleTrajectory.getVelocity(velocity); circleTrajectory.getAcceleration(acceleration); EuclidFrameTestTools.assertFrameTuple3DEquals(offset, position, 1e-12); EuclidFrameTestTools.assertFrameTuple3DEquals(zero, velocity, 1e-12); EuclidFrameTestTools.assertFrameTuple3DEquals(zero, acceleration, 1e-12); }
private void checkVEqualsOmegaCrossR(ReferenceFrame frame, CirclePoseTrajectoryGenerator trajectoryGenerator, Random random) { trajectoryGenerator.compute(random.nextDouble()); FrameVector3D omega = new FrameVector3D(frame); trajectoryGenerator.getAngularVelocity(omega); FrameVector3D v = new FrameVector3D(frame); trajectoryGenerator.getVelocity(v); FramePoint3D r = new FramePoint3D(frame); trajectoryGenerator.getPosition(r); FrameVector3D vCheck = new FrameVector3D(frame); vCheck.cross(omega, r); EuclidFrameTestTools.assertFrameTuple3DEquals(vCheck, v, 1e-8); } }
EuclidFrameTestTools.assertFrameTuple3DEquals(capturePointsToPack.get(0), icpToCheck, 1e-8);
actualCenterOfMassPosition, actualCenterOfMassVelocity, actualCenterOfMassAcceleration); EuclidFrameTestTools.assertFrameTuple3DEquals(expectedCenterOfMassPosition, actualCenterOfMassPosition, EPSILON); EuclidFrameTestTools.assertFrameTuple3DEquals(expectedCenterOfMassVelocity, actualCenterOfMassVelocity, EPSILON); EuclidFrameTestTools.assertFrameTuple3DEquals(expectedCenterOfMassAcceleration, actualCenterOfMassAcceleration, EPSILON);
expected.changeFrame(destinationFrame); EuclidFrameTestTools.assertFrameTuple3DEquals(expected, actual, EPSILON); expected.changeFrame(destinationFrame); EuclidFrameTestTools.assertFrameTuple3DEquals(expected, actual, EPSILON);
EuclidFrameTestTools.assertFrameTuple3DEquals(velocity, numericallyDifferentiatedVelocity, 1e-5); EuclidFrameTestTools.assertFrameTuple3DEquals(acceleration, numericallyDifferentiatedAcceleration, 1e-4);
expected.changeFrame(destinationFrame); EuclidFrameTestTools.assertFrameTuple3DEquals(expected, actual, EPSILON); expected.changeFrame(destinationFrame); EuclidFrameTestTools.assertFrameTuple3DEquals(expected, actual, EPSILON);
icpPositionDesiredFinal, comPositionDesiredInitial, actualCoMQuantityDesired); EuclidFrameTestTools.assertFrameTuple3DEquals(expectedCoMQuantityDesired, actualCoMQuantityDesired, EPSILON);
pointVelocityFromNumericalDifferentiation.scale(1.0 / dt); EuclidFrameTestTools.assertFrameTuple3DEquals(pointVelocityFromNumericalDifferentiation, pointVelocityFromJacobian, 1e-6);
assertTrue(frameVector3D.getReferenceFrame() == randomFrame); EuclidCoreTestTools.assertTuple3DEquals(randomTuple, frameVector3D, EPSILON); EuclidFrameTestTools.assertFrameTuple3DEquals(randomTuple, frameVector3D, EPSILON);
assertTrue(framePoint3D.getReferenceFrame() == randomFrame); EuclidCoreTestTools.assertTuple3DEquals(randomTuple, framePoint3D, EPSILON); EuclidFrameTestTools.assertFrameTuple3DEquals(randomTuple, framePoint3D, EPSILON);