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; }
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; }
/** * Compute the joint accelerations (for a specific subset of joints) by calculating finite-difference on joint velocities using {@link FilteredVelocityYoVariable}. It is then automatically low-pass filtered. * @param alphaFilter low-pass filter parameter. * @param forVizOnly if set to true, the result will not be used as the input of the next processing stage, nor as the output of the sensor processing. * @param jointsToIgnore list of the names of the joints to ignore. */ public void computeJointAccelerationFromFiniteDifferenceWithJointsToIgnore(DoubleYoVariable alphaFilter, boolean forVizOnly, String... jointsToIgnore) { List<String> jointToIgnoreList = new ArrayList<>(); if (jointsToIgnore != null && jointsToIgnore.length > 0) jointToIgnoreList.addAll(Arrays.asList(jointsToIgnore)); for (int i = 0; i < jointSensorDefinitions.size(); i++) { OneDoFJoint oneDoFJoint = jointSensorDefinitions.get(i); String jointName = oneDoFJoint.getName(); if (jointToIgnoreList.contains(jointName)) continue; DoubleYoVariable intermediateJointVelocity = outputJointVelocities.get(oneDoFJoint); List<ProcessingYoVariable> processors = processedJointAccelerations.get(oneDoFJoint); String prefix = JOINT_ACCELERATION.getProcessorNamePrefix(FINITE_DIFFERENCE); String suffix = JOINT_ACCELERATION.getProcessorNameSuffix(jointName, processors.size()); FilteredVelocityYoVariable jointAcceleration = new FilteredVelocityYoVariable(prefix + suffix, "", alphaFilter, intermediateJointVelocity, updateDT, registry); processors.add(jointAcceleration); if (!forVizOnly) outputJointAccelerations.put(oneDoFJoint, jointAcceleration); } }
/** * Compute the joint velocities (for a specific subset of joints) by calculating finite-difference on joint positions using {@link FilteredVelocityYoVariable}. It is then automatically low-pass filtered. * This is not cumulative and has the effect of ignoring the velocity signal provided by the robot. * @param alphaFilter low-pass filter parameter. * @param forVizOnly if set to true, the result will not be used as the input of the next processing stage, nor as the output of the sensor processing. * @param jointsToIgnore list of the names of the joints to ignore. */ public void computeJointVelocityFromFiniteDifferenceWithJointsToIgnore(DoubleYoVariable alphaFilter, boolean forVizOnly, String... jointsToIgnore) { List<String> jointToIgnoreList = new ArrayList<>(); if (jointsToIgnore != null && jointsToIgnore.length > 0) jointToIgnoreList.addAll(Arrays.asList(jointsToIgnore)); for (int i = 0; i < jointSensorDefinitions.size(); i++) { OneDoFJoint oneDoFJoint = jointSensorDefinitions.get(i); String jointName = oneDoFJoint.getName(); if (jointToIgnoreList.contains(jointName)) continue; DoubleYoVariable intermediateJointPosition = outputJointPositions.get(oneDoFJoint); List<ProcessingYoVariable> processors = processedJointVelocities.get(oneDoFJoint); String prefix = JOINT_VELOCITY.getProcessorNamePrefix(FINITE_DIFFERENCE); String suffix = JOINT_VELOCITY.getProcessorNameSuffix(jointName, processors.size()); FilteredVelocityYoVariable jointVelocity = new FilteredVelocityYoVariable(prefix + suffix, "", alphaFilter, intermediateJointPosition, updateDT, registry); processors.add(jointVelocity); if (!forVizOnly) outputJointVelocities.put(oneDoFJoint, jointVelocity); } }
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoY(), dt, registry); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoZ(), dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, yoFrameVectorToDifferentiate.getReferenceFrame()); return ret; }
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, registry); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, yoFramePointToDifferentiate.getReferenceFrame()); 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; }
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, double dt, DoubleProvider slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); backlashState = new YoEnum<BacklashState>(name + "BacklashState", registry, BacklashState.class, true); backlashState.set(null); timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); position = positionVariable; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new YoDouble(name + "_lastPosition", registry); reset(); }
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); backlashState = new YoEnum<BacklashState>(name + "BacklashState", registry, BacklashState.class, true); backlashState.set(null); timeSinceSloppy = new YoDouble(name + "timeInState", registry); this.position = null; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new YoDouble(name + "_lastPosition", registry); reset(); }
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleYoVariable alphaVariable, DoubleYoVariable positionVariable, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); this.hasBeenCalled = new BooleanYoVariable(name + "HasBeenCalled", registry); backlashState = new EnumYoVariable<BacklashState>(name + "BacklashState", registry, BacklashState.class, true); backlashState.set(null); timeSinceSloppy = new DoubleYoVariable(name + "TimeSinceSloppy", registry); position = positionVariable; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new DoubleYoVariable(name + "_lastPosition", registry); reset(); }
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleYoVariable alphaVariable, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); this.hasBeenCalled = new BooleanYoVariable(name + "HasBeenCalled", registry); backlashState = new EnumYoVariable<BacklashState>(name + "BacklashState", registry, BacklashState.class, true); backlashState.set(null); timeSinceSloppy = new DoubleYoVariable(name + "timeInState", registry); this.position = null; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new DoubleYoVariable(name + "_lastPosition", registry); reset(); }
public PositionVelocity1DConsistencyChecker(String namePrefix, DoubleYoVariable rawPosition, DoubleYoVariable rawVelocityToCheck, DoubleYoVariable processedPositionToCheck, DoubleYoVariable processedVelocityToCheck, double dt, YoVariableRegistry parentRegistry) { this.dt = dt; registry = new YoVariableRegistry(namePrefix + "PositionVelocity1DCheck"); referenceVelocity = new FilteredVelocityYoVariable(namePrefix + "_referenceVelocity", "", 0.0, rawPosition, dt, registry); int windowSize = 10; referenceVelocityFiltered = new SimpleMovingAverageFilteredYoVariable(namePrefix + "_referenceVelocityFiltered", windowSize, referenceVelocity, registry); filteredRawVelocityToCheck = new SimpleMovingAverageFilteredYoVariable(namePrefix + "_filteredRawVelocity", windowSize, rawVelocityToCheck, registry); filteredProcessedVelocityToCheck = new SimpleMovingAverageFilteredYoVariable(namePrefix + "_filteredProcessedVelocity", windowSize, processedVelocityToCheck, registry); delayEstimatorProcessedPosition = new DelayEstimatorBetweenTwoSignals(namePrefix + "ProcessedPosition", rawPosition, processedPositionToCheck, dt, registry); delayEstimatorRawVelocity = new DelayEstimatorBetweenTwoSignals(namePrefix + "RawVelocity", referenceVelocityFiltered, filteredRawVelocityToCheck, dt, registry); delayEstimatorProcessedVelocity = new DelayEstimatorBetweenTwoSignals(namePrefix + "ProcessedVelocity", referenceVelocityFiltered, filteredProcessedVelocityToCheck, dt, registry); parentRegistry.addChild(registry); }
FilteredVelocityYoVariable xFiltered = new FilteredVelocityYoVariable("xRef", "", alpha, dt, registry); FilteredVelocityYoVariable yFiltered = new FilteredVelocityYoVariable("yRef", "", alpha, dt, registry);
@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 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); }
YoDouble positionVariable = new YoDouble("positionVariable", registry); FilteredVelocityYoVariable filteredVelocityYoVariable = new FilteredVelocityYoVariable("filteredVelocityYoVariable", "test description", alpha, positionVariable, DT, registry);
@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 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 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); } }
@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); } }