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