public double getAxisAngleRotationToOtherPose(FramePose otherPose, FrameVector rotationAxisToPack) { AxisAngle4d rotationAxisAngle = new AxisAngle4d(); getAxisAngleRotationToOtherPose(otherPose, rotationAxisAngle); rotationAxisToPack.setIncludingFrame(this.referenceFrame, rotationAxisAngle.getX(), rotationAxisAngle.getY(), rotationAxisAngle.getZ()); return rotationAxisAngle.getAngle(); }
/** * Calculate the rotation angle component of the input unit quaternion. * * @param q * unit quaternion Quat4d * @return the angle in radians of the input quaternion */ public static double angle(Quat4d q) { AxisAngle4d axis = new AxisAngle4d(); axis.set(q); return axis.angle; }
public static boolean axisAngleEpsilonEqualsIgnoreFlippedAxes(AxisAngle4d original, AxisAngle4d result, double epsilon) { if (original.epsilonEquals(result, epsilon)) { return true; } else { AxisAngle4d originalNegated = originalAxisAngleForEpsilonEquals.get(); originalNegated.set(original); originalNegated.setAngle(originalNegated.getAngle() * -1.0); originalNegated.setX(originalNegated.getX() * -1.0); originalNegated.setY(originalNegated.getY() * -1.0); originalNegated.setZ(originalNegated.getZ() * -1.0); return originalNegated.epsilonEquals(result, epsilon); } } }
@Override public AxisAngle4d initialValue() { return new AxisAngle4d(); } };
public static void assertQuaternionsEqualUsingDifference(Quat4d q1, Quat4d q2, double epsilon) { try { Quat4d qDifference = new Quat4d(); qDifference.mulInverse(q1, q2); AxisAngle4d axisAngle = new AxisAngle4d(); axisAngle.set(qDifference); assertEquals(0.0, axisAngle.getAngle(), epsilon); } catch (AssertionError e) { throw new AssertionError("expected:\n<" + q1 + ">\n but was:\n<" + q2 + ">"); } } }
public double getOrientationDistance(FramePose framePose) { AxisAngle4d rotationFromThatToThis = new AxisAngle4d(); getAxisAngleRotationToOtherPose(framePose, rotationFromThatToThis); return Math.abs(rotationFromThatToThis.getAngle()); }
public void getAxisAngle(AxisAngle4d axisAngleToPack) { axisAngleToPack.set(quaternion); }
/** * This computes: resultToPack = q^power. * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result. */ public void pow(Quat4d q, double power, Quat4d resultToPack) { axisAngleForPow.set(q); axisAngleForPow.setAngle(power * axisAngleForPow.getAngle()); resultToPack.set(axisAngleForPow); }
public void startComputation() { imuMount.getOrientation(rotationMatrix); yoFrameQuaternionPerfect.set(rotationMatrix); corrupt(rotationMatrix); orientationOutputPort.setData(rotationMatrix); yoFrameQuaternionNoisy.set(rotationMatrix); yoFrameQuaternionPerfect.get(tempQuaternionOne); yoFrameQuaternionNoisy.get(tempQuaternionTwo); tempQuaternionTwo.inverse(); tempQuaternionOne.mul(tempQuaternionTwo); tempAxisAngle.set(tempQuaternionOne); double noiseAngle = tempAxisAngle.getAngle(); if (noiseAngle > Math.PI) noiseAngle = noiseAngle - 2.0 * Math.PI; if (noiseAngle < -Math.PI) noiseAngle = noiseAngle + 2.0 * Math.PI; rotationAngleNoise.set(Math.abs(noiseAngle)); }
public boolean checkIfErrorIsTooBig(FramePose correctedPelvisPoseInWorldFrame, FramePose iterativeClosestPointInWorldFramePose, boolean isRotationCorrectionEnabled) { correctedPelvisPoseReferenceFrame.setPoseAndUpdate(correctedPelvisPoseInWorldFrame); iterativeClosestPointInWorldFramePose.getOrientationIncludingFrame(iterativeClosestPointOrientation); iterativeClosestPointInWorldFramePose.getPositionIncludingFrame(iterativeClosestPointTranslation); iterativeClosestPointOrientation.changeFrame(correctedPelvisPoseReferenceFrame); iterativeClosestPointTranslation.changeFrame(correctedPelvisPoseReferenceFrame); iterativeClosestPointOrientation.getAxisAngle(axisAngleForError); angleError.set(Math.abs(axisAngleForError.getAngle())); translationErrorX.set(Math.abs(iterativeClosestPointTranslation.getX())); translationErrorY.set(Math.abs(iterativeClosestPointTranslation.getY())); translationErrorZ.set(Math.abs(iterativeClosestPointTranslation.getZ())); if(isRotationCorrectionEnabled && Math.abs(axisAngleForError.getAngle()) > Math.toRadians(maximumErrorAngleInDegrees.getDoubleValue())) return true; if(Math.abs(iterativeClosestPointTranslation.getX()) > maximumErrorTranslation.getDoubleValue()) return true; if(Math.abs(iterativeClosestPointTranslation.getY()) > maximumErrorTranslation.getDoubleValue()) return true; if(Math.abs(iterativeClosestPointTranslation.getZ()) > maximumErrorTranslation.getDoubleValue()) return true; return false; }
trimAxisAngleMinusPiToPi(result, resultMinusPiToPi); boolean originalAxisAngleIsZero = MathTools.epsilonEquals(originalMinusPiToPi.getAngle(), 0.0, 0.1 * epsilon); boolean resultAxisAngleIsZero = MathTools.epsilonEquals(resultMinusPiToPi.getAngle(), 0.0, 0.1 * epsilon); boolean originalAngleIs180 = MathTools.epsilonEquals(Math.abs(originalMinusPiToPi.getAngle()), Math.PI, 0.1 * epsilon); boolean resultAngleIs180 = MathTools.epsilonEquals(Math.abs(resultMinusPiToPi.getAngle()), Math.PI, 0.1 * epsilon); return original.epsilonEquals(result, epsilon);
@Override public AxisAngle4d initialValue() { return new AxisAngle4d(); } };
axisAngle.setX(rot21 - rot12); axisAngle.setY(rot02 - rot20); axisAngle.setZ(rot10 - rot01); double mag = axisAngle.getX() * axisAngle.getX() + axisAngle.getY() * axisAngle.getY() + axisAngle.getZ() * axisAngle.getZ(); double cos = 0.5 * (rot00 + rot11 + rot22 - 1.0); axisAngle.setAngle(Math.atan2(sin, cos)); axisAngle.setX(axisAngle.getX() * invMag); axisAngle.setY(axisAngle.getY() * invMag); axisAngle.setZ(axisAngle.getZ() * invMag); axisAngle.setX(0.0); axisAngle.setY(1.0); axisAngle.setZ(0.0); axisAngle.setAngle(0.0);
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)); }
public static double getMagnitudeOfAngleOfRotation(RigidBodyTransform rigidBodyTransform) { AxisAngle4d axisAngle4d = new AxisAngle4d(); rigidBodyTransform.getRotation(axisAngle4d); return Math.abs(axisAngle4d.getAngle()); }
public void getOrientation(AxisAngle4d axisAngleToPack) { axisAngleToPack.set(orientation); }
public void startComputation() { frameUsedForPerfectOrientation.getTransformToDesiredFrame(worldFrame).getRotation(rotationMatrix); yoFrameQuaternionPerfect.set(rotationMatrix); corrupt(rotationMatrix); orientationOutputPort.setData(rotationMatrix); yoFrameQuaternionNoisy.set(rotationMatrix); yoFrameQuaternionPerfect.get(tempQuaternionOne); yoFrameQuaternionNoisy.get(tempQuaternionTwo); tempQuaternionTwo.inverse(); tempQuaternionOne.mul(tempQuaternionTwo); tempAxisAngle.set(tempQuaternionOne); double noiseAngle = tempAxisAngle.getAngle(); if (noiseAngle > Math.PI) noiseAngle = noiseAngle - 2.0 * Math.PI; if (noiseAngle < -Math.PI) noiseAngle = noiseAngle + 2.0 * Math.PI; rotationAngleNoise.set(Math.abs(noiseAngle)); }
private void updateMaxRotationAlphaVariationSpeed() { //Rotation rotationToTravel.setOrientationFromOneToTwo(updatedStartOffset_Rotation, updatedGoalOffsetWithDeadZone_Rotation); rotationToTravel.getAxisAngle(axisAngletoTravel); angleToTravel.set(axisAngletoTravel.getAngle()); rotationalSpeedForGivenAngleToTravel.set(angleToTravel.getDoubleValue() / dt.getDoubleValue()); }