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()); }
private void computeIntegralTerm() { if (gains.getMaximumIntegralError() < 1e-5) { integralTerm.setToZero(bodyFrame); return; } double integratedErrorAngle = errorAngleAxis.getAngle() * dt; double errorIntegratedX = errorAngleAxis.getX() * integratedErrorAngle; double errorIntegratedY = errorAngleAxis.getY() * integratedErrorAngle; double errorIntegratedZ = errorAngleAxis.getZ() * integratedErrorAngle; rotationErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ); double errorMagnitude = rotationErrorCumulated.length(); if (errorMagnitude > gains.getMaximumIntegralError()) { rotationErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude); } rotationErrorCumulated.getFrameTuple(integralTerm); integralGainMatrix.transform(integralTerm.getVector()); }