How to use

setRotationVector

method

in

us.ihmc.euclid.tuple4D.Quaternion

public void add(QuaternionBasics orientation, Vector3DReadOnly rotationVector)
{
 tempRotation.setRotationVector(rotationVector);
 orientation.preMultiply(tempRotation);
}

private QuaternionReadOnly exp(double alpha, Vector3DReadOnly rotation)
{
 tempLogExpVector3D.setAndScale(alpha, rotation);
 tempLogExpQuaternion.setRotationVector(tempLogExpVector3D);
 return tempLogExpQuaternion;
}

/**
* Integrates the given {@code angularVelocity} to estimate the {@code finalOrientation}. The
* given velocity is expected to be expressed in the local frame described by the given
* {@code initialOrientation}.
* <p>
* Pseudo-code:
* 
* <pre>
* finalOrientation = initialOrientation + dt * angularVelocity
* </pre>
* </p>
* 
* @param angularVelocity the angular velocity to integrate. Not modified.
* @param initialOrientation the initial orientation to append the integrated angular velocity
*           to. Not modified.
* @param finalOrientation the estimated orientation after integration. Modified.
*/
public void integrate(Vector3DReadOnly angularVelocity, Orientation3DReadOnly initialOrientation, Orientation3DBasics finalOrientation)
{
 rotationVector.setAndScale(dt, angularVelocity);
 integrated.setRotationVector(rotationVector);
 finalOrientation.set(initialOrientation);
 finalOrientation.append(integrated);
}

integrated.setRotationVector(rotationVector);
finalOrientation.set(initialOrientation);
finalOrientation.append(integrated);

angularVelocityTwo.scale(integrationDT);
quaternionFutureOne.setRotationVector(angularVelocityOne);
quaternionFutureTwo.setRotationVector(angularVelocityTwo);

difference.setRotationVector(limitedRotationVector);
multiply(difference);

public void update(Vector3DReadOnly inputAngularVelocity, Vector3DReadOnly inputLinearAcceleration, Vector3DReadOnly inputMagneticVector)
{
 if (!hasBeenInitialized.getValue())
 {
   initialize(inputLinearAcceleration, inputMagneticVector);
   return;
 }
 boolean success = computeOrientationError((QuaternionReadOnly) estimatedOrientation, inputLinearAcceleration, inputMagneticVector, quaternionUpdate);
 if (success)
 {
   quaternionUpdate.getRotationVector(totalError);
   yoErrorTerm.set(totalError);
   integralTerm.scaleAdd(integralGain.getValue() * updateDT, yoErrorTerm, yoIntegralTerm);
   yoIntegralTerm.set(integralTerm);
   angularVelocityTerm.scaleAdd(proportionalGain.getValue(), totalError, inputAngularVelocity);
   angularVelocityTerm.add(integralTerm);
 }
 else
 {
   yoErrorTerm.setToZero();
   angularVelocityTerm.set(inputAngularVelocity);
 }
 rotationUpdate.setAndScale(updateDT, angularVelocityTerm);
 quaternionUpdate.setRotationVector(rotationUpdate);
 estimatedOrientation.multiply(quaternionUpdate);
 if (estimatedAngularVelocity != null)
   estimatedAngularVelocity.add(inputAngularVelocity, integralTerm);
}

difference.setRotationVector(limitedRotationVector);
quaternionFiltered.multiply(difference);
set(quaternionFiltered);

actualQuaternion.setRotationVector(rotationVector);
EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedQuaternion, actualQuaternion, EPS);

assertEquals(initialFrame, frameQuaternion.getReferenceFrame());
frameQuaternion.setRotationVectorIncludingFrame(newFrame, rotationVector);
quaternion.setRotationVector(rotationVector);
assertEquals(newFrame, frameQuaternion.getReferenceFrame());
EuclidCoreTestTools.assertTuple4DEquals(quaternion, frameQuaternion, EPSILON);
assertEquals(initialFrame, frameQuaternion.getReferenceFrame());
frameQuaternion.setRotationVectorIncludingFrame(rotationVector);
quaternion.setRotationVector(rotationVector);
assertEquals(newFrame, frameQuaternion.getReferenceFrame());
EuclidCoreTestTools.assertTuple4DEquals(quaternion, frameQuaternion, EPSILON);