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.computeQDotByFiniteDifferenceCentral(previousDesiredOrientation, currentDesiredOrientation, controllerDT, derivative); Vector3D angularVelocity = new Vector3D(); quaternionCalculus.computeAngularVelocityInWorldFrame(currentDesiredOrientation, derivative, angularVelocity);
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testIntegrateToQuaternion() throws Exception { for (int i = 0; i < 100; i++) { Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random); Vector3D actualAngularVelocity = new Vector3D(); Quaternion integrationResultPrevious = new Quaternion(); Quaternion integrationResultCurrent = new Quaternion(); Quaternion integrationResultNext = new Quaternion(); Vector4D qDot = new Vector4D(); QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); double dt = 1.0e-4; for (double t = dt; t <= 1.0; t += dt) { RotationTools.integrateAngularVelocity(expectedAngularVelocity, t - dt, integrationResultPrevious); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t, integrationResultCurrent); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t + dt, integrationResultNext); quaternionCalculus.computeQDotByFiniteDifferenceCentral(integrationResultPrevious, integrationResultNext, dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(integrationResultCurrent, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, 1.0e-7)); } } }
qNext.set(axisAngleNext); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qPrevious, qNext, dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(qCurrent, qDot, actualAngularVelocity);
interpolateOrientation(time, qInterpolated); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolatedNext, dtForFiniteDifference, qDot);
finalOrientationDrifted.set(finalQuaternionDrifted); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolatedNext, dtForFiniteDifference, qDot); quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolated, qInterpolatedNext, dtForFiniteDifference, qDDot);
finalOrientationDrifted.set(finalQuaternionDrifted); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolatedNext, dtForFiniteDifference, qDot); quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolated, qInterpolatedNext, dtForFiniteDifference, qDDot);
qNext.set(orientation); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qPrevious, qNext, dtForFD, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity);
qNext.set(orientation); quaternionCalculus.computeQDotByFiniteDifferenceCentral(qPrevious, qNext, dtForFD, qDot); quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qPrevious, q, qNext, dtForFD, qDDot); quaternionCalculus.computeAngularAcceleration(q, qDot, qDDot, actualAngularAcceleration);