@Override public boolean epsilonEquals(IMUPacket other, double epsilon) { boolean ret = linearAcceleration.epsilonEquals(other.linearAcceleration, (float) epsilon); ret &= orientation.epsilonEquals(other.orientation, (float) epsilon); ret &= angularVelocity.epsilonEquals(other.angularVelocity, (float) epsilon); ret &= MathTools.epsilonEquals(time, other.time, epsilon); return ret; } }
else if (desiredForce != null && other.desiredForce == null) return false; else if (!desiredForce.epsilonEquals(other.desiredForce, (float) epsilon)) return false; else if (desiredTorque != null && other.desiredTorque == null) return false; else if (!desiredTorque.epsilonEquals(other.desiredTorque, (float) epsilon)) return false;
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(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; } }
if (!regionOrigin.epsilonEquals(other.regionOrigin, (float) epsilon)) return false; if (!regionNormal.epsilonEquals(other.regionNormal, (float) epsilon)) return false; if (convexPolygonsVertices.size() != other.convexPolygonsVertices.size())