public boolean epsilonEquals(Quat4d quaternion, double epsilon) { return RotationTools.quaternionEpsilonEquals(this.quaternion, quaternion, epsilon); }
public boolean epsilonEquals(FrameOrientation frameOrientation, double epsilon) { boolean referenceFramesMatch = referenceFrame == frameOrientation.referenceFrame; boolean quaternionsAreEqual = RotationTools.quaternionEpsilonEquals(quaternion, frameOrientation.quaternion, epsilon); return referenceFramesMatch && quaternionsAreEqual; }
public boolean epsilonEquals(RobotConfigurationData other, double epsilon) { if (!rootTranslation.epsilonEquals(other.rootTranslation, 1e-3f)) { return false; } if (!RotationTools.quaternionEpsilonEquals(rootOrientation, other.rootOrientation, 1e-3f)) { return false; } for (int i = 0; i < jointAngles.length; i++) { if (Math.abs(jointAngles[i] - other.jointAngles[i]) > epsilon) { System.out.println(i); System.out.println("Diff: " + Math.abs(jointAngles[i] - other.jointAngles[i]) + ", this: " + jointAngles[i] + ", other: " + other.jointAngles[i]); return false; } } return timestamp == other.timestamp; }
@Override public boolean epsilonEquals(FootstepPlanningRequestPacket other, double epsilon) { if (!initialStanceSide.equals(other.initialStanceSide)) return false; if (!stanceFootPositionInWorld.epsilonEquals(other.stanceFootPositionInWorld, (float) epsilon)) return false; if (!RotationTools.quaternionEpsilonEquals(stanceFootOrientationInWorld, other.stanceFootOrientationInWorld, (float) epsilon)) return false; if (!goalPositionInWorld.epsilonEquals(other.goalPositionInWorld, (float) epsilon)) return false; if (!RotationTools.quaternionEpsilonEquals(goalOrientationInWorld, other.goalOrientationInWorld, (float) epsilon)) return false; return true; }
@Override public boolean epsilonEquals(KinematicsToolboxOutputStatus other, double epsilon) { if (!desiredRootTranslation.epsilonEquals(other.desiredRootTranslation, 1e-3f)) { return false; } if (!RotationTools.quaternionEpsilonEquals(desiredRootOrientation, other.desiredRootOrientation, 1e-3f)) { return false; } if (!MathTools.epsilonEquals(solutionQuality, other.solutionQuality, epsilon)) return false; for (int i = 0; i < desiredJointAngles.length; i++) { if (Math.abs(desiredJointAngles[i] - other.desiredJointAngles[i]) > epsilon) { System.out.println(i); System.out.println("Diff: " + Math.abs(desiredJointAngles[i] - other.desiredJointAngles[i]) + ", this: " + desiredJointAngles[i] + ", other: " + other.desiredJointAngles[i]); return false; } } return true; } }
public boolean epsilonEquals(VehiclePosePacket other, double epsilon) { boolean ret = RotationTools.quaternionEpsilonEquals(this.getOrientation(), other.getOrientation(), epsilon); ret &= this.getPosition().epsilonEquals(other.getPosition(), epsilon); return ret; }
public boolean epsilonEquals(SpigotPosePacket other, double epsilon) { boolean ret = RotationTools.quaternionEpsilonEquals(this.getOrientation(), other.getOrientation(), epsilon); ret &= this.getPosition().epsilonEquals(other.getPosition(), epsilon); return ret; }
public boolean epsilonEquals(TorusPosePacket other, double epsilon) { boolean ret; final double thisRadius = this.getFingerHoleRadius(); final double otherRadius = other.getFingerHoleRadius(); final double delta = Math.abs(thisRadius - otherRadius); if (thisRadius == otherRadius) ret = true; else if ((thisRadius == 0) || (otherRadius == 0) || (delta < 1e-2)) ret = (delta < epsilon); else { ret = (delta / (thisRadius + otherRadius) < epsilon); } ret &= RotationTools.quaternionEpsilonEquals(this.getOrientation(), other.getOrientation(), epsilon); ret &= this.getPosition().epsilonEquals(other.getPosition(), epsilon); return ret; }
if (!RotationTools.quaternionEpsilonEquals(getOrientation(), other.getOrientation(), epsilon))
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomGetQuaternionFromYawAndZNormal() { int numTests = 100; Random random = new Random(7362L); Vector3D normal = new Vector3D(); Quaternion quatSolution = new Quaternion(); for (int i = 0; i < numTests; i++) { Quaternion quatToPack = RandomGeometry.nextQuaternion(random); RotationMatrix rotationMatrixToPack = new RotationMatrix(); double yaw = quatToPack.getYaw(); rotationMatrixToPack.set(quatToPack); rotationMatrixToPack.getColumn(2, normal); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); boolean quaternionsAreEqual = RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON); assertTrue(quaternionsAreEqual); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testMultiplication() { Random random = new Random(1776L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry); Quaternion quat4dActual = new Quaternion(), quat4dExpected = new Quaternion(); Quaternion quat4dA, quat4dB; FrameQuaternion frameOrientation = new FrameQuaternion(worldFrame); for (int i = 0; i < 1000; i++) { quat4dA = RandomGeometry.nextQuaternion(random); quat4dB = RandomGeometry.nextQuaternion(random); quat4dExpected.multiply(quat4dA, quat4dB); yoFrameQuaternion.set(quat4dA); yoFrameQuaternion.multiply(quat4dB); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); yoFrameQuaternion.set(quat4dA); frameOrientation.set(quat4dB); yoFrameQuaternion.multiply(frameOrientation); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetQuaternionFromYawAndZNormal() { double yaw = 1.0; double pitch = 0.4; double roll = 0.8; RotationMatrix rotationMatrixToPack = new RotationMatrix(); rotationMatrixToPack.setYawPitchRoll(yaw, pitch, roll); Vector3D normal = new Vector3D(); rotationMatrixToPack.getColumn(2, normal); Quaternion quatToPack = new Quaternion(); quatToPack.set(rotationMatrixToPack); Quaternion quatSolution = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); assertTrue(RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON)); yaw = -Math.PI - 0.01; pitch = Math.PI / 2.0 - 1e-3; roll = 0.8; rotationMatrixToPack.setYawPitchRoll(yaw, pitch, roll); normal = new Vector3D(); rotationMatrixToPack.getColumn(2, normal); quatToPack.set(rotationMatrixToPack); quatSolution = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); assertTrue(RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON)); }
yoFrameQuaternion.set(quat4dExpected); Quaternion quat4dActual = new Quaternion(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS));