From e667743399332bf9e64c434a885b1af5820b3979 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Mon, 17 Nov 2025 21:56:49 -0600 Subject: [PATCH 01/12] Bugfix/fix pelvis rotation on delays (#1106) * started working on a fix for pelvis rotationd elays * rolled back on how the time in the phase is found, and started adding a decay factor so the feedforward values dont go to the moon * fixed a bound inclusiveness problem --- .../capturePoint/ContactStateManager.java | 5 ++-- .../WalkingHighLevelHumanoidController.java | 11 +++++---- .../states/TransferToStandingState.java | 3 ++- .../states/WalkingSingleSupportState.java | 3 ++- .../WalkingTrajectoryPath.java | 23 +++++++++++++++++-- .../generators/MultiCubicSpline1DSolver.java | 5 ++-- 6 files changed, 38 insertions(+), 12 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ContactStateManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ContactStateManager.java index 612db564e7ac..e5b670935518 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ContactStateManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ContactStateManager.java @@ -114,7 +114,6 @@ public void initialize() public void initializeForStanding() { - inSingleSupport.set(false); inStanding.set(true); @@ -136,6 +135,7 @@ public void initializeForStanding() public void initializeForTransfer(double transferDuration, double swingDuration) { timeAtStartOfSupportSequence.set(time.getValue()); + timeInSupportSequence.set(0.0); currentStateDuration.set(transferDuration); totalStateDuration.set(transferDuration + swingDuration); @@ -159,6 +159,7 @@ public void initializeForSingleSupport(double transferDuration, double swingDura double stepDuration = transferDuration + swingDuration; timeAtStartOfSupportSequence.set(time.getValue() - transferDuration); + timeInSupportSequence.set(transferDuration); currentStateDuration.set(stepDuration); totalStateDuration.set(stepDuration); @@ -166,7 +167,6 @@ public void initializeForSingleSupport(double transferDuration, double swingDura adjustedRemainingTimeUnderDisturbance.set(swingDuration); offsetTimeInState.set(transferDuration); - adjustedTimeInSupportSequence.set(transferDuration); totalTimeAdjustment.set(0.0); @@ -176,6 +176,7 @@ public void initializeForSingleSupport(double transferDuration, double swingDura public void initializeForTransferToStanding(double finalTransferDuration ) { timeAtStartOfSupportSequence.set(time.getValue()); + timeInSupportSequence.set(0.0); currentStateDuration.set(finalTransferDuration); totalStateDuration.set(finalTransferDuration); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java index 9782e937c523..aa22513a409c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java @@ -46,6 +46,7 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.ControllerCoreOptimizationSettings; import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointLimitEnforcement; import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointLimitParameters; +import us.ihmc.commonWalkingControlModules.referenceFrames.WalkingTrajectoryPath; import us.ihmc.commonWalkingControlModules.staticEquilibrium.StabilityMarginRegionCalculator; import us.ihmc.commonWalkingControlModules.staticEquilibrium.WholeBodyContactState; import us.ihmc.commons.lists.RecyclingArrayList; @@ -589,10 +590,12 @@ public void initialize() private void initializeWalkingTrajectoryPath() { - controllerToolbox.getWalkingTrajectoryPath().clearFootsteps(); - controllerToolbox.getWalkingTrajectoryPath().reset(); - controllerToolbox.getWalkingTrajectoryPath().initializeDoubleSupport(); - controllerToolbox.getWalkingTrajectoryPath().updateTrajectory(FootControlModule.ConstraintType.FULL, FootControlModule.ConstraintType.FULL); + WalkingTrajectoryPath walkingTrajectoryPath = controllerToolbox.getWalkingTrajectoryPath(); + walkingTrajectoryPath.clearFootsteps(); + walkingTrajectoryPath.reset(); + walkingTrajectoryPath.initializeDoubleSupport(); + walkingTrajectoryPath.updateTrajectory(FootControlModule.ConstraintType.FULL, + FootControlModule.ConstraintType.FULL); } private void initializeManagers() diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java index 6de61fcdfc5a..abdfad8cb5c8 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java @@ -59,7 +59,8 @@ public void doAction(double timeInState) { balanceManager.computeICPPlan(); controllerToolbox.getWalkingTrajectoryPath() - .updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), feetManager.getCurrentConstraintType(RobotSide.RIGHT)); + .updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), + feetManager.getCurrentConstraintType(RobotSide.RIGHT)); switchToPointToeOffIfAlreadyInLine(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java index b4e22bc2ebfa..61acb9a35c07 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java @@ -256,7 +256,8 @@ private void updateWalkingTrajectoryPath() walkingTrajectoryPath.clearFootsteps(); walkingTrajectoryPath.addFootstep(nextFootstep, footstepTiming); walkingTrajectoryPath.addFootsteps(walkingMessageHandler); - walkingTrajectoryPath.updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), feetManager.getCurrentConstraintType(RobotSide.RIGHT)); + walkingTrajectoryPath.updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), + feetManager.getCurrentConstraintType(RobotSide.RIGHT)); } @Override diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/referenceFrames/WalkingTrajectoryPath.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/referenceFrames/WalkingTrajectoryPath.java index 984b60866cd6..4e33b0812806 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/referenceFrames/WalkingTrajectoryPath.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/referenceFrames/WalkingTrajectoryPath.java @@ -45,7 +45,6 @@ import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.filters.AlphaFilterTools; -import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable; import us.ihmc.yoVariables.math.YoMatrix; import us.ihmc.yoVariables.parameters.DoubleParameter; import us.ihmc.yoVariables.providers.DoubleProvider; @@ -77,6 +76,7 @@ public class WalkingTrajectoryPath implements SCS2YoGraphicHolder private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); private static final String WALKING_TRAJECTORY_PATH_FRAME_NAME = "walkingTrajectoryPathFrame"; public static final int WALKING_TRAJECTORY_FRAME_ID = WALKING_TRAJECTORY_PATH_FRAME_NAME.hashCode(); + private static final double DECAY_RATE = 10.0; private static final int MAX_NUMBER_OF_FOOTSTEPS = 2; @@ -105,6 +105,9 @@ public class WalkingTrajectoryPath implements SCS2YoGraphicHolder private final YoDouble currentYawAcceleration = new YoDouble(namePrefix + "CurrentYawAcceleration", registry); private final YoDouble finalTransferDuration = new YoDouble(namePrefix + "FinalTransferDuration", registry); + private final YoDouble decayRate = new YoDouble(namePrefix + "DecayRate", registry); + private final YoDouble decayFactor = new YoDouble(namePrefix + "DecayFactor", registry); + private final YoFrameVector3D initialLinearVelocity = new YoFrameVector3D(namePrefix + "InitialLinearVelocity", worldFrame, registry); private final YoDouble initialYawRate = new YoDouble(namePrefix + "InitialYawRate", registry); @@ -154,6 +157,7 @@ public WalkingTrajectoryPath(DoubleProvider time, this.soleFrames = soleFrames; trajectoryManager = new TrajectoryManager(registry); + decayRate.set(DECAY_RATE); YoGraphicsList yoGraphicList; if (yoGraphicsListRegistry != null) @@ -349,7 +353,16 @@ public void updateTrajectory(ConstraintType leftFootConstraintType, ConstraintTy if (updateWaypoints) updateWaypoints(); - double currentTime = MathTools.clamp(time.getValue() - startTime.getValue(), 0.0, totalDuration.getValue()); + double maxTime = totalDuration.getDoubleValue(); + if (!footsteps.isEmpty()) + { + if (isInDoubleSupport.getBooleanValue()) + maxTime = Math.min(footstepTimings.getFirst().getTransferTime(), maxTime); + else + maxTime = Math.min(footstepTimings.getFirst().getSwingTime(), maxTime); + } + double timeInState = time.getValue() - startTime.getDoubleValue(); + double currentTime = MathTools.clamp(timeInState, 0.0, maxTime); trajectoryManager.initialize(initialLinearVelocity, initialYawRate.getValue(), waypoints, isLastWaypointOpen.getValue()); trajectoryManager.computePosition(currentTime, currentPosition); @@ -359,6 +372,12 @@ public void updateTrajectory(ConstraintType leftFootConstraintType, ConstraintTy currentYawRate.set(trajectoryManager.computeYawRate(currentTime)); currentYawAcceleration.set(trajectoryManager.computeYawAcceleration(currentTime)); + // If we've overshot when this was supposed to end, decay the rate and acceleration + double overageTime = timeInState - currentTime; + decayFactor.set(Math.exp(-decayRate.getDoubleValue() * overageTime)); + currentYawRate.mul(decayFactor.getDoubleValue()); + currentYawAcceleration.mul(decayFactor.getDoubleValue() * decayFactor.getDoubleValue()); + walkingTrajectoryPathFrame.update(); walkingTrajectoryAcceleration.getLinearPart().setMatchingFrame(currentLinearAcceleration); walkingTrajectoryAcceleration.getAngularPart().set(0.0, 0.0, currentYawAcceleration.getValue()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultiCubicSpline1DSolver.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultiCubicSpline1DSolver.java index b76a5509b011..a520bf26bfc0 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultiCubicSpline1DSolver.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultiCubicSpline1DSolver.java @@ -465,9 +465,10 @@ public double computeVelocity(double time, DMatrixRMaj solution) public double computeAcceleration(double time, DMatrixRMaj solution) { - if (time <= 0.0) + // We don't want these bounds to be inclusive. The position and velocity bounds are not inclusive, as they return this trajectory at those bounds. + if (time < 0.0) return 0.0; - if (time >= 1.0) + if (time > 1.0) return 0.0; int index = findSolutionOffsetIndex(time, solution); From 8b3379fc016e128debe518362f9795105d999076 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Mon, 17 Nov 2025 22:20:27 -0600 Subject: [PATCH 02/12] =?UTF-8?q?added=20some=20changes=20to=20prefer=20pr?= =?UTF-8?q?evious=20reachable=20regions=20to=20yield=20more=E2=80=A6=20(#1?= =?UTF-8?q?105)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * added some changes to prefer previous reachable regions to yield more consistent, smoother results * reverted line change --------- Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> --- .../ErrorBasedStepAdjustmentController.java | 122 +++++++++++++----- .../MultiStepCaptureRegionCalculator.java | 20 ++- 2 files changed, 111 insertions(+), 31 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java index e04e7b9d22fd..b8a20f9ac569 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java @@ -9,12 +9,10 @@ import us.ihmc.commonWalkingControlModules.captureRegion.OneStepCaptureRegionCalculator; import us.ihmc.commonWalkingControlModules.configurations.SteppingParameters; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; -import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler; import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly; import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D; import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.*; import us.ihmc.euclid.tuple2D.Point2D; @@ -49,6 +47,8 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl { private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + private static final double AREA_TO_CONSIDER_SWITCHING = 0.05; + private static final boolean VISUALIZE = true; private static final boolean CONTINUOUSLY_UPDATE_DESIRED_POSITION = true; private static final int minTicksIntoStep = 5; @@ -78,7 +78,6 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl private final RecyclingArrayList upcomingFootstepContactPoints = new RecyclingArrayList<>(Point2D.class); private final YoFramePoint3D referenceFootstepPosition = new YoFramePoint3D(yoNamePrefix + "ReferenceFootstepPosition", worldFrame, registry); - private final FramePoint3D tempPoint = new FramePoint3D(); private final FramePoint2D tempPoint2D = new FramePoint2D(); private final YoFrameVector2D footstepAdjustment = new YoFrameVector2D(yoNamePrefix + "FootstepAdjustment", @@ -101,6 +100,13 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl private final YoBoolean footstepWasAdjusted = new YoBoolean(yoNamePrefix + "FootstepWasAdjusted", registry); + private enum ReachableRegion {FORWARD, BACKWARD, BASELINE} + private final YoDouble forwardReachableArea = new YoDouble(yoNamePrefix + "ForwardReachableArea", registry); + private final YoDouble backwardReachableArea = new YoDouble(yoNamePrefix + "BackwardReachableArea", registry); + private final YoDouble baselineReachableArea = new YoDouble(yoNamePrefix + "BaselineReachableArea", registry); + private final YoDouble areaToConsiderSwitching = new YoDouble(yoNamePrefix + "areaToConsiderSwitching", registry); + private final YoEnum selectedReachableRegion = new YoEnum<>(yoNamePrefix + "SelectedReachableRegion",registry, ReachableRegion.class, true); + private final BooleanProvider resetFootstepProjectionEachTick; private final DoubleProvider minimumTimeForStepAdjustment; private final DoubleParameter supportDistanceFromFront; @@ -125,7 +131,6 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl private final BipedSupportPolygons bipedSupportPolygons; - private final FramePoint3D vertexInWorld = new FramePoint3D(); private final FrameConvexPolygon2D allowableAreaForCoPInFoot = new FrameConvexPolygon2D(); public ErrorBasedStepAdjustmentController(WalkingControllerParameters walkingControllerParameters, @@ -189,6 +194,7 @@ public ErrorBasedStepAdjustmentController(WalkingControllerParameters walkingCon DoubleProvider innerLimit = new DoubleParameter(yoNamePrefix + "MinReachabilityWidth", registry, steppingParameters.getMinStepWidth()); DoubleProvider outerLimit = new DoubleParameter(yoNamePrefix + "MaxReachabilityWidth", registry, steppingParameters.getMaxStepWidth()); DoubleProvider inPlaceWidth = new DoubleParameter(yoNamePrefix + "InPlaceWidth", registry, steppingParameters.getInPlaceWidth()); + areaToConsiderSwitching.set(AREA_TO_CONSIDER_SWITCHING); reachabilityConstraintHandler = new StepAdjustmentReachabilityConstraint(soleZUpFrames, lengthLimit, @@ -353,6 +359,7 @@ public void initialize(double initialTime, RobotSide supportSide) previousFootstepSolution.set(footstepSolution.getPosition()); totalStepAdjustment.setToZero(); controlTicksIntoStep.set(0); + selectedReachableRegion.set(null); } @Override @@ -440,7 +447,8 @@ public void compute(double currentTime, if (environmentallyConstrained) { tempPoint2D.set(footstepSolution.getPosition()); - FrameConvexPolygon2DReadOnly reachability = getBestReachabilityConstraintToUseWhenNotIntersecting(); + computeBestReachabilityConstraintToUseWhenNotIntersecting(); + FrameConvexPolygon2DReadOnly reachability = getSelectedRegion(); if (!reachability.isPointInside(tempPoint2D)) reachability.orthogonalProjection(tempPoint2D); footstepSolution.getPosition().set(tempPoint2D); @@ -550,11 +558,13 @@ private void projectAdjustedStepIntoCaptureRegion(FramePoint3DReadOnly pointToPr if (!isTheCaptureRegionReachable()) { captureRegionInWorld.orthogonalProjection(adjustedSolution); - getBestReachabilityConstraintToUseWhenNotIntersecting().orthogonalProjection(adjustedSolution); + computeBestReachabilityConstraintToUseWhenNotIntersecting(); + getSelectedRegion().orthogonalProjection(adjustedSolution); } else { - getBestReachabilityConstraintToUseWhenIntersecting().orthogonalProjection(adjustedSolution); + computeBestReachabilityConstraintToUseWhenIntersecting(); + getSelectedRegion().orthogonalProjection(adjustedSolution); } footstepAdjustment.set(adjustedSolution); @@ -594,38 +604,56 @@ private boolean isTheCaptureRegionReachable() return intersect; } - private FrameConvexPolygon2DReadOnly getBestReachabilityConstraintToUseWhenNotIntersecting() + private void computeBestReachabilityConstraintToUseWhenNotIntersecting() { if (!allowCrossOverSteps.getValue()) - return reachabilityConstraintHandler.getReachabilityConstraint(); + { + selectedReachableRegion.set(ReachableRegion.BASELINE); + return; + } double distanceToForward = reachabilityConstraintHandler.getForwardCrossOverPolygon().distance(adjustedSolution); double distanceToBackward = reachabilityConstraintHandler.getBackwardCrossOverPolygon().distance(adjustedSolution); double distanceToNominal = reachabilityConstraintHandler.getReachabilityConstraint().distance(adjustedSolution); - boolean forwardIsCloser = distanceToForward < distanceToBackward; - - if (forwardIsCloser) + double distanceToPreviouslySelectedRegion = distanceToNominal; + if (selectedReachableRegion.getEnumValue() != null) { - if (distanceToNominal < distanceToForward) - return reachabilityConstraintHandler.getReachabilityConstraint(); + if (selectedReachableRegion.getEnumValue() == ReachableRegion.FORWARD) + distanceToPreviouslySelectedRegion = distanceToForward; + else if (selectedReachableRegion.getEnumValue() == ReachableRegion.BACKWARD) + distanceToPreviouslySelectedRegion = distanceToBackward; else - return reachabilityConstraintHandler.getForwardCrossOverPolygon(); + distanceToPreviouslySelectedRegion = distanceToNominal; } - else if (distanceToNominal < distanceToBackward) + else { - return reachabilityConstraintHandler.getReachabilityConstraint(); + // initialize this as the default + selectedReachableRegion.set(ReachableRegion.BASELINE); } - else + + boolean forwardIsCloser = distanceToForward < distanceToBackward; + + if (forwardIsCloser) + { + if (distanceToForward < distanceToPreviouslySelectedRegion) + { + selectedReachableRegion.set(ReachableRegion.FORWARD); + } + } + else if (distanceToBackward < distanceToPreviouslySelectedRegion) { - return reachabilityConstraintHandler.getBackwardCrossOverPolygon(); + selectedReachableRegion.set(ReachableRegion.BACKWARD); } } - private FrameConvexPolygon2DReadOnly getBestReachabilityConstraintToUseWhenIntersecting() + private void computeBestReachabilityConstraintToUseWhenIntersecting() { if (!allowCrossOverSteps.getValue()) - return reachableCaptureRegion; + { + selectedReachableRegion.set(ReachableRegion.BASELINE); + return; + } double forwardArea = forwardCrossOverReachableCaptureRegion.getArea(); double backwardArea = backwardCrossOverReachableCaptureRegion.getArea(); @@ -635,23 +663,57 @@ private FrameConvexPolygon2DReadOnly getBestReachabilityConstraintToUseWhenInter backwardArea = Double.isNaN(backwardArea) ? Double.NEGATIVE_INFINITY : backwardArea; reachableArea = Double.isNaN(reachableArea) ? Double.NEGATIVE_INFINITY : reachableArea; + forwardReachableArea.set(forwardArea); + backwardReachableArea.set(backwardArea); + baselineReachableArea.set(reachableArea); + + double areaOfPreviouslySelectedRegion = reachableArea; + if (selectedReachableRegion.getEnumValue() != null) + { + areaOfPreviouslySelectedRegion = getSelectedArea(); + // Don't switch if the area of the previous selection isn't too low + if (getSelectedArea() > areaToConsiderSwitching.getDoubleValue()) + return; + } + else + { + // initialize th is as the default + selectedReachableRegion.set(ReachableRegion.BASELINE); + } + boolean forwardIsLargestCrossoverArea = forwardArea > backwardArea; if (forwardIsLargestCrossoverArea) { - if (forwardArea > 2.0 * reachableArea) - return forwardCrossOverReachableCaptureRegion; - else - return reachableCaptureRegion; + if (forwardArea > 2.0 * areaOfPreviouslySelectedRegion) + { + selectedReachableRegion.set(ReachableRegion.FORWARD); + } } - else if (backwardArea > 2.0 * reachableArea) + else if (backwardArea > 2.0 * areaOfPreviouslySelectedRegion) { - return backwardCrossOverReachableCaptureRegion; + selectedReachableRegion.set(ReachableRegion.BACKWARD); } - else + } + + private double getSelectedArea() + { + return switch (selectedReachableRegion.getEnumValue()) { - return reachableCaptureRegion; - } + case FORWARD -> forwardReachableArea.getDoubleValue(); + case BACKWARD -> backwardReachableArea.getDoubleValue(); + case BASELINE -> baselineReachableArea.getDoubleValue(); + }; + } + + private FrameConvexPolygon2DReadOnly getSelectedRegion() + { + return switch (selectedReachableRegion.getEnumValue()) + { + case FORWARD -> forwardCrossOverReachableCaptureRegion; + case BACKWARD -> backwardCrossOverReachableCaptureRegion; + case BASELINE -> reachableCaptureRegion; + }; } private boolean deadbandAndApplyStepAdjustment() diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/captureRegion/MultiStepCaptureRegionCalculator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/captureRegion/MultiStepCaptureRegionCalculator.java index 0b8828bc5f15..90b601a0d575 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/captureRegion/MultiStepCaptureRegionCalculator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/captureRegion/MultiStepCaptureRegionCalculator.java @@ -3,6 +3,7 @@ import java.awt.*; import us.ihmc.commonWalkingControlModules.capturePoint.stepAdjustment.StepAdjustmentReachabilityConstraint; +import us.ihmc.commons.MathTools; import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.euclid.geometry.ConvexPolygon2D; import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly; @@ -35,6 +36,9 @@ */ public class MultiStepCaptureRegionCalculator implements SCS2YoGraphicHolder { + private static final double distanceThresholdToFilter = 0.005; + private static final double distanceThresholdToFilterSquared = MathTools.square(distanceThresholdToFilter); + private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); private final YoInteger stepsInQueue = new YoInteger("stepsInQueue", registry); @@ -162,7 +166,21 @@ public void compute(RobotSide currentStanceSide, FrameConvexPolygon2DReadOnly on expandCaptureRegion(regionToExpand, reachabilityPolygonsWithOrigin.get(currentStanceSide), multiStepRegion, currentSupportMultiplier); } - yoMultiStepRegion.setMatchingFrame(multiStepRegion, false); + multiStepRegion.update(); + updateInternalFilteringRepeats(); + } + + private void updateInternalFilteringRepeats() + { + yoMultiStepRegion.clear(); + for (int i = 0; i < multiStepRegion.getNumberOfVertices(); i++) + { + FramePoint2DReadOnly previousVertex = multiStepRegion.getPreviousVertex(i); + FramePoint2DReadOnly vertex = multiStepRegion.getVertex(i); + if (vertex.distanceSquared(previousVertex) > distanceThresholdToFilterSquared) + yoMultiStepRegion.addVertexMatchingFrame(multiStepRegion.getVertex(i), false); + } + yoMultiStepRegion.update(); } public FrameConvexPolygon2DReadOnly getCaptureRegion() From be4f0080fbc14570e2b5704a42847132e96152d5 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Mon, 17 Nov 2025 22:31:17 -0600 Subject: [PATCH 03/12] remove yaw component from angular velocity filtering of the foot (#1107) Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> --- .../PelvisLinearStateUpdater.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java index 4b7d34c250a1..f5f36f58fafb 100644 --- a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java @@ -11,6 +11,8 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics; import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.tools.EuclidCoreTools; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder; @@ -665,7 +667,8 @@ private int filterTrustedFeetBasedOnVelocity(int numberOfEndEffectorsTrusted) if (!areFeetTrusted.get(foot).getBooleanValue()) continue; - double footAngularVelocity = foot.getBodyFixedFrame().getTwistOfFrame().getAngularPart().norm(); + FrameVector3DReadOnly angularVelocity = foot.getBodyFixedFrame().getTwistOfFrame().getAngularPart(); + double footAngularVelocity = EuclidCoreTools.norm(angularVelocity.getX(), angularVelocity.getY()); footAngularVelocities.get(foot).set(footAngularVelocity); if (footAngularVelocity < slowestVelocity) From eb916084b9a22883c70d38624249686be5492bb8 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Mon, 17 Nov 2025 22:59:19 -0600 Subject: [PATCH 04/12] add a filter for big downward forces on the second threshold (#1108) Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> --- .../sensors/footSwitch/JointTorqueBasedFootSwitch.java | 10 ++++++++-- .../footSwitch/JointTorqueBasedFootSwitchFactory.java | 4 ++++ 2 files changed, 12 insertions(+), 2 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 ed4da1810ed5..51747d1904da 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 @@ -61,6 +61,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, BooleanProvider compensateGravity, DoubleProvider horizontalVelocityThreshold, DoubleProvider verticalVelocityThreshold, + DoubleProvider verticalVelocityHighThreshold, BooleanProvider useJacobianTranspose, YoRegistry parentRegistry) { @@ -113,6 +114,7 @@ public JointTorqueBasedFootSwitch(String namePrefix, compensateGravity, horizontalVelocityThreshold, verticalVelocityThreshold, + verticalVelocityHighThreshold, registry); parentRegistry.addChild(registry); @@ -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; @@ -340,6 +343,7 @@ public JacobianBasedBasedTouchdownDetector(RigidBodyBasics foot, BooleanProvider compensateGravity, DoubleProvider horizontalVelocityThreshold, DoubleProvider verticalVelocityThreshold, + DoubleProvider verticalVelocityHighThreshold, YoRegistry registry) { this.soleFrame = soleFrame; @@ -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); @@ -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)); 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 d33db69f6970..aeaca84dc7b1 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 @@ -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; @@ -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; @@ -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); } @@ -148,6 +151,7 @@ public FootSwitchInterface newFootSwitch(String namePrefix, compensateGravity, horizontalVelocityThreshold, verticalVelocityThreshold, + verticalVelocityHighThreshold, useJacobianTranspose, registry); } From 1d05473a606a842eb165ebb0e6bdcd1fbe207c47 Mon Sep 17 00:00:00 2001 From: Tomasz Bialek <103042423+TomaszTB@users.noreply.github.com> Date: Mon, 17 Nov 2025 23:13:52 -0600 Subject: [PATCH 05/12] [Feature] CSG Heartbeat (#1104) * Make ROS2HeartbeatMonitor garbage free * Add heartbeats to CSG objects * Added destroy method to CSGROS2CommunicationHelper and cleaned up usage in the panel --------- Co-authored-by: Alexander OCU Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> --- .../plugin/CSGROS2CommunicationHelper.java | 19 +++++++++++++++ .../StepGeneratorCommandInputManager.java | 24 ++++++++++++++++++- .../ros2/ROS2HeartbeatMonitor.java | 20 ++++++---------- .../java/us/ihmc/rdx/csg/RDXCSGPanel.java | 21 ++++------------ 4 files changed, 53 insertions(+), 31 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/CSGROS2CommunicationHelper.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/CSGROS2CommunicationHelper.java index e8e9fc95edb1..5c93ed47cee9 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/CSGROS2CommunicationHelper.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/CSGROS2CommunicationHelper.java @@ -3,19 +3,27 @@ import controller_msgs.msg.dds.ContinuousStepGeneratorInputMessage; import controller_msgs.msg.dds.ContinuousStepGeneratorParametersMessage; import controller_msgs.msg.dds.ContinuousStepGeneratorStatusMessage; +import std_msgs.msg.dds.Empty; import us.ihmc.commonWalkingControlModules.configurations.SteppingParameters; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.ContinuousStepGeneratorParametersBasics; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.ROS2Tools; +import us.ihmc.communication.ros2.ROS2Heartbeat; import us.ihmc.ros2.QueuedROS2Subscription; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Publisher; +import us.ihmc.ros2.ROS2Topic; import java.util.function.Consumer; public class CSGROS2CommunicationHelper { + public static final ROS2Topic CSG_HEARTBEAT_TOPIC = new ROS2Topic<>().withPrefix("ihmc") + .withModule("continuous_step_generator") + .withSuffix("heartbeat") + .withType(Empty.class); + private final ROS2Node ros2Node; private final String robotName; @@ -30,6 +38,9 @@ public class CSGROS2CommunicationHelper private final ROS2Publisher csgInputCommandPublisher; private final ROS2Publisher csgParametersCommandPublisher; + // Heartbeat to ensure StepGeneratorCommandInputManager stops walking if connection is broken + private final ROS2Heartbeat heartbeat; + public CSGROS2CommunicationHelper(String robotName, ROS2Node ros2Node, WalkingControllerParameters walkingControllerParameters) { this(robotName, ros2Node); @@ -53,6 +64,9 @@ public CSGROS2CommunicationHelper(String robotName, ROS2Node ros2Node) csgInputCommandPublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(ContinuousStepGeneratorInputMessage.class, robotName)); csgParametersCommandPublisher = ros2Node.createPublisher(HumanoidControllerAPI.getTopic(ContinuousStepGeneratorParametersMessage.class, robotName)); + + heartbeat = new ROS2Heartbeat(ros2Node, CSG_HEARTBEAT_TOPIC); + heartbeat.setAlive(true); // Heartbeat stays alive. The StepGeneratorCommandInputManager will stop if this process dies or Wi-Fi connection is lost } public void publish(ContinuousStepGeneratorInputMessage continuousStepGeneratorInputMessage) @@ -147,4 +161,9 @@ public ContinuousStepGeneratorStatusMessage getCSGStatusMessage() { return csgStatusMessage; } + + public void destroy() + { + heartbeat.destroy(); + } } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java index c1c3fe0ed23b..8b473d5eb58a 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/plugin/StepGeneratorCommandInputManager.java @@ -12,12 +12,15 @@ import us.ihmc.communication.controllerAPI.CommandInputManager; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.communication.controllerAPI.command.Command; +import us.ihmc.communication.ros2.ROS2HeartbeatMonitor; import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HeightMapCommand; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepStatus; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2NodeBuilder; import us.ihmc.yoVariables.euclid.YoVector2D; import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.providers.DoubleProvider; @@ -43,6 +46,7 @@ public class StepGeneratorCommandInputManager implements Updatable private final YoDouble turningVelocity; private final YoDouble swingHeight; private final YoBoolean isUnitVelocities; + private final YoBoolean overrideHeartbeat; private int ticksToUpdateTheEnvironment = Integer.MAX_VALUE; private HighLevelControllerName currentController; private ContinuousStepGenerator continuousStepGenerator; @@ -56,6 +60,11 @@ public class StepGeneratorCommandInputManager implements Updatable private final AtomicBoolean shouldSubmitNewRegions = new AtomicBoolean(true); private final AtomicInteger ticksSinceUpdatingTheEnvironment = new AtomicInteger(0); + // ROS 2 stuff + // TODO: Destroy these objects properly + private final ROS2Node ros2Node; + private final ROS2HeartbeatMonitor heartbeatMonitor; + public StepGeneratorCommandInputManager() { String suffix = "StepGeneratorCommandInputManager"; @@ -65,9 +74,14 @@ public StepGeneratorCommandInputManager() swingHeight = new YoDouble("desiredSwingHeight_" + suffix, registry); isUnitVelocities = new YoBoolean("isUnitVelocities_" + suffix, registry); isUnitVelocities.set(false); + overrideHeartbeat = new YoBoolean("overrideHeartbeat_" + suffix, registry); + overrideHeartbeat.set(false); // by default, command input manager is not enabled, set enabled only if #update is called commandInputManager.setEnabled(false); + + ros2Node = new ROS2NodeBuilder().build(getClass().getSimpleName().toLowerCase() + "Node"); + heartbeatMonitor = new ROS2HeartbeatMonitor(ros2Node, CSGROS2CommunicationHelper.CSG_HEARTBEAT_TOPIC); } public void setCSG(ContinuousStepGenerator continuousStepGenerator) @@ -138,7 +152,15 @@ public void update(double time) isOpen = currentController == HighLevelControllerName.WALKING || currentController == HighLevelControllerName.QUICKSTER; commandInputManager.setEnabled(isOpen); - if (commandInputManager.isNewCommandAvailable(ContinuousStepGeneratorInputCommand.class)) + if (!overrideHeartbeat.getValue() && !heartbeatMonitor.isAlive()) + { + desiredVelocity.setX(0.0); + desiredVelocity.setY(0.0); + turningVelocity.set(0.0); + isUnitVelocities.set(false); + walk.set(false); + } + else if (commandInputManager.isNewCommandAvailable(ContinuousStepGeneratorInputCommand.class)) { ContinuousStepGeneratorInputCommand command = commandInputManager.pollNewestCommand(ContinuousStepGeneratorInputCommand.class); desiredVelocity.setX(command.getForwardVelocity()); diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java index 9d2750242b4e..e29db962ac4d 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java @@ -4,12 +4,11 @@ import us.ihmc.commons.exception.DefaultExceptionHandler; import us.ihmc.commons.exception.ExceptionTools; import us.ihmc.commons.thread.ThreadTools; -import us.ihmc.commons.thread.TypedNotification; -import us.ihmc.ros2.ROS2Input; +import us.ihmc.commons.thread.Throttler; +import us.ihmc.pubsub.subscriber.Subscriber; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Topic; import us.ihmc.tools.Timer; -import us.ihmc.commons.thread.Throttler; import java.util.function.Consumer; @@ -28,24 +27,25 @@ public class ROS2HeartbeatMonitor */ public static final double HEARTBEAT_EXPIRATION = 1.25 * ROS2Heartbeat.HEARTBEAT_PERIOD; private final Timer timer = new Timer(); + private final Empty heartbeatMessage = new Empty(); // To provide callback when aliveness changes private volatile boolean running = true; private final Throttler throttler = new Throttler(); private boolean wasAlive = false; private Consumer callback = null; - private final TypedNotification alivenessChangedNotification = new TypedNotification<>(); public ROS2HeartbeatMonitor(ROS2Node ros2Node, ROS2Topic heartbeatTopic) { - ros2Node.createSubscription2(heartbeatTopic, this::receivedHeartbeat); + ros2Node.createSubscription(heartbeatTopic, this::receivedHeartbeat); ThreadTools.startAsDaemon(() -> ExceptionTools.handle(this::monitorThread, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE), "HeartbeatMonitor"); } - private synchronized void receivedHeartbeat(Empty empty) + private synchronized void receivedHeartbeat(Subscriber subscriber) { - timer.reset(); + if (subscriber.takeNextData(heartbeatMessage, null)) + timer.reset(); } /** @@ -77,16 +77,10 @@ private void monitorThread() if (callback != null) callback.accept(isAlive); - alivenessChangedNotification.set(isAlive); } } } - public TypedNotification getAlivenessChangedNotification() - { - return alivenessChangedNotification; - } - public void destroy() { running = false; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/csg/RDXCSGPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/csg/RDXCSGPanel.java index 5eeb633556c7..6a19f78341de 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/csg/RDXCSGPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/csg/RDXCSGPanel.java @@ -4,12 +4,10 @@ import controller_msgs.msg.dds.ContinuousStepGeneratorParametersMessage; import controller_msgs.msg.dds.ContinuousStepGeneratorStatusMessage; import imgui.ImGui; -import imgui.flag.ImGuiMouseButton; import imgui.type.ImBoolean; import imgui.type.ImDouble; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.CSGROS2CommunicationHelper; -import us.ihmc.commons.thread.Throttler; import us.ihmc.rdx.imgui.ImGuiTools; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.imgui.RDXPanel; @@ -17,8 +15,6 @@ public class RDXCSGPanel extends RDXPanel { - private static final double THROTTLER_THREAD_HERTZ = 11.0; - private static final boolean PUBLISH_IN_VELOCITY_UNITS = true; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); @@ -53,26 +49,16 @@ public class RDXCSGPanel extends RDXPanel private boolean resetDesiredLateralVelocityNextTick = false; private boolean resetDesiredTurningVelocityNextTick = false; - // Throttler for ensuring we aren't publishing too fast/often - private final Throttler csgROS2PublisherThrottler; - public RDXCSGPanel(DRCRobotModel robotModel, ROS2Node ros2Node, boolean createXboxPlugin) - { - this(new CSGROS2CommunicationHelper(robotModel.getSimpleRobotName(), ros2Node, robotModel.getWalkingControllerParameters()), createXboxPlugin); - } - - public RDXCSGPanel(CSGROS2CommunicationHelper communicationHelper, boolean createXboxPlugin) { super("CSG Controls"); super.setRenderMethod(this::renderImGuiWidgets); - this.communicationHelper = communicationHelper; + communicationHelper = new CSGROS2CommunicationHelper(robotModel.getSimpleRobotName(), ros2Node, robotModel.getWalkingControllerParameters()); csgParametersCommand = communicationHelper.getCSGParametersCommand(); csgStatusMessage = communicationHelper.getCSGStatusMessage(); - csgROS2PublisherThrottler = new Throttler().setFrequency(THROTTLER_THREAD_HERTZ); - if (createXboxPlugin) xBoxOneCSGPlugin = new RDXXboxOneCSGPlugin(communicationHelper); } @@ -87,7 +73,6 @@ public void update() public void renderImGuiWidgets() { boolean publishCSGInputCommand = false; - boolean publishCSGParametersCommand = false; reset(csgStatusMessage); @@ -208,7 +193,7 @@ public void renderImGuiWidgets() csgParametersCommand.setSwingHeight(swingHeight.get()); publishCSGInputCommand = publishCSGInputCommand || requestCSGWalkingChanged || desiredForwardVelocityChanged || desiredLateralVelocityChanged || desiredTurningVelocityChanged; - publishCSGParametersCommand = swingDurationChanged || transferDurationChanged || maxStepLengthForwardsChanged || maxStepLengthBackwardsChanged || maxStepWidthChanged || swingHeightChanged; + boolean publishCSGParametersCommand = swingDurationChanged || transferDurationChanged || maxStepLengthForwardsChanged || maxStepLengthBackwardsChanged || maxStepWidthChanged || swingHeightChanged; if (publishCSGInputCommand) communicationHelper.publish(csgInputCommand); @@ -238,5 +223,7 @@ public void destroy() { if (xBoxOneCSGPlugin != null) xBoxOneCSGPlugin.shutDownXboxJoystick(); + + communicationHelper.destroy(); } } From 0f8c94fdfd7c59ab3d143b82489ec6914657e7af Mon Sep 17 00:00:00 2001 From: Robert Date: Tue, 18 Nov 2025 00:56:31 -0600 Subject: [PATCH 06/12] added the special case for the stairs --- .../HumanoidEndToEndStairsTest.java | 92 ++++++++++++++++++- ...LookAheadCoMHeightTrajectoryGenerator.java | 5 +- .../states/StandingState.java | 33 ++++++- .../states/TransferToStandingState.java | 8 +- .../AlexanderEndToEndStairsTest.java | 68 -------------- .../AlexanderEndToEndStairsTest.java | 7 ++ 6 files changed, 134 insertions(+), 79 deletions(-) delete mode 100644 open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java index 3b1dc38a1ec2..3d7d07b63375 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java +++ b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java @@ -85,7 +85,8 @@ public void setUseExperimentalPhysicsEngine(boolean useExperimentalPhysicsEngine this.useExperimentalPhysicsEngine = useExperimentalPhysicsEngine; } - public void testStairs(TestInfo testInfo, boolean squareUpSteps, boolean goingUp, double swingDuration, double transferDuration, double heightOffset) throws Exception + public void testStairs(TestInfo testInfo, boolean squareUpSteps, boolean goingUp, double swingDuration, double transferDuration, double heightOffset) + throws Exception { testStairs(testInfo, squareUpSteps, goingUp, swingDuration, transferDuration, heightOffset, null); } @@ -96,8 +97,7 @@ public void testStairs(TestInfo testInfo, double swingDuration, double transferDuration, double heightOffset, - Consumer corruptor) - throws Exception + Consumer corruptor) throws Exception { DRCRobotModel robotModel = getRobotModel(); double actualFootLength = robotModel.getWalkingControllerParameters().getSteppingParameters().getActualFootLength(); @@ -105,7 +105,9 @@ public void testStairs(TestInfo testInfo, double startZ = goingUp ? 0.0 : numberOfSteps * stepHeight; StairsEnvironment environment = new StairsEnvironment(numberOfSteps, stepHeight, stepLength, true); - SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel, environment, simulationTestingParameters); + SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel, + environment, + simulationTestingParameters); simulationTestHelperFactory.setStartingLocationOffset(new OffsetAndYawRobotInitialSetup(startX, 0, startZ)); simulationTestHelperFactory.setUseImpulseBasedPhysicsEngine(useExperimentalPhysicsEngine); simulationTestHelper = simulationTestHelperFactory.createAvatarTestingSimulation(); @@ -138,6 +140,88 @@ public void testStairs(TestInfo testInfo, } } + public void testSpecialStairs(boolean goingUp, + double swingDuration, + double transferDuration) throws Exception + { + DRCRobotModel robotModel = getRobotModel(); + double startX = 0.4; + double startZ = goingUp ? 0.0 : numberOfSteps * stepHeight; + + // 9" high step + double stepLength = 0.3; + double stepHeight = 9 * 2.54 / 100.0; + double stanceWidth = 0.22; + + StairsEnvironment environment = new StairsEnvironment(2, stepHeight, stepLength, true); + SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel, + environment, + simulationTestingParameters); + simulationTestHelperFactory.setStartingLocationOffset(new OffsetAndYawRobotInitialSetup(startX, 0, startZ)); + simulationTestHelperFactory.setUseImpulseBasedPhysicsEngine(useExperimentalPhysicsEngine); + simulationTestHelper = simulationTestHelperFactory.createAvatarTestingSimulation(); + simulationTestHelper.start(); + + simulationTestHelper.setCameraFocusPosition(startX, 0.0, 0.8 + startZ); + simulationTestHelper.setCameraPosition(startX, -5.0, 0.8 + startZ); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + FootstepDataListMessage firstStepUpList = new FootstepDataListMessage(); + FootstepDataMessage firstStepUp = firstStepUpList.getFootstepDataList().add(); + firstStepUp.setRobotSide(RobotSide.LEFT.toByte()); + firstStepUp.getLocation().setX(startX + stepLength); + firstStepUp.getLocation().setY(stanceWidth / 2.0); + firstStepUp.getLocation().setZ(stepHeight); + firstStepUp.setSwingDuration(swingDuration); + firstStepUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, firstStepUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + FootstepDataListMessage firstSquareUpList = new FootstepDataListMessage(); + FootstepDataMessage firstSquareUp = firstSquareUpList.getFootstepDataList().add(); + firstSquareUp.setRobotSide(RobotSide.RIGHT.toByte()); + firstSquareUp.getLocation().setX(startX + stepLength); + firstSquareUp.getLocation().setY(-stanceWidth / 2.0); + firstSquareUp.getLocation().setZ(stepHeight); + firstSquareUp.setSwingDuration(swingDuration); + firstSquareUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, firstSquareUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + FootstepDataListMessage secondStepUpList = new FootstepDataListMessage(); + FootstepDataMessage secondStepUp = secondStepUpList.getFootstepDataList().add(); + secondStepUp.setRobotSide(RobotSide.LEFT.toByte()); + secondStepUp.getLocation().setX(2.0 * startX + stepLength); + secondStepUp.getLocation().setY(stanceWidth / 2.0); + secondStepUp.getLocation().setZ(2.0 * stepHeight); + secondStepUp.setSwingDuration(swingDuration); + secondStepUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, secondStepUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + + FootstepDataListMessage secondSquareUpList = new FootstepDataListMessage(); + FootstepDataMessage secondSquareUp = secondSquareUpList.getFootstepDataList().add(); + secondSquareUp.setRobotSide(RobotSide.RIGHT.toByte()); + secondSquareUp.getLocation().setX(2.0 * startX + stepLength); + secondSquareUp.getLocation().setY(-stanceWidth / 2.0); + secondSquareUp.getLocation().setZ(2.0 * stepHeight); + secondSquareUp.setSwingDuration(swingDuration); + secondSquareUp.setTransferDuration(transferDuration); + + publishFootstepsAndSimulate(robotModel, secondSquareUpList); + + assertTrue(simulationTestHelper.simulateNow(1.0)); + + } + private void publishHeightOffset(double heightOffset) { if (!Double.isFinite(heightOffset) || EuclidCoreTools.epsilonEquals(0.0, heightOffset, 1.0e-3)) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java index f73c85141907..44a22f5c065c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java @@ -422,9 +422,10 @@ private void computeWaypoints(FramePoint3DReadOnly startCoMPosition, fourthMidpoint.setXY(fourthMidpointX, fourthMidpointY); endWaypoint.setXY(endWaypointX, endWaypointY); - // Handle the case where the feet are basically squared up when stepping up. In that case, the max height needs to be pinned by the "from" foot, as the + // Handle the case where the feet are basically squared up when stepping up, but are at different heights In that case, the max height needs to be pinned + // by the "from" foot, as the // way the phase variable works means we can approach what is meant to be swing on the "to" foot while still in the "from" foot. - if (Math.abs(transferToPosition.getX()) < 0.1 && endGroundHeight > startGroundHeight) + if (Math.abs(transferToPosition.getX()) < 0.1 && endGroundHeight > startGroundHeight && transferToPosition.getZ() > transferFromPosition.getZ() + 0.05) { double blend = Math.abs(transferToPosition.getX()) / 0.1; double minHeight = Math.min(startGroundHeight + extraToeOffHeight, endGroundHeight); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java index 9cb731604944..08aaf271dff4 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java @@ -12,6 +12,7 @@ import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler; import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; import us.ihmc.communication.controllerAPI.CommandInputManager; +import us.ihmc.humanoidRobotics.footstep.Footstep; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.robotSide.RobotSide; @@ -34,6 +35,8 @@ public class StandingState extends WalkingState private final SideDependentList handManagers = new SideDependentList<>(); private final FeetManager feetManager; + private RobotSide supportingSide; + public StandingState(CommandInputManager commandInputManager, WalkingMessageHandler walkingMessageHandler, TouchdownErrorCompensator touchdownErrorCompensator, @@ -74,7 +77,7 @@ public StandingState(CommandInputManager commandInputManager, public void doAction(double timeInState) { if (!holdDesiredHeightConstantWhenStanding) - comHeightManager.setSupportLeg(RobotSide.LEFT); + comHeightManager.setSupportLeg(supportingSide); balanceManager.computeICPPlan(); controllerToolbox.getWalkingTrajectoryPath().updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), feetManager.getCurrentConstraintType(RobotSide.RIGHT), @@ -99,10 +102,17 @@ public void onEntry() if (holdDesiredHeightConstantWhenStanding) { comHeightManager.initializeToNominalDesiredHeight(); + supportingSide = RobotSide.LEFT; } else { - TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(RobotSide.RIGHT); + Footstep footstepLeft = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.LEFT); + Footstep footstepRight = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.RIGHT); + + supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight); + supportingSide = supportingSide == null ? RobotSide.LEFT : supportingSide; + + TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide.getOppositeSide()); comHeightManager.initialize(transferToAndNextFootstepsDataForDoubleSupport, 0.0); } @@ -151,4 +161,23 @@ public boolean isDone(double timeInState) { return true; } + + private RobotSide getSideCarryingMostWeight(Footstep leftFootstep, Footstep rightFootstep) + { + WalkingStateEnum previousWalkingState = getPreviousWalkingStateEnum(); + if (previousWalkingState == null) + return null; + + RobotSide mostSupportingSide = null; + boolean leftStepLower = leftFootstep.getZ() <= rightFootstep.getZ(); + boolean rightStepLower = leftFootstep.getZ() > rightFootstep.getZ(); + if (leftStepLower) + mostSupportingSide = RobotSide.LEFT; + else if (rightStepLower) + mostSupportingSide = RobotSide.RIGHT; + else if (previousWalkingState.getTransferToSide() != null) + mostSupportingSide = previousWalkingState.getTransferToSide().getOppositeSide(); + + return mostSupportingSide; + } } \ No newline at end of file diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java index abdfad8cb5c8..9f2a57904467 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java @@ -11,6 +11,7 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox; import us.ihmc.commonWalkingControlModules.referenceFrames.WalkingTrajectoryPath; import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.humanoidRobotics.footstep.Footstep; @@ -33,6 +34,7 @@ public class TransferToStandingState extends WalkingState private final FeetManager feetManager; private final Point3D midFootPosition = new Point3D(); + private RobotSide supportingSide; public TransferToStandingState(WalkingMessageHandler walkingMessageHandler, TouchdownErrorCompensator touchdownErrorCompensator, @@ -65,7 +67,7 @@ public void doAction(double timeInState) switchToPointToeOffIfAlreadyInLine(); // Always do this so that when a foot slips or is loaded in the air, the height gets adjusted. - comHeightManager.setSupportLeg(RobotSide.LEFT); + comHeightManager.setSupportLeg(supportingSide); } private final FramePoint2D desiredCoP = new FramePoint2D(); @@ -171,14 +173,14 @@ else if (previousStateEnum.getTransferToSide() != null) Footstep footstepLeft = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.LEFT); Footstep footstepRight = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.RIGHT); - RobotSide supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight); + supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight); supportingSide = supportingSide == null ? RobotSide.RIGHT : supportingSide; double extraToeOffHeight = 0.0; if (feetManager.getCurrentConstraintType(supportingSide.getOppositeSide()) == FootControlModule.ConstraintType.TOES) extraToeOffHeight = feetManager.getExtraCoMMaxHeightWithToes(); - TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide); + TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide.getOppositeSide()); comHeightManager.setSupportLeg(supportingSide); comHeightManager.initialize(transferToAndNextFootstepsDataForDoubleSupport, extraToeOffHeight); diff --git a/open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java b/open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java deleted file mode 100644 index 66f6e7524501..000000000000 --- a/open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java +++ /dev/null @@ -1,68 +0,0 @@ -package us.ihmc.openAlexander.controllerAPI; - -import org.junit.jupiter.api.Disabled; -import org.junit.jupiter.api.Test; -import org.junit.jupiter.api.TestInfo; -import us.ihmc.openAlexander.OpenAlexanderVersion; -import us.ihmc.openAlexander.OpenAlexanderRobotModel; -import us.ihmc.avatar.drcRobot.DRCRobotModel; -import us.ihmc.avatar.roughTerrainWalking.HumanoidEndToEndStairsTest; -import us.ihmc.simulationConstructionSetTools.tools.CITools; -import us.ihmc.simulationConstructionSetTools.tools.CITools.SimpleRobotNameKeys; - -public class AlexanderEndToEndStairsTest extends HumanoidEndToEndStairsTest -{ - @Override - public DRCRobotModel getRobotModel() - { - return new OpenAlexanderRobotModel(OpenAlexanderVersion.V1_FULL_ROBOT); - } - - @Disabled - @Test - public void testUpStairsSlow(TestInfo testInfo) throws Exception - { - testStairs(testInfo, true, true, 0.6, 0.25, 0.0); - } - - @Disabled - @Test - public void testDownStairsSlow(TestInfo testInfo) throws Exception - { - testStairs(testInfo, true, false, 0.9, 0.25, 0.0); - } - - @Disabled - @Test - public void testUpStairs(TestInfo testInfo) throws Exception - { - testStairs(testInfo, false, true, 0.9, 0.25, 0.0); - } - - @Disabled - @Test - public void testDownStairs(TestInfo testInfo) throws Exception - { - testStairs(testInfo, false, false, 1.0, 0.35, 0.0); - } - - /** - * Test for comparison against real robot data \\Gideon\LogData\Atlas\20210624_AtlasDemoFilming\20210624_140852_AtlasStairsPipeBombDisposalBestRunYet - */ - @Disabled - @Test - public void testUpStairsSlowFBDemo(TestInfo testInfo) throws Exception - { - setStepHeight(6.75 * 0.0254); - setStepLength(0.285); - setUseExperimentalPhysicsEngine(true); - setNumberOfSteps(3); - testStairs(testInfo, true, true, 2.0, 0.8, 0.0); - } - - @Override - public String getSimpleRobotName() - { - return CITools.getSimpleRobotNameFor(SimpleRobotNameKeys.ALEXANDER); - } -} diff --git a/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java b/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java index f7d30a11e0b7..e5ea6e77c863 100644 --- a/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java +++ b/open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java @@ -42,4 +42,11 @@ public void testDownStairs(TestInfo testInfo) throws Exception { testStairs(testInfo, false, false, 1.0, 0.35, 0.0); } + + @Test + public void testSpecialStairs() throws Exception + { + testSpecialStairs(true, 0.8, 0.5); + } + } From 55b0690675accd8efacdab8e3eb4be48d00ab1bc Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 18 Nov 2025 09:58:11 -0600 Subject: [PATCH 07/12] Add null check to ImageSensor --- ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java index 9c5a6fe6f025..45c72bfefac1 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java @@ -199,6 +199,9 @@ public void grabAndNotify() { RawImage image = getImage(imageKey); + if (image == null) + return; + if (!queue.offer(image)) { // Meaning we couldn't add the image to the queue From 0eb9c5e1ee4596fa63b3c19befca74d6411f9f0d Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Tue, 18 Nov 2025 15:44:29 -0600 Subject: [PATCH 08/12] Bugfix/fix capture region and rate limit (#1110) * fixed the reachability constraint return * properly reset the feedback conditions for the ICP feedback --- .../controller/ICPController.java | 8 +++++-- .../controller/ICPControllerQPSolver.java | 21 +++++++++++++++++++ .../ErrorBasedStepAdjustmentController.java | 19 +++++++++++++---- 3 files changed, 42 insertions(+), 6 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java index 87ec0faf176c..c61e01ad730a 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java @@ -417,8 +417,7 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly UnrolledInverseFromMinor_DDRM.inv(transformedGains, inverseTransformedGains); - solver.resetCoPFeedbackConditions(); - solver.resetFeedbackDirection(); + solver.resetFeedbackConditions(); solver.setFeedbackConditions(scaledCoPFeedbackWeight, transformedGains, dynamicsObjectiveWeight.getValue()); solver.setMaxCMPDistanceFromEdge(maxAllowedDistanceCMPSupport.getValue()); solver.setCopSafeDistanceToEdge(safeCoPDistanceToEdge.getValue()); @@ -442,6 +441,11 @@ private void submitSolverTaskConditions(FrameConvexPolygon2DReadOnly supportPoly solver.setMaximumFeedbackRate(feedbackGains.getFeedbackPartMaxRate(), controlDT); } + else + { + solver.removeMaximumFeedbackMagnitude(); + solver.removeFeedbackRateLimit(); + } ignoreRateLimitThisTick = false; double feedbackRateWeight = usingHighCoPDamping.getValue() ? highlyDampedFeedbackRateWeight.getValue() : this.feedbackRateWeight.getValue(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerQPSolver.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerQPSolver.java index b7932170ed21..c6a51ba72c0d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerQPSolver.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerQPSolver.java @@ -294,6 +294,18 @@ public void setMaximumFeedbackRate(double maximumFeedbackRate, double controlDT) this.controlDT = controlDT; } + public void removeFeedbackRateLimit() + { + this.maximumFeedbackRate = Double.POSITIVE_INFINITY; + } + + public void removeMaximumFeedbackMagnitude() + { + CommonOps_DDRM.setIdentity(maxFeedbackTransform); + maxFeedbackXMagnitude = Double.POSITIVE_INFINITY; + maxFeedbackYMagnitude = Double.POSITIVE_INFINITY; + } + /** * Zeros all the pertinent scalars, vectors, and matrices for the solver. Should be called at the beginning of every computation tick. */ @@ -357,6 +369,15 @@ private void reshape() solution.reshape(problemSize, 1); } + public void resetFeedbackConditions() + { + resetCoPFeedbackConditions(); + resetCMPFeedbackConditions(); + resetFeedbackDirection(); + removeFeedbackRateLimit(); + removeMaximumFeedbackMagnitude(); + } + /** * Resets the controller conditions on the feedback minimization task, the feedback gains, and the dynamic relaxation minimization task. * Also sets that the controller is not to attempt to regularize the feedback minimization task. diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java index b8a20f9ac569..f4790281bce9 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java @@ -448,7 +448,7 @@ public void compute(double currentTime, { tempPoint2D.set(footstepSolution.getPosition()); computeBestReachabilityConstraintToUseWhenNotIntersecting(); - FrameConvexPolygon2DReadOnly reachability = getSelectedRegion(); + FrameConvexPolygon2DReadOnly reachability = getSelectedReachableRegion(); if (!reachability.isPointInside(tempPoint2D)) reachability.orthogonalProjection(tempPoint2D); footstepSolution.getPosition().set(tempPoint2D); @@ -559,12 +559,12 @@ private void projectAdjustedStepIntoCaptureRegion(FramePoint3DReadOnly pointToPr { captureRegionInWorld.orthogonalProjection(adjustedSolution); computeBestReachabilityConstraintToUseWhenNotIntersecting(); - getSelectedRegion().orthogonalProjection(adjustedSolution); + getSelectedReachableRegion().orthogonalProjection(adjustedSolution); } else { computeBestReachabilityConstraintToUseWhenIntersecting(); - getSelectedRegion().orthogonalProjection(adjustedSolution); + getSelectedReachableCaptureRegion().orthogonalProjection(adjustedSolution); } footstepAdjustment.set(adjustedSolution); @@ -706,7 +706,7 @@ private double getSelectedArea() }; } - private FrameConvexPolygon2DReadOnly getSelectedRegion() + private FrameConvexPolygon2DReadOnly getSelectedReachableCaptureRegion() { return switch (selectedReachableRegion.getEnumValue()) { @@ -716,6 +716,17 @@ private FrameConvexPolygon2DReadOnly getSelectedRegion() }; } + private FrameConvexPolygon2DReadOnly getSelectedReachableRegion() + { + return switch (selectedReachableRegion.getEnumValue()) + { + case FORWARD -> reachabilityConstraintHandler.getForwardCrossOverPolygon(); + case BACKWARD -> reachabilityConstraintHandler.getBackwardCrossOverPolygon(); + case BASELINE -> reachabilityConstraintHandler.getReachabilityConstraint(); + }; + } + + private boolean deadbandAndApplyStepAdjustment() { boolean adjusted; From e3531b118abaff590f9bf2d1eb7eb36d90572afa Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 8 Dec 2025 09:11:45 -0600 Subject: [PATCH 09/12] Create variables for hard coded values so its easier to understand --- .../LookAheadCoMHeightTrajectoryGenerator.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java index 44a22f5c065c..a5e66bced836 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java @@ -425,9 +425,12 @@ private void computeWaypoints(FramePoint3DReadOnly startCoMPosition, // Handle the case where the feet are basically squared up when stepping up, but are at different heights In that case, the max height needs to be pinned // by the "from" foot, as the // way the phase variable works means we can approach what is meant to be swing on the "to" foot while still in the "from" foot. - if (Math.abs(transferToPosition.getX()) < 0.1 && endGroundHeight > startGroundHeight && transferToPosition.getZ() > transferFromPosition.getZ() + 0.05) + double THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP = 0.1; + double THRESHOLD_FOR_STEP_UP_HEIGHT = 0.05; + if (Math.abs(transferToPosition.getX()) < THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP && endGroundHeight > startGroundHeight + && transferToPosition.getZ() > transferFromPosition.getZ() + THRESHOLD_FOR_STEP_UP_HEIGHT) { - double blend = Math.abs(transferToPosition.getX()) / 0.1; + double blend = Math.abs(transferToPosition.getX()) / THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP; double minHeight = Math.min(startGroundHeight + extraToeOffHeight, endGroundHeight); endGroundHeight = EuclidCoreTools.interpolate(minHeight, endGroundHeight, blend); } From 96466a3978fb33931e882d7a07d7819932989e6d Mon Sep 17 00:00:00 2001 From: Robert Date: Sat, 13 Dec 2025 23:57:01 -0600 Subject: [PATCH 10/12] upped height for going up cinder blocks --- .../AlexanderCustomSteppingStonesTest.java | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/open-alexander/src/test/java/us/ihmc/openAlexander/obstacleCourseTests/AlexanderCustomSteppingStonesTest.java b/open-alexander/src/test/java/us/ihmc/openAlexander/obstacleCourseTests/AlexanderCustomSteppingStonesTest.java index 184d0ab93550..a31e6ce96daa 100644 --- a/open-alexander/src/test/java/us/ihmc/openAlexander/obstacleCourseTests/AlexanderCustomSteppingStonesTest.java +++ b/open-alexander/src/test/java/us/ihmc/openAlexander/obstacleCourseTests/AlexanderCustomSteppingStonesTest.java @@ -3,11 +3,13 @@ import org.junit.jupiter.api.Tag; import org.junit.jupiter.api.Test; import org.junit.jupiter.api.TestInfo; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.openAlexander.OpenAlexanderVersion; import us.ihmc.openAlexander.OpenAlexanderRobotModel; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.RobotTarget; import us.ihmc.avatar.obstacleCourseTests.AvatarCustomSteppingStonesTest; +import us.ihmc.openAlexander.parameters.controller.OpenAlexanderWalkingControllerParameters; import us.ihmc.simulationConstructionSetTools.tools.CITools; public class AlexanderCustomSteppingStonesTest extends AvatarCustomSteppingStonesTest @@ -66,7 +68,27 @@ public double getStepLength() @Override public DRCRobotModel getRobotModel() { - return new OpenAlexanderRobotModel(OpenAlexanderVersion.V1_FULL_ROBOT, RobotTarget.SCS); + return new OpenAlexanderRobotModel(OpenAlexanderVersion.V1_FULL_ROBOT, RobotTarget.SCS) + { + @Override + public WalkingControllerParameters getWalkingControllerParameters() + { + return new OpenAlexanderWalkingControllerParameters(getRobotVersion(), getTarget(), getJointMap(), getPhysicalProperties()) + { + @Override + public double nominalHeightAboveAnkle() + { + return 0.91; + } + + @Override + public double maximumHeightAboveAnkle() + { + return 0.93; + } + }; + } + }; } @Override From c9dc51bb5195b0cab21dc91f188bfa823fb095e8 Mon Sep 17 00:00:00 2001 From: Robert Date: Sat, 13 Dec 2025 23:58:55 -0600 Subject: [PATCH 11/12] cleaned up variable definition in look and step --- .../LookAheadCoMHeightTrajectoryGenerator.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java index a5e66bced836..7a2c8c936b84 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java @@ -44,6 +44,9 @@ public class LookAheadCoMHeightTrajectoryGenerator implements SCS2YoGraphicHolder { private static final double defaultPercentageInOffset = 0.05; + private static final double THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP = 0.1; + private static final double THRESHOLD_FOR_STEP_UP_HEIGHT = 0.05; + private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); private boolean visualize = true; @@ -425,8 +428,6 @@ private void computeWaypoints(FramePoint3DReadOnly startCoMPosition, // Handle the case where the feet are basically squared up when stepping up, but are at different heights In that case, the max height needs to be pinned // by the "from" foot, as the // way the phase variable works means we can approach what is meant to be swing on the "to" foot while still in the "from" foot. - double THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP = 0.1; - double THRESHOLD_FOR_STEP_UP_HEIGHT = 0.05; if (Math.abs(transferToPosition.getX()) < THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP && endGroundHeight > startGroundHeight && transferToPosition.getZ() > transferFromPosition.getZ() + THRESHOLD_FOR_STEP_UP_HEIGHT) { From 4141341c60675b02818f231b53211687d7e5636a Mon Sep 17 00:00:00 2001 From: Robert Date: Mon, 15 Dec 2025 14:14:17 -0600 Subject: [PATCH 12/12] added a comment --- .../avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java index 3d7d07b63375..1c7beb2e207b 100644 --- a/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java +++ b/ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java @@ -140,6 +140,9 @@ public void testStairs(TestInfo testInfo, } } + /** + * This tests a set of stairs that require the robot square up on each step, and are also exactly one 9" cinder block high. + */ public void testSpecialStairs(boolean goingUp, double swingDuration, double transferDuration) throws Exception