@Override public void compute(FrameVector output, FramePoint desiredPosition, FrameVector desiredVelocity, FrameVector currentVelocity, FrameVector feedForward) { computeProportionalTerm(desiredPosition); if (currentVelocity != null) computeDerivativeTerm(desiredVelocity, currentVelocity); computeIntegralTerm(); output.setToNaN(bodyFrame); output.add(proportionalTerm, 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 feedbackLinearActionMagnitude = output.length(); if (feedbackLinearActionMagnitude > gains.getMaximumFeedback()) { output.scale(gains.getMaximumFeedback() / feedbackLinearActionMagnitude); } feedbackLinearAction.set(output); rateLimitedFeedbackLinearAction.update(); rateLimitedFeedbackLinearAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
@Override public void compute(FrameVector3D output, FramePoint3D desiredPosition, FrameVector3D desiredVelocity, FrameVector3D currentVelocity, FrameVector3D feedForward) { computeProportionalTerm(desiredPosition); if (currentVelocity != null) computeDerivativeTerm(desiredVelocity, currentVelocity); computeIntegralTerm(); output.setToNaN(bodyFrame); output.add(proportionalTerm, 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 feedbackLinearActionMagnitude = output.length(); if (feedbackLinearActionMagnitude > gains.getMaximumFeedback()) { output.scale(gains.getMaximumFeedback() / feedbackLinearActionMagnitude); } feedbackLinearAction.set(output); rateLimitedFeedbackLinearAction.update(); output.set(rateLimitedFeedbackLinearAction); feedForward.changeFrame(bodyFrame); output.add(feedForward); }