public void computeQDDot(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DReadOnly angularAcceleration, Vector4DBasics qDDotToPack) { computeAngularVelocityInWorldFrame(q, qDot, intermediateAngularVelocity); computeQDDot(q, qDot, intermediateAngularVelocity, angularAcceleration, qDDotToPack); }
public void computeQDDot(Quat4d q, Quat4d qDot, Vector3d angularAcceleration, Quat4d qDDotToPack) { computeAngularVelocityInWorldFrame(q, qDot, intermediateAngularVelocity); computeQDDot(q, qDot, intermediateAngularVelocity, angularAcceleration, qDDotToPack); }
@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)); } }
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); double speed = angularVelocity.length(); if (speed > maxSpeed.getDoubleValue())
@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)); } } }
quaternionCalculus.computeAngularVelocityInWorldFrame(qCurrent, qDot, actualAngularVelocity);
quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolated, qInterpolatedNext, dtForFiniteDifference, qDDot); quaternionCalculus.computeAngularVelocityInWorldFrame(qInterpolated, qDot, tempAngularVelocity); quaternionCalculus.computeAngularAcceleration(qInterpolated, qDot, qDDot, tempAngularAcceleration);
quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qInterpolatedPrevious, qInterpolated, qInterpolatedNext, dtForFiniteDifference, qDDot); quaternionCalculus.computeAngularVelocityInWorldFrame(qInterpolated, qDot, tempAngularVelocity); quaternionCalculus.computeAngularAcceleration(qInterpolated, qDot, qDDot, tempAngularAcceleration);
quaternionCalculus.computeAngularVelocityInWorldFrame(qInterpolated, qDot, tempAngularVelocity); quaternionCalculus.computeAngularAcceleration(qInterpolated, qDot, qDDot, tempAngularAcceleration);
quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity);