Skip to content

set up some singularity filtering for the joint based contact switch #1130

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
rjgriffin42 merged 4 commits into from
Dec 17, 2025
+36 −7
Merged

set up some singularity filtering for the joint based contact switch #1130

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
bd987b5
set up some singularity filtering for the joint based contact switch
rjgriffin42 Dec 5, 2025
3500276
Merge branch 'develop' into feature/singularity_filter
rjgriffin42 Dec 15, 2025
9988633
made a variable name even longer
rjgriffin42 Dec 15, 2025
ab9e386
lowered the high velocity filter on the contact switch
Dec 16, 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
30 changes: 25 additions & 5 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 @@ -62,6 +62,7 @@ public JointTorqueBasedFootSwitch(String namePrefix,
DoubleProvider horizontalVelocityThreshold,
DoubleProvider verticalVelocityThreshold,
DoubleProvider verticalVelocityHighThreshold,
DoubleProvider jacobianDeterminantSingularityThreshold,
BooleanProvider useJacobianTranspose,
YoRegistry parentRegistry)
{
Expand Down Expand Up @@ -115,6 +116,7 @@ public JointTorqueBasedFootSwitch(String namePrefix,
horizontalVelocityThreshold,
verticalVelocityThreshold,
verticalVelocityHighThreshold,
jacobianDeterminantSingularityThreshold,
registry);

parentRegistry.addChild(registry);
Expand Down Expand Up @@ -315,13 +317,15 @@ private static class JacobianBasedBasedTouchdownDetector
private final DoubleProvider horizontalVelocityThreshold;
private final DoubleProvider verticalVelocityThreshold;
private final DoubleProvider verticalVelocityHighThreshold;
private final DoubleProvider jacobianDeterminantThreshold;
private final YoBoolean isPastForceThresholdLow;
private final GlitchFilteredYoBoolean isPastForceThresholdLowFiltered;
private final YoBoolean isPastForceThresholdHigh;
private final YoBoolean hasFootHitGround, isPastCoPThreshold;
private final GlitchFilteredYoBoolean hasFootHitGroundFiltered;
private final GlitchFilteredYoBoolean isPastCoPThresholdFiltered;

private final YoDouble jacobianDeterminant;
private final YoDouble copDistance;
private final YoDouble footForceMagnitude;
private final YoDouble alphaFootLoadFiltering;
Expand All @@ -344,6 +348,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot,
DoubleProvider horizontalVelocityThreshold,
DoubleProvider verticalVelocityThreshold,
DoubleProvider verticalVelocityHighThreshold,
DoubleProvider jacobianDeterminantThreshold,
YoRegistry registry)
{
this.soleFrame = soleFrame;
Expand All @@ -355,6 +360,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot,
this.horizontalVelocityThreshold = horizontalVelocityThreshold;
this.verticalVelocityThreshold = verticalVelocityThreshold;
this.verticalVelocityHighThreshold = verticalVelocityHighThreshold;
this.jacobianDeterminantThreshold = jacobianDeterminantThreshold;
Copy link
Contributor

@stefanfasano stefanfasano Dec 11, 2025

Choose a reason for hiding this comment

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

since there are lots of thresholds in this class, maybe change this to be called "jacobianDeterrminantSingularityThreshold" to indicate this is for singularity checking, not touchdown detection checking


legJoints = MultiBodySystemTools.createOneDoFJointPath(pelvis, foot);

Expand Down Expand Up @@ -392,6 +398,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot,

footForceMagnitude = new YoDouble(namePrefix + "FootForceMag", registry);
copDistance = new YoDouble(namePrefix + "CoPDistance", registry);
jacobianDeterminant = new YoDouble(namePrefix + "JacobianDeterminant", registry);

alphaFootLoadFiltering = new YoDouble(namePrefix + "AlphaFootLoadFiltering", registry);
alphaFootLoadFiltering.set(0.1);
Expand All @@ -417,6 +424,9 @@ public void calculate()
jacobianTranspose.reshape(footJacobian.getNumberOfColumns(), 6);
CommonOps_DDRM.transpose(footJacobian.getJacobianMatrix(), jacobianTranspose);

// Compute the determinant of the jacobian to help evaluate singular configurations.
jacobianDeterminant.set(CommonOps_DDRM.det(jacobianTranspose));

for (int i = 0; i < legJoints.length; i++)
torqueVector.set(i, 0, legJoints[i].getTau());

Expand Down Expand Up @@ -491,12 +501,22 @@ private void updateFootSwitch(WrenchReadOnly wrench)
horizontalVelocity.set(EuclidCoreTools.norm(linearVelocity.getX(), linearVelocity.getY()));
verticalVelocity.set(linearVelocity.getZ());

boolean validCoP = isPastCoPThresholdFiltered.getValue();
boolean hitGroundLow = isPastForceThresholdLowFiltered.getValue() && validCoP;
boolean allowableSpeed = horizontalVelocity.getValue() < horizontalVelocityThreshold.getValue() && Math.abs(verticalVelocity.getValue()) < verticalVelocityThreshold.getValue();
boolean allowableHighSpeed = Math.abs(verticalVelocity.getValue()) < verticalVelocityHighThreshold.getValue() ;
if (jacobianDeterminant.getDoubleValue() > jacobianDeterminantThreshold.getValue())
{ // The jacobian determinant is above the threshold, so it's not in a singular configuration
boolean validCoP = isPastCoPThresholdFiltered.getValue();
boolean hitGroundLow = isPastForceThresholdLowFiltered.getValue() && validCoP;
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() && allowableHighSpeed));
hasFootHitGround.set((hitGroundLow && allowableSpeed) || (isPastForceThresholdHigh.getValue() && allowableHighSpeed));
}
else
{ // The jacobian determinant is below the threshold, so the system is in a singular configuration. This means the forces can't be trusted.
boolean allowableSpeed = horizontalVelocity.getValue() < horizontalVelocityThreshold.getValue()
&& Math.abs(verticalVelocity.getValue()) < verticalVelocityThreshold.getValue();
hasFootHitGround.set(allowableSpeed);
}
hasFootHitGroundFiltered.update();
}

Expand Down
13 changes: 11 additions & 2 deletions ...hmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import java.util.Collection;

import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.sensors.FootSwitchFactory;
Expand All @@ -27,7 +26,8 @@ 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 double defaultVerticalVelocityHighThreshold = 0.3;
private double defaultJacobianDeterminantSingularityThreshold = 2e-3;

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

Expand Down Expand Up @@ -112,6 +113,11 @@ public void setDefaultVerticalVelocityThreshold(double defaultVerticalVelocityTh
this.defaultVerticalVelocityThreshold = defaultVerticalVelocityThreshold;
}

public void setDefaultJacobianDeterminantSingularityThreshold(double defaultJacobianDeterminantSingularityThreshold)
{
this.defaultJacobianDeterminantSingularityThreshold = defaultJacobianDeterminantSingularityThreshold;
}

@Override
public FootSwitchInterface newFootSwitch(String namePrefix,
ContactablePlaneBody foot,
Expand All @@ -136,6 +142,8 @@ public FootSwitchInterface newFootSwitch(String namePrefix,
verticalVelocityThreshold = new DoubleParameter(namePrefix + "VerticalVelocityThreshold", registry, defaultVerticalVelocityThreshold);
verticalVelocityHighThreshold = new DoubleParameter(namePrefix + "VerticalVelocityHighThreshold", registry, defaultVerticalVelocityHighThreshold);
horizontalVelocityThreshold = new DoubleParameter(namePrefix + "HorizontalVelocityThreshold", registry, defaultHorizontalVelocityThreshold);
jacobianDeterminantSingularityThreshold = new DoubleParameter(namePrefix + "JacobianDeterminantSingularityThreshold", registry,
defaultJacobianDeterminantSingularityThreshold);
}

return new JointTorqueBasedFootSwitch(namePrefix,
Expand All @@ -152,6 +160,7 @@ public FootSwitchInterface newFootSwitch(String namePrefix,
horizontalVelocityThreshold,
verticalVelocityThreshold,
verticalVelocityHighThreshold,
jacobianDeterminantSingularityThreshold,
useJacobianTranspose,
registry);
}
Expand Down
Loading