@Override public void update() { if (position == null) { throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } update(position.getDoubleValue()); }
public void update() { if (position == null) { throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } update(position.getDoubleValue()); }
@Override public void update() { referenceVelocity.update(); referenceVelocityFiltered.update(); filteredRawVelocityToCheck.update(); filteredProcessedVelocityToCheck.update(); if (!referenceVelocityFiltered.getHasBufferWindowFilled()) return; delayEstimatorProcessedPosition.update(); delayEstimatorRawVelocity.update(); delayEstimatorProcessedVelocity.update(); }
@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); } }
@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); } }
@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); }
@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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testVelocityPositiveWithoutCrossingZero2() { 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 backlashAndFiltered = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, registry); filtVelocity.update(); backlashAndFiltered.update(); for (int i = 0; i < 1000; i++) { slopTime.set(RandomNumbers.nextDouble(rand, 0.0, 100.0)); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); rawPosition.add(RandomNumbers.nextDouble(rand, 0.0, 101.0)); filtVelocity.update(); backlashAndFiltered.update(); assertEquals(filtVelocity.getDoubleValue(), backlashAndFiltered.getDoubleValue(), EPSILON); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testVelocityNegativeWithoutCrossingZero2() { 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 backlashAndFiltered = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, registry); filtVelocity.update(); backlashAndFiltered.update(); for (int i = 0; i < 1000; i++) { slopTime.set(RandomNumbers.nextDouble(rand, 0.0, 100.0)); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); rawPosition.sub(RandomNumbers.nextDouble(rand, 0.0, 101.0)); filtVelocity.update(); backlashAndFiltered.update(); assertEquals(filtVelocity.getDoubleValue(), backlashAndFiltered.getDoubleValue(), EPSILON); } }