public void update() { if (rawPosition == null) { throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } update(rawPosition); }
public void update(Tuple3DReadOnly vectorUnfiltered) { update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); }
public void update(FrameTuple3DReadOnly frameVectorUnfiltered) { checkReferenceFrameMatch(frameVectorUnfiltered); update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); }
@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); }
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 void compute(FrameVector3D output, FrameQuaternion desiredOrientation, FrameVector3D desiredAngularVelocity, FrameVector3D currentAngularVelocity, FrameVector3D 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(); output.set(rateLimitedFeedbackAngularAction); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
public void compute(FrameVector acceleration, FramePoint desiredPosition, FrameVector desiredVelocity, FrameVector feedForward, RigidBody base) { ReferenceFrame baseFrame = base.getBodyFixedFrame(); computeProportionalTerm(desiredPosition, baseFrame); computeDerivativeTerm(desiredVelocity, base); computeIntegralTerm(baseFrame); acceleration.setIncludingFrame(proportionalTerm); acceleration.add(derivativeTerm); acceleration.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackLinearAccelerationMagnitude = acceleration.length(); if (feedbackLinearAccelerationMagnitude > gains.getMaximumFeedback()) { acceleration.scale(gains.getMaximumFeedback() / feedbackLinearAccelerationMagnitude); } feedbackLinearAcceleration.setAndMatchFrame(acceleration); rateLimitedFeedbackLinearAcceleration.update(); rateLimitedFeedbackLinearAcceleration.getFrameTupleIncludingFrame(acceleration); acceleration.changeFrame(baseFrame); feedForward.changeFrame(baseFrame); acceleration.add(feedForward); }