From bd987b50e3f7d0d106bdb99166d5cc90deafa710 Mon Sep 17 00:00:00 2001 From: Robert Date: Fri, 5 Dec 2025 15:55:00 -0600 Subject: [PATCH 1/3] set up some singularity filtering for the joint based contact switch --- .../JointTorqueBasedFootSwitch.java | 30 +++++++++++++++---- .../JointTorqueBasedFootSwitchFactory.java | 9 ++++++ 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java index 51747d1904d..52d8bd0b66d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java @@ -62,6 +62,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, DoubleProvider horizontalVelocityThreshold, DoubleProvider verticalVelocityThreshold, DoubleProvider verticalVelocityHighThreshold, + DoubleProvider jacobianDeterminantThreshold, BooleanProvider useJacobianTranspose, YoRegistry parentRegistry) { @@ -115,6 +116,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, horizontalVelocityThreshold, verticalVelocityThreshold, verticalVelocityHighThreshold, + jacobianDeterminantThreshold, registry); parentRegistry.addChild(registry); @@ -315,6 +317,7 @@ 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; @@ -322,6 +325,7 @@ private static class JacobianBasedBasedTouchdownDetector private final GlitchFilteredYoBoolean hasFootHitGroundFiltered; private final GlitchFilteredYoBoolean isPastCoPThresholdFiltered; + private final YoDouble jacobianDeterminant; private final YoDouble copDistance; private final YoDouble footForceMagnitude; private final YoDouble alphaFootLoadFiltering; @@ -344,6 +348,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot, DoubleProvider horizontalVelocityThreshold, DoubleProvider verticalVelocityThreshold, DoubleProvider verticalVelocityHighThreshold, + DoubleProvider jacobianDeterminantThreshold, YoRegistry registry) { this.soleFrame = soleFrame; @@ -355,6 +360,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot, this.horizontalVelocityThreshold = horizontalVelocityThreshold; this.verticalVelocityThreshold = verticalVelocityThreshold; this.verticalVelocityHighThreshold = verticalVelocityHighThreshold; + this.jacobianDeterminantThreshold = jacobianDeterminantThreshold; legJoints = MultiBodySystemTools.createOneDoFJointPath(pelvis, foot); @@ -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); @@ -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()); @@ -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(); } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java index aeaca84dc7b..0b48c594776 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java @@ -28,6 +28,7 @@ public class JointTorqueBasedFootSwitchFactory implements FootSwitchFactory private double defaultHorizontalVelocityThreshold = 0.5; private double defaultVerticalVelocityThreshold = 0.125; private double defaultVerticalVelocityHighThreshold = 0.5; + private double defaultJacobianDeterminantThreshold = 2e-3; private DoubleProvider contactThresholdTorque; private DoubleProvider higherContactThresholdTorque; @@ -38,6 +39,7 @@ public class JointTorqueBasedFootSwitchFactory implements FootSwitchFactory private DoubleProvider horizontalVelocityThreshold; private DoubleProvider verticalVelocityThreshold; private DoubleProvider verticalVelocityHighThreshold; + private DoubleProvider jacobianDeterminantThreshold; private YoInteger contactWindowSize; private BooleanProvider useJacobianTranspose; @@ -112,6 +114,11 @@ public void setDefaultVerticalVelocityThreshold(double defaultVerticalVelocityTh this.defaultVerticalVelocityThreshold = defaultVerticalVelocityThreshold; } + public void setDefaultJacobianDeterminantThreshold(double defaultJacobianDeterminantThreshold) + { + this.defaultJacobianDeterminantThreshold = defaultJacobianDeterminantThreshold; + } + @Override public FootSwitchInterface newFootSwitch(String namePrefix, ContactablePlaneBody foot, @@ -136,6 +143,7 @@ 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); + jacobianDeterminantThreshold = new DoubleParameter(namePrefix + "JacobianDeterminantThreshold", registry, defaultJacobianDeterminantThreshold); } return new JointTorqueBasedFootSwitch(namePrefix, @@ -152,6 +160,7 @@ public FootSwitchInterface newFootSwitch(String namePrefix, horizontalVelocityThreshold, verticalVelocityThreshold, verticalVelocityHighThreshold, + jacobianDeterminantThreshold, useJacobianTranspose, registry); } From 99886337d37138306576aec83dbb2adf49af3abd Mon Sep 17 00:00:00 2001 From: Robert Date: Mon, 15 Dec 2025 16:41:25 -0600 Subject: [PATCH 2/3] made a variable name even longer --- .../footSwitch/JointTorqueBasedFootSwitch.java | 4 ++-- .../JointTorqueBasedFootSwitchFactory.java | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java index 52d8bd0b66d..2dd50c532da 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java @@ -62,7 +62,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, DoubleProvider horizontalVelocityThreshold, DoubleProvider verticalVelocityThreshold, DoubleProvider verticalVelocityHighThreshold, - DoubleProvider jacobianDeterminantThreshold, + DoubleProvider jacobianDeterminantSingularityThreshold, BooleanProvider useJacobianTranspose, YoRegistry parentRegistry) { @@ -116,7 +116,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, horizontalVelocityThreshold, verticalVelocityThreshold, verticalVelocityHighThreshold, - jacobianDeterminantThreshold, + jacobianDeterminantSingularityThreshold, registry); parentRegistry.addChild(registry); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java index 0b48c594776..94134183a37 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java @@ -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; @@ -28,7 +27,7 @@ public class JointTorqueBasedFootSwitchFactory implements FootSwitchFactory private double defaultHorizontalVelocityThreshold = 0.5; private double defaultVerticalVelocityThreshold = 0.125; private double defaultVerticalVelocityHighThreshold = 0.5; - private double defaultJacobianDeterminantThreshold = 2e-3; + private double defaultJacobianDeterminantSingularityThreshold = 2e-3; private DoubleProvider contactThresholdTorque; private DoubleProvider higherContactThresholdTorque; @@ -39,7 +38,7 @@ public class JointTorqueBasedFootSwitchFactory implements FootSwitchFactory private DoubleProvider horizontalVelocityThreshold; private DoubleProvider verticalVelocityThreshold; private DoubleProvider verticalVelocityHighThreshold; - private DoubleProvider jacobianDeterminantThreshold; + private DoubleProvider jacobianDeterminantSingularityThreshold; private YoInteger contactWindowSize; private BooleanProvider useJacobianTranspose; @@ -114,9 +113,9 @@ public void setDefaultVerticalVelocityThreshold(double defaultVerticalVelocityTh this.defaultVerticalVelocityThreshold = defaultVerticalVelocityThreshold; } - public void setDefaultJacobianDeterminantThreshold(double defaultJacobianDeterminantThreshold) + public void setDefaultJacobianDeterminantSingularityThreshold(double defaultJacobianDeterminantSingularityThreshold) { - this.defaultJacobianDeterminantThreshold = defaultJacobianDeterminantThreshold; + this.defaultJacobianDeterminantSingularityThreshold = defaultJacobianDeterminantSingularityThreshold; } @Override @@ -143,7 +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); - jacobianDeterminantThreshold = new DoubleParameter(namePrefix + "JacobianDeterminantThreshold", registry, defaultJacobianDeterminantThreshold); + jacobianDeterminantSingularityThreshold = new DoubleParameter(namePrefix + "JacobianDeterminantSingularityThreshold", registry, + defaultJacobianDeterminantSingularityThreshold); } return new JointTorqueBasedFootSwitch(namePrefix, @@ -160,7 +160,7 @@ public FootSwitchInterface newFootSwitch(String namePrefix, horizontalVelocityThreshold, verticalVelocityThreshold, verticalVelocityHighThreshold, - jacobianDeterminantThreshold, + jacobianDeterminantSingularityThreshold, useJacobianTranspose, registry); } From ab9e3862eedad1781a412a6968ce9d18d759198a Mon Sep 17 00:00:00 2001 From: Alexander OCU Date: Tue, 16 Dec 2025 12:56:13 -0600 Subject: [PATCH 3/3] lowered the high velocity filter on the contact switch --- .../sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java index 94134183a37..38dc2e84b16 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitchFactory.java @@ -26,7 +26,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 double defaultVerticalVelocityHighThreshold = 0.3; private double defaultJacobianDeterminantSingularityThreshold = 2e-3; private DoubleProvider contactThresholdTorque;