public void compute(
FrameVector output,
FrameOrientation desiredOrientation,
FrameVector desiredAngularVelocity,
FrameVector currentAngularVelocity,
FrameVector feedForward) {
computeProportionalTerm(desiredOrientation);
if (currentAngularVelocity != null)
computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity);
computeIntegralTerm();
output.setToZero(proportionalTerm.getReferenceFrame());
output.add(proportionalTerm);
output.add(derivativeTerm);
output.add(integralTerm);
// Limit the max acceleration of the feedback, but not of the feedforward...
// JEP changed 150430 based on Atlas hitting limit stops.
double feedbackAngularActionMagnitude = output.length();
double maximumAction = gains.getMaximumFeedback();
if (feedbackAngularActionMagnitude > maximumAction) {
output.scale(maximumAction / feedbackAngularActionMagnitude);
}
feedbackAngularAction.set(output);
rateLimitedFeedbackAngularAction.update();
rateLimitedFeedbackAngularAction.getFrameTuple(output);
feedForward.changeFrame(bodyFrame);
output.add(feedForward);
}
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());
}
public AxisAngleOrientationController(
String prefix,
ReferenceFrame bodyFrame,
double dt,
YoOrientationPIDGainsInterface gains,
YoVariableRegistry parentRegistry) {
this.dt = dt;
this.bodyFrame = bodyFrame;
registry = new YoVariableRegistry(prefix + getClass().getSimpleName());
if (gains == null) gains = new YoAxisAngleOrientationGains(prefix, registry);
this.gains = gains;
proportionalGainMatrix = gains.createProportionalGainMatrix();
derivativeGainMatrix = gains.createDerivativeGainMatrix();
integralGainMatrix = gains.createIntegralGainMatrix();
rotationErrorInBody = new YoFrameVector(prefix + "RotationErrorInBody", bodyFrame, registry);
rotationErrorCumulated =
new YoFrameVector(prefix + "RotationErrorCumulated", bodyFrame, registry);
velocityError = new YoFrameVector(prefix + "AngularVelocityError", bodyFrame, registry);
proportionalTerm = new FrameVector(bodyFrame);
derivativeTerm = new FrameVector(bodyFrame);
integralTerm = new FrameVector(bodyFrame);
feedbackAngularAction =
new YoFrameVector(prefix + "FeedbackAngularAction", bodyFrame, registry);
rateLimitedFeedbackAngularAction =
RateLimitedYoFrameVector.createRateLimitedYoFrameVector(
prefix + "RateLimitedFeedbackAngularAction",
"",
registry,
gains.getYoMaximumFeedbackRate(),
dt,
feedbackAngularAction);
parentRegistry.addChild(registry);
}
private void computeDerivativeTerm(
FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) {
desiredAngularVelocity.changeFrame(bodyFrame);
currentAngularVelocity.changeFrame(bodyFrame);
derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity);
// Limit the maximum velocity error considered for control action
double maximumVelocityError = gains.getMaximumDerivativeError();
double velocityErrorMagnitude = derivativeTerm.length();
if (velocityErrorMagnitude > maximumVelocityError) {
derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude);
}
velocityError.set(derivativeTerm);
derivativeGainMatrix.transform(derivativeTerm.getVector());
}
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 void setMaxProportionalError(double maxProportionalError) {
gains.setMaxProportionalError(maxProportionalError);
}
public void setMaxDerivativeError(double maxDerivativeError) {
gains.setMaxDerivativeError(maxDerivativeError);
}
public void setMaxFeedbackAndFeedbackRate(double maxFeedback, double maxFeedbackRate) {
gains.setMaxFeedbackAndFeedbackRate(maxFeedback, maxFeedbackRate);
}
public void setIntegralGains(double[] integralGains, double maxIntegralError) {
gains.setIntegralGains(integralGains, maxIntegralError);
}
public void setDerivativeGains(double[] derivativeGains) {
gains.setDerivativeGains(derivativeGains);
}
public void setProportionalGains(double[] proportionalGains) {
gains.setProportionalGains(proportionalGains);
}
public void setIntegralGains(
double integralGainX, double integralGainY, double integralGainZ, double maxIntegralError) {
gains.setIntegralGains(integralGainX, integralGainY, integralGainZ, maxIntegralError);
}
public void setDerivativeGains(
double derivativeGainX, double derivativeGainY, double derivativeGainZ) {
gains.setDerivativeGains(derivativeGainX, derivativeGainY, derivativeGainZ);
}
public void setProportionalGains(
double proportionalGainX, double proportionalGainY, double proportionalGainZ) {
gains.setProportionalGains(proportionalGainX, proportionalGainY, proportionalGainZ);
}
