public boolean hasHeelHitGround() { heelHitGround.set(isForceMagnitudePastThreshold()); heelHitGroundFilter.update(); return heelHitGroundFilter.getBooleanValue(); }
public void resetHeelSwitch() { heelHitGroundFilter.set(false); }
public GlitchFilteredBooleanYoVariable(String name, int windowSize) { super(name, "GlitchFilteredBooleanYoVariable", null); counter = new IntegerYoVariable(name + "Count", getYoVariableRegistry()); this.windowSize = new IntegerYoVariable(name + "WindowSize", getYoVariableRegistry()); initialize(null, windowSize); }
haveFeetHitGroundFiltered.get(foot).update(true); else haveFeetHitGroundFiltered.get(foot).set(false); if (haveFeetHitGroundFiltered.get(foot).getBooleanValue()) numberOfEndEffectorsTrusted++; areFeetTrusted.get(foot).set(haveFeetHitGroundFiltered.get(foot).getBooleanValue());
public void update(boolean value) { if (value != this.getBooleanValue()) { counter.set(counter.getIntegerValue() + 1); } else counter.set(0); if (counter.getIntegerValue() >= (windowSize.getIntegerValue() )) set(value); }
public void update() { if (variableToFilter == null) { throw new RuntimeException("variableToFilter was not initialized. Use the other constructor"); } else update(variableToFilter.getBooleanValue()); } }
final GlitchFilteredBooleanYoVariable hasFootHitTheGroundFiltered = new GlitchFilteredBooleanYoVariable("has" + footPrefix + "FootHitGroundFiltered", registry, windowSize); hasFootHitTheGroundFiltered.set(true); haveFeetHitGroundFiltered.put(foot, hasFootHitTheGroundFiltered);
public ActuatorForceBasedTouchdownDetector(String name, ForceSensorDataReadOnly forceSensorData, double touchdownForceThreshold, YoVariableRegistry registry) { this.foreSensorData = forceSensorData; this.touchdownForceThreshold = new DoubleYoVariable(name + "_touchdownForceThreshold", registry); this.touchdownForceThreshold.set(touchdownForceThreshold); touchdownDetected = new BooleanYoVariable(name + "_touchdownDetected", registry); touchdownDetectedFiltered = new GlitchFilteredBooleanYoVariable(touchdownDetected.getName() + "Filtered", registry, touchdownDetected, GLITCH_FLITER_WINDOW_SIZE); }
@Override public boolean hasTouchedDown() { return touchdownDetectedFiltered.getBooleanValue(); }
@Override public void variableChanged(YoVariable<?> v) { int windowSize = (int) (delayBeforeTrustingFoot.getDoubleValue() / estimatorDT); for (int i = 0; i < allFeet.size(); i++) areFeetTrusted.get(allFeet.get(i)).setWindowSize(windowSize); } });
public GlitchFilteredBooleanYoVariable(String name, YoVariableRegistry registry, int windowSize) { super(name, registry); counter = new IntegerYoVariable(name + "Count", registry); this.windowSize = new IntegerYoVariable(name + "WindowSize", registry); initialize(null, windowSize); }
isFootTrusted.update(true); else isFootTrusted.set(false); if (isFootTrusted.getBooleanValue())
@Override public void update() { foreSensorData.getWrench(wrenchToPack); wrenchToPack.getLinearPart(vectorToPack); touchdownDetected.set(vectorToPack.length() > touchdownForceThreshold.getDoubleValue()); touchdownDetectedFiltered.update(); } }
GlitchFilteredBooleanYoVariable isFootTrusted = new GlitchFilteredBooleanYoVariable("is" + footNamePascalCase + "TrustedIMUDrift", registry, windowSize); areFeetTrusted.put(foot, isFootTrusted);
public boolean isRobotBackToSafeState() { return isRobotBackToSafeState.getBooleanValue(); }
@Override public void variableChanged(YoVariable<?> v) { int windowSize = (int) (delayTimeBeforeTrustingFoot.getDoubleValue() / estimatorDT); hasFootHitTheGroundFiltered.setWindowSize(windowSize); } });
public GlitchFilteredBooleanYoVariable(String name, String description, YoVariableRegistry registry, BooleanYoVariable yoVariableToFilter, int windowSize) { super(name, description, registry); counter = new IntegerYoVariable(name + "Count", description, registry); this.windowSize = new IntegerYoVariable(name + "WindowSize", description, registry); initialize(yoVariableToFilter, windowSize); }
@Override public void update() { if(initialized) { velocityFiniteDifferenceFiltered.update(Math.abs(joint.getQd() - previousVelocity) * 1000); previousVelocity = joint.getQd(); if(!controllerSetFootSwitch.getBooleanValue() || !touchdownDetected.getBooleanValue()) { footInSwingFiltered.update(velocityFiniteDifferenceFiltered.getDoubleValue() < footInSwingThreshold.getDoubleValue()); if(footInSwingFiltered.getBooleanValue()) { touchdownDetected.set(velocityFiniteDifferenceFiltered.getDoubleValue() > touchdownThreshold.getDoubleValue()); } } else { footInSwingFiltered.set(false); } } else { previousVelocity = joint.getQd(); initialized = true; } } }