public static void trimAxisAngleMinusPiToPi(AxisAngle4d axisAngle4d, AxisAngle4d axisAngleTrimmedToPack) { axisAngleTrimmedToPack.set(axisAngle4d); axisAngleTrimmedToPack.setAngle(AngleTools.trimAngleMinusPiToPi(axisAngleTrimmedToPack.getAngle())); }
/** * 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); }
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()); }
public static boolean axisAngleEpsilonEqualsIgnoreFlippedAxes(AxisAngle4d original, AxisAngle4d result, double epsilon) { if (original.epsilonEquals(result, epsilon)) { return true; } else { AxisAngle4d originalNegated = originalAxisAngleForEpsilonEquals.get(); originalNegated.set(original); originalNegated.setAngle(originalNegated.getAngle() * -1.0); originalNegated.setX(originalNegated.getX() * -1.0); originalNegated.setY(originalNegated.getY() * -1.0); originalNegated.setZ(originalNegated.getZ() * -1.0); return originalNegated.epsilonEquals(result, epsilon); } } }
rotation2.setAngle(-angle);