public EuclideanPositionController(String prefix, ReferenceFrame bodyFrame, double dt, YoPositionPIDGainsInterface gains, YoVariableRegistry parentRegistry) { this.dt = dt; this.bodyFrame = bodyFrame; registry = new YoVariableRegistry(prefix + getClass().getSimpleName()); if (gains == null) gains = new YoEuclideanPositionGains(prefix, registry); this.gains = gains; proportionalGainMatrix = gains.createProportionalGainMatrix(); derivativeGainMatrix = gains.createDerivativeGainMatrix(); integralGainMatrix = gains.createIntegralGainMatrix(); positionError = new YoFrameVector(prefix + "PositionError", bodyFrame, registry); positionErrorCumulated = new YoFrameVector(prefix + "PositionErrorCumulated", bodyFrame, registry); velocityError = new YoFrameVector(prefix + "LinearVelocityError", bodyFrame, registry); proportionalTerm = new FrameVector(bodyFrame); derivativeTerm = new FrameVector(bodyFrame); integralTerm = new FrameVector(bodyFrame); feedbackLinearAction = new YoFrameVector(prefix + "FeedbackLinearAction", bodyFrame, registry); rateLimitedFeedbackLinearAction = RateLimitedYoFrameVector.createRateLimitedYoFrameVector(prefix + "RateLimitedFeedbackLinearAction", "", registry, gains.getYoMaximumFeedbackRate(), dt, feedbackLinearAction); tangentialDampingCalculator = new EuclideanTangentialDampingCalculator(prefix, bodyFrame, gains.getTangentialDampingGains()); parentRegistry.addChild(registry); }