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()); }