/** * 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); }
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); } }
/** * Asserts on a per component basis that the two frame convex polygon 2D 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 convex polygon 2D. Not modified. * @param actual the actual frame convex polygon 2D. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame convex polygon 2D are not equal. If only one of the * arguments is equal to {@code null}. */ public static void assertFrameConvexPolygon2DEquals(FrameConvexPolygon2DReadOnly expected, FrameConvexPolygon2DReadOnly actual, double epsilon) { assertFrameConvexPolygon2DEquals(null, expected, actual, epsilon); }
private void assertGeometricEquals(FramePose3D poseA, FramePose3D poseB, double epsilon) { EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(poseA.getPosition(), poseB.getPosition(), epsilon); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(poseA.getOrientation(), poseB.getOrientation(), epsilon); }
/** * Asserts on a per component basis that the two frame points 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 point. Not modified. * @param actual the actual frame point. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame points 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 assertFramePoint3DGeometricallyEquals(FramePoint3DReadOnly expected, FramePoint3DReadOnly actual, double epsilon) { assertFramePoint3DGeometricallyEquals(null, expected, actual, 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);
actualCoMAcceleration); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(expectedCoMPosition, actualCoMPosition, EPSILON); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(expectedCoMVelocity, actualCoMVelocity, EPSILON); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(expectedCoMAcceleration, actualCoMAcceleration, 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 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 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 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 assertFrameTuple4DEquals(FrameTuple4DReadOnly expected, FrameTuple4DReadOnly actual, double epsilon) { assertFrameTuple4DEquals(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 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 assertFrameTuple2DEquals(FrameTuple2DReadOnly expected, FrameTuple2DReadOnly actual, double epsilon) { assertFrameTuple2DEquals(null, expected, actual, epsilon); }
/** * Asserts on a per component basis that the two frame vectors 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 vector. Not modified. * @param actual the actual frame vector. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame vectors 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 assertFrameVector3DGeometricallyEquals(FrameVector3DReadOnly expected, FrameVector3DReadOnly actual, double epsilon) { assertFrameVector3DGeometricallyEquals(null, expected, actual, epsilon); }
/** * Asserts on a per component basis that the frame yaw-pitch-roll orientations are equal to an * {@code epsilon}. * <p> * The method returns {@code true} for angles such as: * {@code actualAngle = expectedAngle +/- 2.0 * Math.PI}. * </p> * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param expected the expected frame yaw-pitch-roll orientation. Not modified. * @param actual the actual frame yaw-pitch-roll orientation. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two yaw-pitch-rolls are not equal or not expressed in the same * reference frame. If only one of the arguments is equal to {@code null}. */ public static void assertFrameYawPitchRollEquals(FrameYawPitchRollReadOnly expected, FrameYawPitchRollReadOnly actual, double epsilon) { assertFrameYawPitchRollEquals(null, expected, actual, epsilon); }
/** * Asserts on a per component basis that the two frame points 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 point. Not modified. * @param actual the actual frame point. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame points 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 assertFramePoint2DGeometricallyEquals(FramePoint2DReadOnly expected, FramePoint2DReadOnly actual, double epsilon) { assertFramePoint2DGeometricallyEquals(null, expected, actual, epsilon); }
RigidBodyPositionControlHelper.modifyControlFrame(actualDesiredPosition_c2, actualDesiredOrienation_c2, tempC1ToBody, c2ToBody); 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);
/** * Asserts on a per component basis that the two frame points 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 automated message. * @param expected the expected frame point. Not modified. * @param actual the actual frame point. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame points 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 assertFramePoint3DGeometricallyEquals(String messagePrefix, FramePoint3DReadOnly expected, FramePoint3DReadOnly actual, double epsilon) { assertFramePoint3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
/** * 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); }
/** * 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 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 assertFrameTuple4DEquals(String messagePrefix, FrameTuple4DReadOnly expected, FrameTuple4DReadOnly actual, double epsilon) { assertFrameTuple4DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
/** * 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 automated 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 assertFrameTuple2DEquals(String messagePrefix, FrameTuple2DReadOnly expected, FrameTuple2DReadOnly actual, double epsilon) { assertFrameTuple2DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
/** * Asserts on a per component basis that the two frame vectors 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 automated message. * @param expected the expected frame vector. Not modified. * @param actual the actual frame vector. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two frame vectors 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 assertFrameVector3DGeometricallyEquals(String messagePrefix, FrameVector3DReadOnly expected, FrameVector3DReadOnly actual, double epsilon) { assertFrameVector3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
/** * Asserts on a per component basis that the frame yaw-pitch-roll orientations are equal to an * {@code epsilon}. * <p> * The method returns {@code true} for angles such as: * {@code actualAngle = expectedAngle +/- 2.0 * Math.PI}. * </p> * <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 yaw-pitch-roll orientation. Not modified. * @param actual the actual frame yaw-pitch-roll orientation. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two yaw-pitch-rolls are not equal or not expressed in the same * reference frame. If only one of the arguments is equal to {@code null}. */ public static void assertFrameYawPitchRollEquals(String messagePrefix, FrameYawPitchRollReadOnly expected, FrameYawPitchRollReadOnly actual, double epsilon) { assertFrameYawPitchRollEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }