public void computeQDDot(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DReadOnly angularAcceleration, Vector4DBasics qDDotToPack) { computeAngularVelocityInWorldFrame(q, qDot, intermediateAngularVelocity); computeQDDot(q, qDot, intermediateAngularVelocity, angularAcceleration, qDDotToPack); }
public void computeAngularAcceleration(Quat4d q, Quat4d qDDot, Vector3d angularVelocity, Vector3d angularAccelerationToPack) { computeQDot(q, angularVelocity, intermediateQDot); computeAngularAcceleration(q, intermediateQDot, qDDot, angularAccelerationToPack); }
/** * Interpolation from q0 to q1 for a given alpha = [0, 1] using SLERP. * Computes: resultToPack = q0 (q0^-1 q1)^alpha. */ public void interpolate(double alpha, Quat4d q0, Quat4d q1, Quat4d qInterpolatedToPack) { interpolate(alpha, q0, q1, qInterpolatedToPack, true); }
public void computeQDDot(QuaternionReadOnly q, Vector3DReadOnly angularVelocity, Vector3DReadOnly angularAcceleration, Vector4DBasics qDDotToPack) { computeQDot(q, angularVelocity, intermediateQDot); computeQDDot(q, intermediateQDot, angularVelocity, angularAcceleration, qDDotToPack); }
finalOrientationDrifted.set(finalQuaternionDrifted); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolatedNext, dtForFiniteDifference, qDot); quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolated, qInterpolatedNext, dtForFiniteDifference, qDDot); quaternionCalculus.computeAngularVelocityInWorldFrame(qInterpolated, qDot, tempAngularVelocity); quaternionCalculus.computeAngularAcceleration(qInterpolated, qDot, qDDot, tempAngularAcceleration);
public static FrameVector3D computeAngularVelocityByFiniteDifference(double dt, ReferenceFrame bodyFrame, ReferenceFrame bodyFrameInFuture) { FrameQuaternion bodyOrientation = new FrameQuaternion(bodyFrame); bodyOrientation.changeFrame(worldFrame); FrameQuaternion bodyOrientationInFuture = new FrameQuaternion(bodyFrameInFuture); bodyOrientationInFuture.changeFrame(worldFrame); FrameVector3D bodyAngularVelocity = new FrameVector3D(worldFrame); QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDotByFiniteDifferenceCentral(bodyOrientation, bodyOrientationInFuture, 0.5 * dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(bodyOrientation, qDot, bodyAngularVelocity); bodyAngularVelocity.changeFrame(bodyFrame); return bodyAngularVelocity; }
QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); double length = RandomNumbers.nextDouble(random, 0.0, 10.0); Vector4D qDDot = new Vector4D(); quaternionCalculus.computeQDot(q, angularVelocity, qDot); quaternionCalculus.computeQDDot(q, qDot, expectedAngularAcceleration, qDDot); quaternionCalculus.computeAngularAcceleration(q, qDot, qDDot, actualAngularAcceleration); assertTrue(expectedAngularAcceleration.epsilonEquals(actualAngularAcceleration, EPSILON)); quaternionCalculus.computeQDDot(q, angularVelocity, actualAngularAcceleration, qDDot); quaternionCalculus.computeAngularAcceleration(q, qDot, qDDot, actualAngularAcceleration); assertTrue(expectedAngularAcceleration.epsilonEquals(actualAngularAcceleration, EPSILON)); quaternionCalculus.computeQDDot(q, qDot, angularVelocity, actualAngularAcceleration, qDDot); quaternionCalculus.computeAngularAcceleration(q, qDot, qDDot, actualAngularAcceleration); assertTrue(expectedAngularAcceleration.epsilonEquals(actualAngularAcceleration, EPSILON)); quaternionCalculus.computeQDDot(q, qDot, expectedAngularAcceleration, qDDot); quaternionCalculus.computeAngularAcceleration(q, qDDot, angularVelocity, actualAngularAcceleration); assertTrue(expectedAngularAcceleration.epsilonEquals(actualAngularAcceleration, EPSILON));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConversionQDotToAngularVelocityBackAndForth() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); double length = RandomNumbers.nextDouble(random, 0.0, 10.0); Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random, length); if (random.nextBoolean()) expectedAngularVelocity.negate(); Vector3D actualAngularVelocity = new Vector3D(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDot(q, expectedAngularVelocity, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, EPSILON)); } }
initialAngularVelocity.get(tempAngularVelocity); quaternionCalculus.invertTransform(tempControlQuaternions[0], tempAngularVelocity); tempAngularVelocity.scale(trajectoryTime.getDoubleValue()); tempAngularVelocity.scale(1 / 3.0); quaternionCalculus.exp(tempAngularVelocity, tempQuatForControlQuats); finalAngularVelocity.get(tempAngularVelocity); quaternionCalculus.invertTransform(tempControlQuaternions[3], tempAngularVelocity); tempAngularVelocity.scale(trajectoryTime.getDoubleValue()); tempAngularVelocity.scale(1 / 3.0); tempAngularVelocity.negate(); quaternionCalculus.exp(tempAngularVelocity, tempQuatForControlQuats); tempControlQuaternions[2].mul(tempQuatForControlQuats); quaternionCalculus.inverseMultiply(tempControlQuaternions[i - 1], tempControlQuaternions[i], tempQuatForControlQuats); quaternionCalculus.log(tempQuatForControlQuats, tempAngularVelocity); controlAngularVelocities[i].set(tempAngularVelocity);
quaternionCalculus.computeQDotByFiniteDifferenceCentral(previousDesiredOrientation, currentDesiredOrientation, controllerDT, derivative); Vector3D angularVelocity = new Vector3D(); quaternionCalculus.computeAngularVelocityInWorldFrame(currentDesiredOrientation, derivative, angularVelocity); double speed = angularVelocity.length(); if (speed > maxSpeed.getDoubleValue())
public void testContinuityForSlowTrajectory() throws Exception QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); Random random = new Random(5165165161L); previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt; maxVelocityRecorded = Math.max(maxVelocityRecorded, velocityFD);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLogAndExpAlgebra() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); Vector4D qLog = new Vector4D(); Quaternion vExp = new Quaternion(); quaternionCalculus.log(q, qLog); Vector3D v = new Vector3D(qLog.getX(),qLog.getY(),qLog.getZ()); quaternionCalculus.exp(v, vExp); assertTrue(Math.abs(q.getX() - vExp.getX()) < 10e-10); assertTrue(Math.abs(q.getY() - vExp.getY()) < 10e-10); assertTrue(Math.abs(q.getZ() - vExp.getZ()) < 10e-10); assertTrue(Math.abs(q.getS() - vExp.getS()) < 10e-10); } }
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); } }
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); }
public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack, boolean preventExtraSpin) { tempQ1ForInterpolation.set(q1); if (preventExtraSpin && q0.dot(tempQ1ForInterpolation) < 0.0) { tempQ1ForInterpolation.negate(); } computeQuaternionDifference(q0, tempQ1ForInterpolation, qInterpolatedToPack); pow(qInterpolatedToPack, alpha, qInterpolatedToPack); qInterpolatedToPack.multiply(q0, qInterpolatedToPack); }
angularVelocity.scale(maximumSixDoFJointAngularVelocity / angularVelocityMagnitude); rotationVectorIntegrated.scale(controlDT, angularVelocity); quaternionCalculus.exp(rotationVectorIntegrated, rotationIntegrated); linearVelocity.scale(maximumSixDoFJointLinearVelocity / linearVelocityMagnitude); translationIntegrated.scale(controlDT, linearVelocity); quaternionCalculus.transform(rotation, translationIntegrated);
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testInterpolateAgainstQuat4d() throws Exception { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Random random = new Random(6546545L); Quaternion q0 = RandomGeometry.nextQuaternion(random); Quaternion q1 = RandomGeometry.nextQuaternion(random); Quaternion expectedQInterpolated = new Quaternion(); Quaternion actualQInterpolated = new Quaternion(); for (double alpha = 0.0; alpha <= 1.0; alpha += 1.0e-6) { expectedQInterpolated.interpolate(q0, q1, alpha); quaternionCalculus.interpolate(alpha, q0, q1, actualQInterpolated); assertTrue(expectedQInterpolated.epsilonEquals(actualQInterpolated, EPSILON)); } } }
private void computeBezierQuaternionCurveTerm(int i, Quat4d resultToPack) { double cumulativeBernsteinCoefficient = cumulativeBeziers[i].getDoubleValue(); controlAngularVelocities[i].get(tempAngularVelocity); tempAngularVelocity.scale(cumulativeBernsteinCoefficient); quaternionCalculus.exp(tempAngularVelocity, resultToPack); }
private void computeAngularVelocity(Vector3D angularVelocityToPack, Quaternion startRotationQuaternion, Quaternion endRotationQuaternion, double alphaDot) { if (startRotationQuaternion.dot(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.multiply(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); endRotationQuaternion.transform(angularVelocityToPack); } }
interpolateOrientation(time, qInterpolated); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolatedNext, dtForFiniteDifference, qDot); quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolated, qInterpolatedNext, dtForFiniteDifference, qDDot); quaternionCalculus.computeAngularVelocityInWorldFrame(qInterpolated, qDot, tempAngularVelocity); quaternionCalculus.computeAngularAcceleration(qInterpolated, qDot, qDDot, tempAngularAcceleration);