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