public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, double DT, YoVariableRegistry registry) { super(name, description, maxPassThroughFrequency_Hz, DT, FirstOrderFilterType.LOW_PASS, registry); this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, DT, FirstOrderFilterType.HIGH_PASS, registry); }
private void updateHighPassFilterAndThenLowPassFilterThat(double filterInput) { this.highPassFilteredInput.update(filterInput); super.update(highPassFilteredInput.getDoubleValue()); } }
private double computeLowPassUpdate(double filterInput, double dt) { double alpha = computeAlpha( dt, cutoffFrequency_Hz.getDoubleValue()); double ret = alpha * this.getDoubleValue() + (1.0-alpha) * filterInput; return ret; }
filteredYoVariable.reset(); filteredYoVariable.update(sineWaveInput); filterOutput = filteredYoVariable.getDoubleValue();
filterUpdateTimeOld = 0.0; this.set(filterInput); reset(); filterOutput = computeLowPassUpdate(filterInput, dt); break; case HIGH_PASS: filterOutput = computeHighPassUpdate(filterInput, dt); break; this.set(filterOutput);
@ContinuousIntegrationTest(estimatedDuration = 1.1) @Test(timeout=300000) public void testHighPassAttenuationForSinusoidalInput() { double inputFrequencyRadPerSec = 15.0; double cutoffFrequencyRadPerSec = inputFrequencyRadPerSec / 5.0; double filterAttenuation = 1.0; double properHighPassAttenuation; FirstOrderFilteredYoVariable highPassFilteredYoVariable = new FirstOrderFilteredYoVariable("highPass", "", cutoffFrequencyRadPerSec / (2.0*Math.PI), yoTime, FirstOrderFilterType.HIGH_PASS, registry); while (filterAttenuation > 0.1 && cutoffFrequencyRadPerSec > 0.0 ) { highPassFilteredYoVariable.setCutoffFrequencyHz(cutoffFrequencyRadPerSec / (2.0*Math.PI)); filterAttenuation = computeSteadyStateFilteredOutputAmplitude(yoTime, DT, inputFrequencyRadPerSec, highPassFilteredYoVariable); properHighPassAttenuation = computeProperHighPassAttenuation(inputFrequencyRadPerSec, cutoffFrequencyRadPerSec); assertEquals(properHighPassAttenuation, filterAttenuation, 1e-2); cutoffFrequencyRadPerSec += 10.0; } }
private void stopArmIfHandCollisionIsDetected(double time) { estimateStiffnessOfConstraintsActingUponWrist(); yoWristSensorForceMagnitudeBias.update(yoWristSensorForceMagnitude.getDoubleValue()); yoWristSensorForceMagnitudeBandPassFiltered.update(yoWristSensorForceMagnitude.getDoubleValue()); yoForceLimitExceeded.set(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > yoImpactForceThreshold_N.getDoubleValue()); double forceToForceLimitRatio = yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() / yoImpactForceThreshold_N.getDoubleValue(); yoCollisionSeverityLevelOneToThree.set(MathTools.clipToMinMax((int) Math.round(forceToForceLimitRatio), 1, 3)); // yoForceLimitExceeded.set( taskspaceStiffnessCalc.getForceAlongDirectionOfMotion() > yoImpactForceThreshold_N.getDoubleValue() ); // yoStiffnessLimitExceeded.set(taskspaceStiffnessCalc.getStiffnessAlongDirectionOfMotion() > yoImpactStiffnessThreshold_NperM.getDoubleValue()); if (yoForceLimitExceeded.getBooleanValue()) { if (!yoImpactDetected.getBooleanValue() && time > 1.0 || yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > maxFilteredForce) { yoImpactDetected.set(true); yoImpactTime.set(time); controllerCommunicator.send(new HandCollisionDetectedPacket(robotSide, yoCollisionSeverityLevelOneToThree.getIntegerValue())); if (DEBUG) PrintTools.debug(this, "Sending Collision Detected Packet. FilteredForce = " + yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); } } if (Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()) > maxFilteredForce) { maxFilteredForce = Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); if (DEBUG) PrintTools.debug(this, "maxFilteredForce = " + maxFilteredForce); } }
public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz) { checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz); highPassFilteredInput.setCutoffFrequencyHz(minPassThroughFreqHz); this.setCutoffFrequencyHz(maxPassThroughFreqHz); }
filterUpdateTimeOld = 0.0; this.set(filterInput); reset(); filterOutput = computeLowPassUpdate(filterInput, dt); break; case HIGH_PASS: filterOutput = computeHighPassUpdate(filterInput, dt); break; this.set(filterOutput);
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testLowPassAttenuationForSinusoidalInput() { double inputFrequencyRadPerSec = 10.0; double cutoffFrequencyRadPerSec = inputFrequencyRadPerSec * 5.0; double filterAttenuation = 1.0; double properLowPassAttenuation; FirstOrderFilteredYoVariable lowPassFilteredYoVariable = new FirstOrderFilteredYoVariable("lowPass", "", cutoffFrequencyRadPerSec / (2.0*Math.PI), yoTime, FirstOrderFilterType.LOW_PASS, registry); while (filterAttenuation > 0.1 && cutoffFrequencyRadPerSec > 0.0) { lowPassFilteredYoVariable.setCutoffFrequencyHz(cutoffFrequencyRadPerSec / (2.0*Math.PI)); filterAttenuation = computeSteadyStateFilteredOutputAmplitude(yoTime, DT, inputFrequencyRadPerSec, lowPassFilteredYoVariable); properLowPassAttenuation = computeProperLowPassAttenuation(inputFrequencyRadPerSec, cutoffFrequencyRadPerSec); assertEquals(properLowPassAttenuation, filterAttenuation, 1e-2); cutoffFrequencyRadPerSec -= 10.0; } }
private void stopArmIfHandCollisionIsDetected(double time) { estimateStiffnessOfConstraintsActingUponWrist(); yoWristSensorForceMagnitudeBias.update(yoWristSensorForceMagnitude.getDoubleValue()); yoWristSensorForceMagnitudeBandPassFiltered.update(yoWristSensorForceMagnitude.getDoubleValue()); yoForceLimitExceeded.set(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > yoImpactForceThreshold_N.getDoubleValue()); double forceToForceLimitRatio = yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() / yoImpactForceThreshold_N.getDoubleValue(); yoCollisionSeverityLevelOneToThree.set(MathTools.clamp((int) Math.round(forceToForceLimitRatio), 1, 3)); // yoForceLimitExceeded.set( taskspaceStiffnessCalc.getForceAlongDirectionOfMotion() > yoImpactForceThreshold_N.getDoubleValue() ); // yoStiffnessLimitExceeded.set(taskspaceStiffnessCalc.getStiffnessAlongDirectionOfMotion() > yoImpactStiffnessThreshold_NperM.getDoubleValue()); if (yoForceLimitExceeded.getBooleanValue()) { if (!yoImpactDetected.getBooleanValue() && time > 1.0 || yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > maxFilteredForce) { yoImpactDetected.set(true); yoImpactTime.set(time); publisher.publish(HumanoidMessageTools.createHandCollisionDetectedPacket(robotSide, yoCollisionSeverityLevelOneToThree.getIntegerValue())); if (DEBUG) PrintTools.debug(this, "Sending Collision Detected Packet. FilteredForce = " + yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); } } if (Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()) > maxFilteredForce) { maxFilteredForce = Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); if (DEBUG) PrintTools.debug(this, "maxFilteredForce = " + maxFilteredForce); } }
public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz) { checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz); highPassFilteredInput.setCutoffFrequencyHz(minPassThroughFreqHz); this.setCutoffFrequencyHz(maxPassThroughFreqHz); }
public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, double DT, YoVariableRegistry registry) { super(name, description, maxPassThroughFrequency_Hz, DT, FirstOrderFilterType.LOW_PASS, registry); this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, DT, FirstOrderFilterType.HIGH_PASS, registry); }
private void updateHighPassFilterAndThenLowPassFilterThat(double filterInput) { this.highPassFilteredInput.update(filterInput); super.update(highPassFilteredInput.getDoubleValue()); } }
private double computeLowPassUpdate(double filterInput, double dt) { double alpha = computeAlpha( dt, cutoffFrequency_Hz.getDoubleValue()); double ret = alpha * this.getDoubleValue() + (1.0-alpha) * filterInput; return ret; }
public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, YoDouble yoTime, YoVariableRegistry registry) { super(name, description, maxPassThroughFrequency_Hz, yoTime, FirstOrderFilterType.LOW_PASS, registry); this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, yoTime, FirstOrderFilterType.HIGH_PASS, registry); setPassBand(minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz); }
private double computeHighPassUpdate(double filterInput, double dt) { double alpha = computeAlpha(dt, cutoffFrequency_Hz.getDoubleValue()); double ret = alpha * (this.getDoubleValue() + filterInput - filterInputOld); return ret; }
public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, DoubleYoVariable yoTime, YoVariableRegistry registry) { super(name, description, maxPassThroughFrequency_Hz, yoTime, FirstOrderFilterType.LOW_PASS, registry); this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, yoTime, FirstOrderFilterType.HIGH_PASS, registry); setPassBand(minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz); }
private double computeHighPassUpdate(double filterInput, double dt) { double alpha = computeAlpha(dt, cutoffFrequency_Hz.getDoubleValue()); double ret = alpha * (this.getDoubleValue() + filterInput - filterInputOld); return ret; }
yoWristSensorForceMagnitudeBias = new FirstOrderFilteredYoVariable(forceSensorName + "ForceBias", "", 0.0001, DT, FirstOrderFilterType.LOW_PASS, registry); yoWristSensorForceMagnitudeBandPassFiltered = new FirstOrderBandPassFilteredYoVariable(forceSensorName + "ForceMagFiltered", "", forceSensorMinPassThroughFreq_Hz, forceSensorMaxPassThroughFreq_Hz, DT, registry);