/** * @deprecated Use * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, ReferenceFrame)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); }
public void update(FrameTuple2DReadOnly frameTuple) { checkReferenceFrameMatch(frameTuple); update((Tuple2DReadOnly) frameTuple); }
public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, YoFrameTuple2d<?, ?> yoFrameVectorToDifferentiate) { return createFilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, "", alpha, dt, registry, yoFrameVectorToDifferentiate); }
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(); }
YoVariableRegistry registry = new YoVariableRegistry("blop"); FilteredVelocityYoFrameVector2d filteredPoint = new FilteredVelocityYoFrameVector2d("tested", "", () -> alpha, dt, registry, ReferenceFrame.getWorldFrame()); FilteredVelocityYoVariable xFiltered = new FilteredVelocityYoVariable("xRef", "", alpha, dt, registry); FilteredVelocityYoVariable yFiltered = new FilteredVelocityYoVariable("yRef", "", alpha, dt, registry); filteredPoint.update(unfilteredPoint); xFiltered.update(unfilteredPoint.getX()); yFiltered.update(unfilteredPoint.getY());
public void update() { if (currentPosition == null) { throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); } update(currentPosition); }
@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); }
public void update(FrameTuple2d<?, ?> tupleToDifferentiate) { checkReferenceFrameMatch(tupleToDifferentiate); xDot.update(tupleToDifferentiate.getX()); yDot.update(tupleToDifferentiate.getY()); }
public void update(Tuple2DReadOnly currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); lastPosition.set(currentPosition); setToZero(); } currentRawDerivative.sub(currentPosition, lastPosition); currentRawDerivative.scale(1.0 / dt); interpolate(currentRawDerivative, this, alphaProvider.getValue()); lastPosition.set(currentPosition); } }
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(); }
public void update(YoFrameVector2d vector) { checkReferenceFrameMatch(vector.getReferenceFrame()); xDot.update(vector.getX()); yDot.update(vector.getY()); }
/** * @deprecated Use * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple2DReadOnly)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, FrameTuple2DReadOnly frameTuple2DToDifferentiate) { return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); }
yoCoRVelocityAlphaFilter = new DoubleYoVariable(namePrefix + "CoRVelocityAlphaFilter", generalDescription, registry); yoCoRTransversalVelocity = new DoubleYoVariable(namePrefix + "CoRTransversalVelocity", generalDescription, registry); yoCoRVelocityFiltered = FilteredVelocityYoFrameVector2d.createFilteredVelocityYoFrameVector2d(namePrefix + "CoRVelocity", "", generalDescription, yoCoRVelocityAlphaFilter, dt, registry, yoCoRPositionFiltered);
/** * @deprecated Use * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple2DReadOnly)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, DoubleProvider alpha, double dt, YoVariableRegistry registry, FrameTuple2DReadOnly frameTuple2DToDifferentiate) { return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); }
public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoFrameVector2d ret = new FilteredVelocityYoFrameVector2d(xDot, yDot, alpha, dt, registry, referenceFrame); return ret; }
public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, YoFrameTuple2d<?, ?> yoFrameVectorToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, alpha, yoFrameVectorToDifferentiate.getYoX(), dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, alpha, yoFrameVectorToDifferentiate.getYoY(), dt, registry); FilteredVelocityYoFrameVector2d ret = new FilteredVelocityYoFrameVector2d(xDot, yDot, alpha, dt, registry, yoFrameVectorToDifferentiate.getReferenceFrame()); return ret; }