/** * 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); }
/** * 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); }
private void assertGeometricEquals(FramePose3D poseA, FramePose3D poseB, double epsilon) { EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(poseA.getPosition(), poseB.getPosition(), epsilon); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(poseA.getOrientation(), poseB.getOrientation(), 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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAddAndSetIncludingFrameWithFramePoint() { FramePoint3D testLocation = new FramePoint3D(footSpoof.getSoleFrame(), Math.random(), Math.random(), Math.random()); assertTrue(copPointsInFoot.isEmpty()); copPointsInFoot.addWaypoint(CoPPointName.MIDFOOT_COP, 0.2, testLocation); assertEquals(1, copPointsInFoot.getCoPPointList().size()); testLocation.changeFrame(worldFrame); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation, copPointsInFoot.get(0).getPosition(), epsilon); assertEquals(0.2, copPointsInFoot.get(0).getTime(), epsilon); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAddAndSetIncludingFrameWithYoFramePoint() { YoFramePoint3D testLocation1 = new YoFramePoint3D("TestLocation1", footSpoof.getSoleFrame(), null); YoFramePoint3D testLocation2 = new YoFramePoint3D("TestLocation2", footSpoof.getSoleFrame(), null); testLocation1.set(Math.random(), Math.random(), Math.random()); testLocation2.set(Math.random(), Math.random(), Math.random()); copPointsInFoot.addWaypoint(CoPPointName.ENTRY_COP, 0.87, testLocation1); copPointsInFoot.addWaypoint(CoPPointName.MIDFOOT_COP, 0.12, testLocation2); assertEquals(2, copPointsInFoot.getCoPPointList().size()); FramePoint3D pointToTest = new FramePoint3D(testLocation1); pointToTest.changeFrame(worldFrame); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(pointToTest, copPointsInFoot.get(0).getPosition(), epsilon); assertEquals(0.87, copPointsInFoot.get(0).getTime(), epsilon); pointToTest = new FramePoint3D(testLocation2); pointToTest.changeFrame(worldFrame); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(pointToTest, copPointsInFoot.get(1).getPosition(), epsilon); assertEquals(0.12, copPointsInFoot.get(1).getTime(), epsilon); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddAndSetIncludingFrameWithCoPTrajectoryPoint() { CoPTrajectoryPoint testLocation1 = new CoPTrajectoryPoint("TestLocation1", "", null, framesToRegister); CoPTrajectoryPoint testLocation2 = new CoPTrajectoryPoint("TestLocation2", "", null, framesToRegister); testLocation1.changeFrame(footSpoof.getSoleFrame()); testLocation1.setPosition(new FramePoint3D(footSpoof.getSoleFrame(), Math.random(), Math.random(), Math.random())); testLocation2.changeFrame(footSpoof.getSoleFrame()); testLocation2.setPosition(new FramePoint3D(footSpoof.getSoleFrame(), Math.random(), Math.random(), Math.random())); copPointsInFoot.addWaypoint(CoPPointName.ENTRY_COP, 0.87, testLocation1); copPointsInFoot.addWaypoint(CoPPointName.MIDFOOT_COP, 0.12, testLocation2); assertEquals(2, copPointsInFoot.getCoPPointList().size()); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation1.getPosition(), copPointsInFoot.get(0).getPosition(), epsilon); assertEquals(0.87, copPointsInFoot.get(0).getTime(), epsilon); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation2.getPosition(), copPointsInFoot.get(1).getPosition(), epsilon); assertEquals(0.12, copPointsInFoot.get(1).getTime(), epsilon); FramePoint3D tempFramePointForTesting = new FramePoint3D(footSpoof.getSoleFrame()); copPointsInFoot.getFinalCoPPosition(tempFramePointForTesting); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation2.getPosition(), tempFramePointForTesting, 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);
actualCoMAcceleration); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(expectedCoMPosition, actualCoMPosition, EPSILON); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(expectedCoMVelocity, actualCoMVelocity, EPSILON); EuclidFrameTestTools.assertFrameVector3DGeometricallyEquals(expectedCoMAcceleration, actualCoMAcceleration, EPSILON);
swingTrajectory.getPose(tempPose); EuclidFrameTestTools.assertFrameQuaternionGeometricallyEquals(finalOrientation, tempPose.getOrientation(), EPSILON); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(finalPosition, tempPose.getPosition(), EPSILON);