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); }
final GlitchFilteredBooleanYoVariable hasFootHitTheGroundFiltered = new GlitchFilteredBooleanYoVariable("has" + footPrefix + "FootHitGroundFiltered", registry, windowSize); hasFootHitTheGroundFiltered.set(true);
GlitchFilteredBooleanYoVariable isFootTrusted = new GlitchFilteredBooleanYoVariable("is" + footNamePascalCase + "TrustedIMUDrift", registry, windowSize); areFeetTrusted.put(foot, isFootTrusted);
public JointVelocityFiniteDifferenceBasedTouchdownDetector(OneDoFJoint joint, BooleanYoVariable controllerSetFootSwitch, YoVariableRegistry registry) { this.joint = joint; this.controllerSetFootSwitch = controllerSetFootSwitch; finiteDifferenceAlphaFilter = new DoubleYoVariable(joint.getName() + "_velocityFiniteDifferenceAlpha", registry); finiteDifferenceAlphaFilter.set(0.99); velocityFiniteDifferenceFiltered = new AlphaFilteredYoVariable(joint.getName() + "_velocityFiniteDifferenceFiltered", registry, finiteDifferenceAlphaFilter); footInSwingThreshold = new DoubleYoVariable(joint.getName() + "_footInSwingThreshold", registry); touchdownThreshold = new DoubleYoVariable(joint.getName() + "__velocityFiniteDifferenceTouchdownThreshold", registry); footInSwingFiltered = new GlitchFilteredBooleanYoVariable(joint.getName() + "_footInSwingFiltered", registry, 50); touchdownDetected = new BooleanYoVariable(joint.getName() + "_velocityFiniteDifferenceTouchdownDetected", registry); }
filteredHasFootHitGround = new GlitchFilteredBooleanYoVariable(namePrefix + "FilteredFootHitGround", registry, hasFootHitGround, 1); filteredIsForceMagnitudePastThreshold = new GlitchFilteredBooleanYoVariable(namePrefix + "FilteredForcePastThresh", registry, isForceMagnitudePastThreshold, 2); isForceMagnitudePastSecondThreshold = new BooleanYoVariable(namePrefix + "ForcePastSecondThresh", registry); isCoPPastThreshold = new BooleanYoVariable(namePrefix + "CoPPastThresh", registry); this.pastThresholdFilter = new GlitchFilteredBooleanYoVariable(namePrefix + "PastFootswitchThresholdFilter", registry, pastThreshold, filterWindowSize); this.heelHitGroundFilter = new GlitchFilteredBooleanYoVariable(namePrefix + "HeelHitGroundFilter", registry, heelHitGround, filterWindowSize); this.toeHitGroundFilter = new GlitchFilteredBooleanYoVariable(namePrefix + "ToeHitGroundFilter", registry, toeHitGround, filterWindowSize);
public WalkOnTheEdgesManager(FullHumanoidRobotModel fullRobotModel, WalkingControllerParameters walkingControllerParameters, SideDependentList<? extends ContactablePlaneBody> feet, SideDependentList<YoPlaneContactState> footContactStates, YoVariableRegistry parentRegistry) { this.doToeOffIfPossible.set(walkingControllerParameters.doToeOffIfPossible()); this.doToeOffIfPossibleInSingleSupport.set(walkingControllerParameters.doToeOffIfPossibleInSingleSupport()); this.doToeOffWhenHittingAnkleLimit.set(walkingControllerParameters.doToeOffWhenHittingAnkleLimit()); this.ankleLowerLimitToTriggerToeOff.set(walkingControllerParameters.getAnkleLowerLimitToTriggerToeOff()); this.icpProximityToLeadingFootForDSToeOff.set(walkingControllerParameters.getICPProximityToLeadingFootForToeOff()); this.icpPercentOfStanceForSSToeOff.set(walkingControllerParameters.getICPPercentOfStanceForSSToeOff()); this.walkingControllerParameters = walkingControllerParameters; this.fullRobotModel = fullRobotModel; this.feet = feet; this.inPlaceWidth = walkingControllerParameters.getInPlaceWidth(); this.footLength = walkingControllerParameters.getFootBackwardOffset() + walkingControllerParameters.getFootForwardOffset(); extraCoMMaxHeightWithToes.set(0.08); minStepLengthForToeOff.set(walkingControllerParameters.getMinStepLengthForToeOff()); minStepHeightForToeOff.set(walkingControllerParameters.getMinStepHeightForToeOff()); isRearAnklePitchHittingLimit = new BooleanYoVariable("isRearAnklePitchHittingLimit", registry); isRearAnklePitchHittingLimitFilt = new GlitchFilteredBooleanYoVariable("isRearAnklePitchHittingLimitFilt", registry, isRearAnklePitchHittingLimit, 10); footDefaultPolygons = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { footDefaultPolygons.put(robotSide, new FrameConvexPolygon2d(feet.get(robotSide).getContactPoints2d())); } this.footContactStates = footContactStates; parentRegistry.addChild(registry); }
swingSideForDoubleSupportRecovery.set(null); isRobotBackToSafeState = new GlitchFilteredBooleanYoVariable("isRobotBackToSafeState", registry, 100); isCaptureRegionEmpty = new BooleanYoVariable("isCaptureRegionEmpty", registry);