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); }
public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz) { checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz); highPassFilteredInput.setCutoffFrequencyHz(minPassThroughFreqHz); this.setCutoffFrequencyHz(maxPassThroughFreqHz); }
public void update(double filterInput) { if (!hasBeenCalled) { hasBeenCalled = true; this.set(filterInput); } else { updateHighPassFilterAndThenLowPassFilterThat(filterInput); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testBandPassAttenuationForSinusoidalInput() { double inputFrequencyRadPerSec = 10.0; double a = inputFrequencyRadPerSec / 5.0; double b = inputFrequencyRadPerSec * 5.0; double filterAttenuation = 1.0; double properBandPassAttenuation; FirstOrderBandPassFilteredYoVariable bandPassFilteredYoVariable = new FirstOrderBandPassFilteredYoVariable("sineWave", "", a, b, yoTime, registry); while (filterAttenuation > 0.1 && a > 0.0 && b > 0.0) { bandPassFilteredYoVariable.setPassBand(a / (2.0*Math.PI), b / (2.0*Math.PI)); filterAttenuation = computeSteadyStateFilteredOutputAmplitude(yoTime, DT, inputFrequencyRadPerSec, bandPassFilteredYoVariable); properBandPassAttenuation = computeProperBandPassAttenuation(inputFrequencyRadPerSec, a, b); assertEquals(properBandPassAttenuation, filterAttenuation, 1e-2); a -= 10.0; b -= 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); } }
yoWristSensorForceMagnitudeBandPassFiltered = new FirstOrderBandPassFilteredYoVariable(forceSensorName + "ForceMagFiltered", "", forceSensorMinPassThroughFreq_Hz, forceSensorMaxPassThroughFreq_Hz, DT, registry);
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); } }
yoWristSensorForceMagnitudeBias = new FirstOrderFilteredYoVariable(forceSensorName + "ForceBias", "", 0.0001, DT, FirstOrderFilterType.LOW_PASS, registry); yoWristSensorForceMagnitudeBandPassFiltered = new FirstOrderBandPassFilteredYoVariable(forceSensorName + "ForceMagFiltered", "", forceSensorMinPassThroughFreq_Hz, forceSensorMaxPassThroughFreq_Hz, DT, registry);
public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz) { checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz); highPassFilteredInput.setCutoffFrequencyHz(minPassThroughFreqHz); this.setCutoffFrequencyHz(maxPassThroughFreqHz); }
public void update(double filterInput) { if (!hasBeenCalled) { hasBeenCalled = true; this.set(filterInput); } else { updateHighPassFilterAndThenLowPassFilterThat(filterInput); } }
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); }