public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); this.alphaProvider = alpha; this.dt = dt; hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); currentPosition = null; lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); reset(); }
public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, FrameTuple2DReadOnly frameTuple2DToDifferentiate) { super(namePrefix, nameSuffix, frameTuple2DToDifferentiate.getReferenceFrame(), registry); this.alphaProvider = alpha; this.dt = dt; hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); currentPosition = frameTuple2DToDifferentiate; lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); reset(); }
@Override public void reset() { yoLineOfRotation.setToNaN(); yoFootAngularVelocityFiltered.reset(); yoFootAngularVelocityFiltered.setToNaN(); yoCoRPositionFiltered.reset(); yoCoRPositionFiltered.setToNaN(); yoCoRVelocityFiltered.reset(); yoCoRVelocityFiltered.setToNaN(); yoAngleOfLoR.set(0.0); yoLoRAngularVelocityFiltered.set(Double.NaN); yoLoRAngularVelocityFiltered.reset(); yoAngularVelocityAroundLoR.set(Double.NaN); yoIsLoRStable.set(false); yoIsCoRStable.set(false); yoIsFootRotating.set(false); hasBeenInitialized.set(false); }