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);
}
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);
}
public void reset() {
rateLimitedFeedbackAngularAction.reset();
}