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