/** * Asserts on a per component basis that the two frame quaternions represent the same geometry 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 quaternions do not represent the same geometry or not expressed * in the reference frame. If only one of the arguments is equal to {@code null}. */ public static void assertFrameQuaternionGeometricallyEquals(FrameQuaternionReadOnly expected, FrameQuaternionReadOnly actual, double epsilon) { assertFrameQuaternionGeometricallyEquals(null, expected, actual, epsilon); }
/** * Asserts on a per component basis that the two frame quaternions represent the same geometry 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 quaternions do not represent the same geometry or not expressed * in the reference frame. If only one of the arguments is equal to {@code null}. */ public static void assertFrameQuaternionGeometricallyEquals(String messagePrefix, FrameQuaternionReadOnly expected, FrameQuaternionReadOnly actual, double epsilon) { assertFrameQuaternionGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
public void assertEpsilonEquals(PoseTrajectoryState other, double epsilon) { EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(position, other.position, epsilon); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(orientation, other.orientation, epsilon); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(linearVelocity, other.linearVelocity, epsilon); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(angularVelocity, other.angularVelocity, epsilon); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(linearAcceleration, other.linearAcceleration, epsilon); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(angularAcceleration, other.angularAcceleration, epsilon); } }
private void assertGeometricEquals(FramePose3D poseA, FramePose3D poseB, double epsilon) { EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(poseA.getPosition(), poseB.getPosition(), epsilon); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(poseA.getOrientation(), poseB.getOrientation(), epsilon); }
traj.getAngularData(currentOrientation, currentAngularVelocity, currentAngularAcceleration); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(initialOrientation, currentOrientation, 1.0e-2); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(angularVelocity, currentAngularVelocity, 1.0e-2); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(angularVelocity, currentAngularAcceleration, 1.0e-2);
private void checkOrientationAtVariousPoints(CirclePoseTrajectoryGenerator trajectoryGenerator, OrientationProvider initialOrientationProvider, double tMax, ReferenceFrame frame) { FrameQuaternion orientation = new FrameQuaternion(frame); trajectoryGenerator.compute(0.0); trajectoryGenerator.getOrientation(orientation); FrameQuaternion initialOrientation = new FrameQuaternion(frame); initialOrientationProvider.getOrientation(initialOrientation); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(initialOrientation, orientation, 1e-12); FramePoint3D initialPosition = new FramePoint3D(frame); trajectoryGenerator.getPosition(initialPosition); FramePoint3D newPosition = new FramePoint3D(frame); /* * check against rotated version of initial position */ int nTests = 10; for (int i = 1; i < nTests; i++) { double t = i * (tMax / nTests); trajectoryGenerator.compute(t); trajectoryGenerator.getPosition(newPosition); trajectoryGenerator.getOrientation(orientation); FrameQuaternion difference = new FrameQuaternion(initialOrientation.getReferenceFrame()); difference.difference(initialOrientation, orientation); FramePoint3D rotatedInitialPosition = new FramePoint3D(initialPosition); difference.transform(rotatedInitialPosition); // JUnitTools.assertFramePointEquals(newPosition, rotatedInitialPosition, 1e-9); } }
RigidBodyOrientationControlHelper.modifyControlFrame(actualDesiredOrienation_c2, c1_orientation, c2_orientation); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(expectedDesiredPosition_c2, actualDesiredPosition_c2, epsilon); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(expectedDesiredOrientation_c2, actualDesiredOrienation_c2, epsilon); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(expectedDesiredOrientation_c2, actualDesiredOrienation_c2, epsilon);
swingTrajectory.compute(trajectoryDuration); swingTrajectory.getPose(tempPose); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(finalOrientation, tempPose.getOrientation(), EPSILON); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(finalPosition, tempPose.getPosition(), EPSILON); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(stateA.getPose().getOrientation(), stateB.getPose().getOrientation(), 1e-1);
EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(initialOrientation, orientation, 1e-1);