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..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,6 +62,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, DoubleProvider horizontalVelocityThreshold, DoubleProvider verticalVelocityThreshold, DoubleProvider verticalVelocityHighThreshold, + DoubleProvider jacobianDeterminantSingularityThreshold, BooleanProvider useJacobianTranspose, YoRegistry parentRegistry) { @@ -115,6 +116,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, horizontalVelocityThreshold, verticalVelocityThreshold, verticalVelocityHighThreshold, + jacobianDeterminantSingularityThreshold, 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..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 @@ -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; @@ -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; @@ -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; @@ -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, @@ -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, @@ -152,6 +160,7 @@ public FootSwitchInterface newFootSwitch(String namePrefix, horizontalVelocityThreshold, verticalVelocityThreshold, verticalVelocityHighThreshold, + jacobianDeterminantSingularityThreshold, useJacobianTranspose, registry); }