Skip to content

add a filter for big downward forces on the second threshold #1108

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Nov 18, 2025
Merged

add a filter for big downward forces on the second threshold #1108

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cc5c47b
add a filter for big downward forces on the second threshold
rjgriffin42 Nov 18, 2025
129e46c
Merge branch 'demo/alex-nov-2025' into feature/second_velocity_threshold
PotatoPeeler3000 Nov 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions ...va/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@ public JointTorqueBasedFootSwitch(String namePrefix,
BooleanProvider compensateGravity,
DoubleProvider horizontalVelocityThreshold,
DoubleProvider verticalVelocityThreshold,
DoubleProvider verticalVelocityHighThreshold,
BooleanProvider useJacobianTranspose,
YoRegistry parentRegistry)
{
Expand Down Expand Up @@ -113,6 +114,7 @@ public JointTorqueBasedFootSwitch(String namePrefix,
compensateGravity,
horizontalVelocityThreshold,
verticalVelocityThreshold,
verticalVelocityHighThreshold,
registry);

parentRegistry.addChild(registry);
Expand Down Expand Up @@ -312,6 +314,7 @@ private static class JacobianBasedBasedTouchdownDetector

private final DoubleProvider horizontalVelocityThreshold;
private final DoubleProvider verticalVelocityThreshold;
private final DoubleProvider verticalVelocityHighThreshold;
private final YoBoolean isPastForceThresholdLow;
private final GlitchFilteredYoBoolean isPastForceThresholdLowFiltered;
private final YoBoolean isPastForceThresholdHigh;
Expand Down Expand Up @@ -340,6 +343,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot,
BooleanProvider compensateGravity,
DoubleProvider horizontalVelocityThreshold,
DoubleProvider verticalVelocityThreshold,
DoubleProvider verticalVelocityHighThreshold,
YoRegistry registry)
{
this.soleFrame = soleFrame;
Expand All @@ -350,6 +354,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot,
this.compensateGravity = compensateGravity;
this.horizontalVelocityThreshold = horizontalVelocityThreshold;
this.verticalVelocityThreshold = verticalVelocityThreshold;
this.verticalVelocityHighThreshold = verticalVelocityHighThreshold;

legJoints = MultiBodySystemTools.createOneDoFJointPath(pelvis, foot);

Expand Down Expand Up @@ -488,9 +493,10 @@ private void updateFootSwitch(WrenchReadOnly wrench)

boolean validCoP = isPastCoPThresholdFiltered.getValue();
boolean hitGroundLow = isPastForceThresholdLowFiltered.getValue() && validCoP;
boolean allowableSpeed = horizontalVelocity.getValue() < horizontalVelocityThreshold.getValue() && Math.abs(verticalVelocity.getValue()) < verticalVelocityThreshold.getValue() ;
boolean allowableSpeed = horizontalVelocity.getValue() < horizontalVelocityThreshold.getValue() && Math.abs(verticalVelocity.getValue()) < verticalVelocityThreshold.getValue();
boolean allowableHighSpeed = Math.abs(verticalVelocity.getValue()) < verticalVelocityHighThreshold.getValue() ;

hasFootHitGround.set((hitGroundLow && allowableSpeed) || isPastForceThresholdHigh.getValue());
hasFootHitGround.set((hitGroundLow && allowableSpeed) || (isPastForceThresholdHigh.getValue() && allowableHighSpeed));
Copy link
Member

@Beomyeong Beomyeong Nov 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't the 0.5 of defaultVerticalVelcoityhighThreshold high?
It sounds, even the foot moves faster than the defaultVerticalVelocityThreshold, it could be triggered as hit the ground.

hasFootHitGroundFiltered.update();
}

Expand Down
4 changes: 4 additions & 0 deletions ...hmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ public class JointTorqueBasedFootSwitchFactory implements FootSwitchFactory
private boolean defaultUseJacobianTranspose = false;
private double defaultHorizontalVelocityThreshold = 0.5;
private double defaultVerticalVelocityThreshold = 0.125;
private double defaultVerticalVelocityHighThreshold = 0.5;

private DoubleProvider contactThresholdTorque;
private DoubleProvider higherContactThresholdTorque;
Expand All @@ -36,6 +37,7 @@ public class JointTorqueBasedFootSwitchFactory implements FootSwitchFactory
private BooleanProvider compensateGravity;
private DoubleProvider horizontalVelocityThreshold;
private DoubleProvider verticalVelocityThreshold;
private DoubleProvider verticalVelocityHighThreshold;
private YoInteger contactWindowSize;
private BooleanProvider useJacobianTranspose;

Expand Down Expand Up @@ -132,6 +134,7 @@ public FootSwitchInterface newFootSwitch(String namePrefix,
compensateGravity = new BooleanParameter(namePrefix + "JacobianTCompensateGravity", registry, true);
useJacobianTranspose = new BooleanParameter(namePrefix + "UseJacobianTranspose", registry, defaultUseJacobianTranspose);
verticalVelocityThreshold = new DoubleParameter(namePrefix + "VerticalVelocityThreshold", registry, defaultVerticalVelocityThreshold);
verticalVelocityHighThreshold = new DoubleParameter(namePrefix + "VerticalVelocityHighThreshold", registry, defaultVerticalVelocityHighThreshold);
horizontalVelocityThreshold = new DoubleParameter(namePrefix + "HorizontalVelocityThreshold", registry, defaultHorizontalVelocityThreshold);
}

Expand All @@ -148,6 +151,7 @@ public FootSwitchInterface newFootSwitch(String namePrefix,
compensateGravity,
horizontalVelocityThreshold,
verticalVelocityThreshold,
verticalVelocityHighThreshold,
useJacobianTranspose,
registry);
}
Expand Down
Loading