public static void convertRotationVectorToMatrix(Vector3d rotationVector, Matrix3d rotationMatrixToPack) { AxisAngle4d localAxisAngle = axisAngleForRotationVectorConvertor.get(); convertRotationVectorToAxisAngle(rotationVector, localAxisAngle); rotationMatrixToPack.set(localAxisAngle); }
public static void convertRotationVectorToQuaternion(Vector3d rotationVector, Quat4d quaternionToPack) { AxisAngle4d localAxisAngle = axisAngleForRotationVectorConvertor.get(); convertRotationVectorToAxisAngle(rotationVector, localAxisAngle); quaternionToPack.set(localAxisAngle); }
public static void integrateAngularVelocity(Vector3d angularVelocityToIntegrate, double integrationTime, AxisAngle4d orientationResultToPack) { Vector3d angularVelocityIntegrated = angularVelocityForIntegrator.get(); angularVelocityIntegrated.set(angularVelocityToIntegrate); angularVelocityIntegrated.scale(integrationTime); convertRotationVectorToAxisAngle(angularVelocityIntegrated, orientationResultToPack); }
public void correctState(DenseMatrix64F correction) { orientationPort.getData().getQuaternion(quaternion); MatrixTools.extractTuple3dFromEJMLVector(tempRotationVector, correction, 0); RotationTools.convertRotationVectorToAxisAngle(tempRotationVector, tempAxisAngle); quaternionDelta.set(tempAxisAngle); quaternion.mul(quaternionDelta); orientation.set(quaternion); orientationPort.setData(orientation); } }
public void propagateState(double dt) { angularVelocityPort.getData().get(angularVelocity); orientationPort.getData().getQuaternion(quaternion); tempRotationVector.set(angularVelocity); tempRotationVector.scale(dt); RotationTools.convertRotationVectorToAxisAngle(tempRotationVector, tempAxisAngle); quaternionDelta.set(tempAxisAngle); quaternion.mul(quaternionDelta); quaternion.normalize(); // the previous operation should preserve norm, so this might not be necessary every step orientation.set(quaternion); orientationPort.setData(orientation); }
private void applyCorrection(FramePoint desiredPosition, FrameOrientation desiredOrientation) { ReferenceFrame originalFrame = desiredPosition.getReferenceFrame(); desiredPosition.changeFrame(controlFrame); desiredOrientation.changeFrame(controlFrame); desiredPosition.sub(totalLinearCorrection); totalAngularCorrection.negate(); RotationTools.convertRotationVectorToAxisAngle(totalAngularCorrection.getVector(), angularDisplacementAsAxisAngle); angularDisplacementAsMatrix.set(angularDisplacementAsAxisAngle); desiredOrientation.getMatrix3d(correctedRotationMatrix); correctedRotationMatrix.mul(angularDisplacementAsMatrix, correctedRotationMatrix); desiredOrientation.set(correctedRotationMatrix); desiredPosition.changeFrame(originalFrame); desiredOrientation.changeFrame(originalFrame); }