@Override public boolean hasTouchedDown() { return touchdownDetectedFiltered.getBooleanValue(); }
public boolean isRobotBackToSafeState() { return isRobotBackToSafeState.getBooleanValue(); }
public boolean hasHeelHitGround() { heelHitGround.set(isForceMagnitudePastThreshold()); heelHitGroundFilter.update(); return heelHitGroundFilter.getBooleanValue(); }
public boolean hasToeHitGround() { toeHitGround.set(isForceMagnitudePastThreshold()); toeHitGroundFilter.update(); return toeHitGroundFilter.getBooleanValue(); }
haveFeetHitGroundFiltered.get(foot).set(false); if (haveFeetHitGroundFiltered.get(foot).getBooleanValue()) numberOfEndEffectorsTrusted++; areFeetTrusted.get(foot).set(haveFeetHitGroundFiltered.get(foot).getBooleanValue());
private boolean isCoPPastThreshold() { if (Double.isNaN(footSwitchCoPThresholdFraction.getDoubleValue())) return true; updateCoP(); minThresholdX = (footMinX + footSwitchCoPThresholdFraction.getDoubleValue() * footLength); maxThresholdX = (footMaxX - footSwitchCoPThresholdFraction.getDoubleValue() * footLength); if (toeHitGroundFilter.getBooleanValue()) pastThreshold.set(resolvedCoP.getX() <= maxThresholdX); else if (heelHitGroundFilter.getBooleanValue()) pastThreshold.set(resolvedCoP.getX() >= minThresholdX); else pastThreshold.set(resolvedCoP.getX() >= minThresholdX && resolvedCoP.getX() <= maxThresholdX); pastThresholdFilter.update(); AppearanceDefinition appearanceDefinition = pastThresholdFilter.getBooleanValue() ? redAppearance : blueAppearance; footswitchCOPBagOfBalls.setBall(resolvedCoP3d, appearanceDefinition, 0); return pastThresholdFilter.getBooleanValue(); }
isFootTrusted.set(false); if (isFootTrusted.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); }
private boolean checkAnkleLimitForToeOff(RobotSide trailingLeg) { OneDoFJoint anklePitch = fullRobotModel.getLegJoint(trailingLeg, LegJointName.ANKLE_PITCH); double lowerLimit = Math.max(anklePitch.getJointLimitLower() + 0.02, ankleLowerLimitToTriggerToeOff.getDoubleValue()); isRearAnklePitchHittingLimit.set(anklePitch.getQ() < lowerLimit); isRearAnklePitchHittingLimitFilt.update(); if (!doToeOffWhenHittingAnkleLimit.getBooleanValue()) return false; if (!isDesiredICPOKForToeOff.getBooleanValue() || !isCurrentICPOKForToeOff.getBooleanValue()) return false; return isRearAnklePitchHittingLimitFilt.getBooleanValue(); }
public boolean hasFootHitGround() { isForceMagnitudePastThreshold.set(isForceMagnitudePastThreshold()); filteredIsForceMagnitudePastThreshold.update(); isForceMagnitudePastSecondThreshold.set(yoFootForceInFoot.getZ() > secondContactThresholdForce.getDoubleValue()); isCoPPastThreshold.set(isCoPPastThreshold()); hasFootHitGround.set( (filteredIsForceMagnitudePastThreshold.getBooleanValue() && isCoPPastThreshold.getBooleanValue()) || isForceMagnitudePastSecondThreshold.getBooleanValue()); // hasFootHitGround.set(isForceMagnitudePastThreshold.getBooleanValue()); filteredHasFootHitGround.update(); if (trustFootSwitch.getBooleanValue()) return filteredHasFootHitGround.getBooleanValue(); else return controllerDetectedTouchdown.getBooleanValue(); }
@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; } } }