public static boolean quaternionEpsilonEquals(Quat4d original, Quat4d result, double epsilon) { if (original.epsilonEquals(result, epsilon)) { return true; } else { Quat4d originalNegated = quaternionForEpsilonEquals.get(); originalNegated.negate(original); return originalNegated.epsilonEquals(result, epsilon); } }
temp.negate(orientation); orientationEquals = temp.epsilonEquals(footstepData.orientation, epsilon);
@Override public boolean epsilonEquals(HandstepPacket handstepPacket, double epsilon) { boolean robotSideEquals = robotSide == handstepPacket.robotSide; boolean locationEquals = location.epsilonEquals(handstepPacket.location, epsilon); boolean orientationEquals = orientation.epsilonEquals(handstepPacket.orientation, epsilon); if (!orientationEquals) { Quat4d temp = new Quat4d(); temp.negate(orientation); orientationEquals = temp.epsilonEquals(handstepPacket.orientation, epsilon); } return robotSideEquals && locationEquals && orientationEquals; }
public void interpolate(double alpha, Quat4d q0, Quat4d q1, Quat4d qInterpolatedToPack, boolean preventExtraSpin) { tempQ1ForInterpolation.set(q1); if (preventExtraSpin && dot(q0, tempQ1ForInterpolation) < 0.0) { tempQ1ForInterpolation.negate(); } computeQuaternionDifference(q0, tempQ1ForInterpolation, qInterpolatedToPack); pow(qInterpolatedToPack, alpha, qInterpolatedToPack); qInterpolatedToPack.mul(q0, qInterpolatedToPack); }
private void computeAngularVelocity(Vector3d angularVelocityToPack, Quat4d startRotationQuaternion, Quat4d endRotationQuaternion, double alphaDot) { if (quaternionCalculus.dot(startRotationQuaternion, endRotationQuaternion) < 0.0) endRotationQuaternion.negate(); // compute relative orientation: orientation of interpolated frame w.r.t. start frame relativeRotationQuaternion.set(startRotationQuaternion); // R_W_S: orientation of start w.r.t. world relativeRotationQuaternion.conjugate(); // R_S_W: orientation of world w.r.t. start relativeRotationQuaternion.mul(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); quaternionCalculus.transform(endRotationQuaternion, angularVelocityToPack); } }
temp.negate(orientation); orientationEquals = temp.epsilonEquals(footstepData.orientation, epsilon);
private void computeOrientationError() { orientationEstimator.getEstimatedOrientation(estimatedOrientation); Quat4d estimatedOrientationQuat4d = new Quat4d(); estimatedOrientation.getQuaternion(estimatedOrientationQuat4d); Quat4d actualOrientation = new Quat4d(); estimationJoint.getRotationToWorld(actualOrientation); if (((estimatedOrientationQuat4d.getW() > 0.0) && (actualOrientation.getW() < 0.0)) || ((estimatedOrientationQuat4d.getW() < 0.0) && (actualOrientation.getW() > 0.0))) { actualOrientation.negate(); } perfectOrientation.set(actualOrientation); Quat4d orientationErrorQuat4d = new Quat4d(actualOrientation); orientationErrorQuat4d.mulInverse(estimatedOrientationQuat4d); AxisAngle4d orientationErrorAxisAngle = new AxisAngle4d(); orientationErrorAxisAngle.set(orientationErrorQuat4d); double errorAngle = AngleTools.trimAngleMinusPiToPi(orientationErrorAxisAngle.getAngle()); orientationError.set(Math.abs(errorAngle)); }