@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testUpdateForTranslationalVelocity() { YoVariableRegistry registry = new YoVariableRegistry("testRegistry"); double alpha = 0.3; YoDouble positionVariable = new YoDouble("positionVariable", registry); FilteredVelocityYoVariable filteredVelocityYoVariable = new FilteredVelocityYoVariable("filteredVelocityYoVariable", "test description", alpha, positionVariable, DT, registry); filteredVelocityYoVariable.set(0); positionVariable.set(0); for (int i = 0; i < 10000; i++) { positionVariable.add(10); filteredVelocityYoVariable.update(); } assertEquals(100, filteredVelocityYoVariable.getDoubleValue(), 1e-7); }
public void updateForAngles() { if (position == null) { throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } updateForAngles(position.getDoubleValue()); }
private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getValue(); set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }
public void update(double currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); lastPosition.set(currentPosition); set(0.0); } double difference = currentPosition - lastPosition.getDoubleValue(); updateUsingDifference(difference); lastPosition.set(currentPosition); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testWithoutBacklash2() { Random rand = new Random(1798L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoDouble alphaVariable = new YoDouble("alpha", registry); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); YoDouble slopTime = new YoDouble("slop", registry); YoDouble rawPosition = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable filtVelocity = new FilteredVelocityYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); BacklashCompensatingVelocityYoVariable filteredOnly = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, registry); filtVelocity.update(); filteredOnly.update(); for (int i = 0; i < 1000; i++) { alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); rawPosition.set(RandomNumbers.nextDouble(rand, -100.0, 100.0)); filtVelocity.update(); filteredOnly.update(); assertEquals(filtVelocity.getDoubleValue(), filteredOnly.getDoubleValue(), EPSILON); } }
rawRate.update(raw.getDoubleValue()); rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue());
YoDouble positionVariable = new YoDouble("positionVariable", registry); FilteredVelocityYoVariable filteredVelocityYoVariable = new FilteredVelocityYoVariable("filteredVelocityYoVariable", "test description", alpha, positionVariable, DT, registry); filteredVelocityYoVariable.set(0); positionVariable.set(-Math.PI); filteredVelocityYoVariable.updateForAngles(); assertEquals(5, filteredVelocityYoVariable.getDoubleValue(), 1e-5);
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(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); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, referenceFrame); return ret; }
FilteredVelocityYoVariable xFiltered = new FilteredVelocityYoVariable("xRef", "", alpha, dt, registry); FilteredVelocityYoVariable yFiltered = new FilteredVelocityYoVariable("yRef", "", alpha, dt, registry); FilteredVelocityYoVariable zFiltered = new FilteredVelocityYoVariable("zRef", "", alpha, dt, registry); xFiltered.update(unfilteredPoint.getX()); yFiltered.update(unfilteredPoint.getY()); zFiltered.update(unfilteredPoint.getZ()); EuclidCoreTestTools.assertTuple3DEquals(new Point3D(xFiltered.getValue(), yFiltered.getValue(), zFiltered.getValue()), filteredPoint, EPSILON);
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstRevisedBacklash() { YoVariableRegistry registry = new YoVariableRegistry("dummy"); YoDouble slopTime = new YoDouble("slopTime", registry); double dt = 0.002; YoDouble alpha = new YoDouble("alpha", registry); alpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(16.0, dt)); YoDouble positionVariable = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable velocityVariable = new FilteredVelocityYoVariable("fd", "", alpha, positionVariable, dt, registry); BacklashProcessingYoVariable blToTest = new BacklashProcessingYoVariable("blTest", "", velocityVariable, dt, slopTime, registry); RevisedBacklashCompensatingVelocityYoVariable blExpected = new RevisedBacklashCompensatingVelocityYoVariable("blExpected", "", alpha, positionVariable, dt, slopTime, registry); Random random = new Random(561651L); for (double t = 0.0; t < 100.0; t += dt) { positionVariable.set(2.0 * Math.sin(2.0 * Math.PI * 10.0) + RandomNumbers.nextDouble(random, 1.0) * Math.sin(2.0 * Math.PI * 30.0 + 2.0 / 3.0 * Math.PI)); velocityVariable.update(); blToTest.update(); blExpected.update(); assertEquals(blToTest.getDoubleValue(), blExpected.getDoubleValue(), 1.0e-10); } }
yoLoRAngularVelocityFiltered.updateForAngles(); yoIsFootDropPastThreshold.set(yoFootDropOrLift.getDoubleValue() < yoFootDropThreshold.getDoubleValue()); yoIsLoRStable.set(Math.abs(yoLoRAngularVelocityFiltered.getDoubleValue()) < yoStableLoRAngularVelocityThreshold.getDoubleValue());
@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); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testWithoutBacklash1() { Random rand = new Random(1798L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoDouble alphaVariable = new YoDouble("alpha", registry); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); YoDouble slopTime = new YoDouble("slop", registry); YoDouble rawPosition = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable filtVelocity = new FilteredVelocityYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); BacklashCompensatingVelocityYoVariable filteredOnly = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, dt, slopTime, registry); filtVelocity.update(); filteredOnly.update(rawPosition.getDoubleValue()); for (int i = 0; i < 1000; i++) { alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); rawPosition.set(RandomNumbers.nextDouble(rand, -100.0, 100.0)); filtVelocity.update(); filteredOnly.update(rawPosition.getDoubleValue()); assertEquals(filtVelocity.getDoubleValue(), filteredOnly.getDoubleValue(), EPSILON); } }
rawRate.update(raw.getDoubleValue()); rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue());
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; }
FilteredVelocityYoVariable xFiltered = new FilteredVelocityYoVariable("xRef", "", alpha, dt, registry); FilteredVelocityYoVariable yFiltered = new FilteredVelocityYoVariable("yRef", "", alpha, dt, registry); xFiltered.update(unfilteredPoint.getX()); yFiltered.update(unfilteredPoint.getY()); EuclidCoreTestTools.assertTuple2DEquals(new Point2D(xFiltered.getValue(), yFiltered.getValue()), filteredPoint, EPSILON);
private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getDoubleValue(); set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }