public void getAxisAngle(AxisAngle4d axisAngleToPack) { axisAngleToPack.set(quaternion); }
/** * This computes: resultToPack = q^power. * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result. */ public void pow(Quat4d q, double power, Quat4d resultToPack) { axisAngleForPow.set(q); axisAngleForPow.setAngle(power * axisAngleForPow.getAngle()); resultToPack.set(axisAngleForPow); }
public void getOrientation(AxisAngle4d axisAngleToPack) { axisAngleToPack.set(orientation); }
public static void trimAxisAngleMinusPiToPi(AxisAngle4d axisAngle4d, AxisAngle4d axisAngleTrimmedToPack) { axisAngleTrimmedToPack.set(axisAngle4d); axisAngleTrimmedToPack.setAngle(AngleTools.trimAngleMinusPiToPi(axisAngleTrimmedToPack.getAngle())); }
public void getAverageAxisAngle(AxisAngle4d averageAxisAngleToPack) { averageAxisAngleToPack.set(averageQuaternion); }
private void computeProportionalTerm(FrameOrientation desiredOrientation) { desiredOrientation.changeFrame(bodyFrame); desiredOrientation.getQuaternion(errorQuaternion); errorAngleAxis.set(errorQuaternion); errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle())); // Limit the maximum position error considered for control action double maximumError = gains.getMaximumProportionalError(); if (errorAngleAxis.getAngle() > maximumError) { errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError); } proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ()); proportionalTerm.scale(errorAngleAxis.getAngle()); rotationErrorInBody.set(proportionalTerm); proportionalGainMatrix.transform(proportionalTerm.getVector()); }
/** * @param index Index between 0 and {@link #getSphereCount()}-1 * @param axisAngle (Output) modified to contain the index-th sampled orientation */ public static void getAxisAngle(int index, AxisAngle4d axisAngle) { axisAngle.set(orientations.get(index)); }
/** * Calculate the rotation angle component of the input unit quaternion. * * @param q * unit quaternion Quat4d * @return the angle in radians of the input quaternion */ public static double angle(Quat4d q) { AxisAngle4d axis = new AxisAngle4d(); axis.set(q); return axis.angle; }
/** * Returns the AxisAngle of the helix transformation * @param transformation helix transformation * @return */ public AxisAngle4d getAxisAngle() { AxisAngle4d axis = new AxisAngle4d(); axis.set(this.transformation); return axis; }
public static void convertMatrixToAxisAngle(Matrix3d rotationMatrix, AxisAngle4d axisAngle4dToPack) { Quat4d quaternion = threadLocalTemporaryQuaternion.get(); convertMatrixToQuaternion(rotationMatrix, quaternion); axisAngle4dToPack.set(quaternion); }
/** * Returns the AxisAngle of the helix transformation * * @param transformation * helix transformation * @return */ private static AxisAngle4d getAxisAngle(Matrix4d transformation) { AxisAngle4d axis = new AxisAngle4d(); axis.set(transformation); return axis; } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempAxisAngle.set(axis, angle); transformToParent.setRotationAndZeroTranslation(tempAxisAngle); } }
public static void getAxisAngle(int index, AxisAngle4d axisAngle) { quat.set(orientations[index]); axisAngle.set(quat); }
private static void generateGaussianRotation(AxisAngle4d axisAngleToPack, Random random, double standardDeviation) { /* * random direction obtained from * from http://mathworld.wolfram.com/SpherePointPicking.html, equation (16) * * not completely sure about the right way to do Gaussian distribution on SO(3), but this should be OK for now. */ double x = random.nextGaussian(); double y = random.nextGaussian(); double z = random.nextGaussian(); double inverseSquareRoot = 1.0 / FastMath.sqrt(MathTools.square(x) + MathTools.square(y) + MathTools.square(z)); double angle = random.nextGaussian() * standardDeviation; axisAngleToPack.set(x * inverseSquareRoot, y * inverseSquareRoot, z * inverseSquareRoot, angle); } }
public static void convertQuaternionToRotationVector(Quat4d quaternion, Vector3d rotationVectorToPack) { AxisAngle4d axisAngle = axisAngleForRotationVectorConvertor.get(); axisAngle.set(quaternion); rotationVectorToPack.set(axisAngle.getX(), axisAngle.getY(), axisAngle.getZ()); rotationVectorToPack.scale(axisAngle.getAngle()); }
public void convertRotationVectorToAxisAngle(Vector3d rotationVector, double epsilonNoRotation, AxisAngle4d axisAngleToPack) { tempAxisOfRotation.set(rotationVector); double angularExcursion = tempAxisOfRotation.length(); if (angularExcursion > epsilonNoRotation) tempAxisOfRotation.scale(1.0 / angularExcursion); else { tempAxisOfRotation.set(1.0, 0.0, 0.0); angularExcursion = 0.0; } axisAngleToPack.set(tempAxisOfRotation, angularExcursion); } }
/** * This computes: resultToPack = log(q) * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result and is a pure quaternion (w = 0.0). */ public void log(Quat4d q, Quat4d resultToPack) { axisAngleForLog.set(q); resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ(), 0.0); resultToPack.scale(axisAngleForLog.getAngle()); }
/** * This computes: resultToPack = log(q) * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result. */ public void log(Quat4d q, Vector3d resultToPack) { axisAngleForLog.set(q); resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ()); resultToPack.scale(axisAngleForLog.getAngle()); }
private void corruptOrientation(Quat4d orientation) { Vector3d axis = RandomTools.generateRandomVector(random); double angle = RandomTools.generateRandomDouble(random, -maxRotationCorruption, maxRotationCorruption); AxisAngle4d axisAngle4d = new AxisAngle4d(); axisAngle4d.set(axis, angle); Quat4d corruption = new Quat4d(); corruption.set(axisAngle4d); orientation.mul(corruption); }
private DenseMatrix64F getSpatialErrorEndEffectorDesired() { transformEndEffectorToDesired.getTranslation(errorTranslationVector); transformEndEffectorToDesired.getRotation(errorQuat); errorAxisAngle.set(errorQuat); errorAngularVector.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorAngularVector.scale(errorAxisAngle.getAngle()); MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorAngularVector, 0, 0); MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorTranslationVector, 3, 0); return spatialError; }