Skip to content

added some changes to prefer previous reachable regions to yield more… #1105

New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Nov 18, 2025
+111 −31
Merged

added some changes to prefer previous reachable regions to yield more… #1105

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
186c97a
added some changes to prefer previous reachable regions to yield more…
rjgriffin42 Nov 17, 2025
1e0627b
reverted line change
rjgriffin42 Nov 18, 2025
8d4c4a7
Merge branch 'demo/alex-nov-2025' into bugfix/capture-regions
PotatoPeeler3000 Nov 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
122 changes: 92 additions & 30 deletions ...WalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -78,7 +78,6 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl
private final RecyclingArrayList<Point2D> 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",
Expand All @@ -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<ReachableRegion> selectedReachableRegion = new YoEnum<>(yoNamePrefix + "SelectedReachableRegion",registry, ReachableRegion.class, true);

private final BooleanProvider resetFootstepProjectionEachTick;
private final DoubleProvider minimumTimeForStepAdjustment;
private final DoubleParameter supportDistanceFromFront;
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -353,6 +359,7 @@ public void initialize(double initialTime, RobotSide supportSide)
previousFootstepSolution.set(footstepSolution.getPosition());
totalStepAdjustment.setToZero();
controlTicksIntoStep.set(0);
selectedReachableRegion.set(null);
}

@Override
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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();
Expand All @@ -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()
Expand Down
20 changes: 19 additions & 1 deletion ...a/us/ihmc/commonWalkingControlModules/captureRegion/MultiStepCaptureRegionCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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()
Expand Down
Loading