Skip to content

Commit 8b3379f

Browse files
rjgriffin42PotatoPeeler3000
rjgriffin42
and
PotatoPeeler3000
authored
added some changes to prefer previous reachable regions to yield more… (#1105)
* 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>
1 parent e667743 commit 8b3379f

File tree

2 files changed

+111
-31
lines changed

2 files changed

+111
-31
lines changed

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java‎

Lines changed: 92 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,10 @@
99
import us.ihmc.commonWalkingControlModules.captureRegion.OneStepCaptureRegionCalculator;
1010
import us.ihmc.commonWalkingControlModules.configurations.SteppingParameters;
1111
import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
12-
import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler;
1312
import us.ihmc.commons.lists.RecyclingArrayList;
1413
import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly;
1514
import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D;
1615
import us.ihmc.euclid.referenceFrame.FramePoint2D;
17-
import us.ihmc.euclid.referenceFrame.FramePoint3D;
1816
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
1917
import us.ihmc.euclid.referenceFrame.interfaces.*;
2018
import us.ihmc.euclid.tuple2D.Point2D;
@@ -49,6 +47,8 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl
4947
{
5048
private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
5149

50+
private static final double AREA_TO_CONSIDER_SWITCHING = 0.05;
51+
5252
private static final boolean VISUALIZE = true;
5353
private static final boolean CONTINUOUSLY_UPDATE_DESIRED_POSITION = true;
5454
private static final int minTicksIntoStep = 5;
@@ -78,7 +78,6 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl
7878
private final RecyclingArrayList<Point2D> upcomingFootstepContactPoints = new RecyclingArrayList<>(Point2D.class);
7979
private final YoFramePoint3D referenceFootstepPosition = new YoFramePoint3D(yoNamePrefix + "ReferenceFootstepPosition", worldFrame, registry);
8080

81-
private final FramePoint3D tempPoint = new FramePoint3D();
8281
private final FramePoint2D tempPoint2D = new FramePoint2D();
8382

8483
private final YoFrameVector2D footstepAdjustment = new YoFrameVector2D(yoNamePrefix + "FootstepAdjustment",
@@ -101,6 +100,13 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl
101100

102101
private final YoBoolean footstepWasAdjusted = new YoBoolean(yoNamePrefix + "FootstepWasAdjusted", registry);
103102

103+
private enum ReachableRegion {FORWARD, BACKWARD, BASELINE}
104+
private final YoDouble forwardReachableArea = new YoDouble(yoNamePrefix + "ForwardReachableArea", registry);
105+
private final YoDouble backwardReachableArea = new YoDouble(yoNamePrefix + "BackwardReachableArea", registry);
106+
private final YoDouble baselineReachableArea = new YoDouble(yoNamePrefix + "BaselineReachableArea", registry);
107+
private final YoDouble areaToConsiderSwitching = new YoDouble(yoNamePrefix + "areaToConsiderSwitching", registry);
108+
private final YoEnum<ReachableRegion> selectedReachableRegion = new YoEnum<>(yoNamePrefix + "SelectedReachableRegion",registry, ReachableRegion.class, true);
109+
104110
private final BooleanProvider resetFootstepProjectionEachTick;
105111
private final DoubleProvider minimumTimeForStepAdjustment;
106112
private final DoubleParameter supportDistanceFromFront;
@@ -125,7 +131,6 @@ public class ErrorBasedStepAdjustmentController implements StepAdjustmentControl
125131

126132
private final BipedSupportPolygons bipedSupportPolygons;
127133

128-
private final FramePoint3D vertexInWorld = new FramePoint3D();
129134
private final FrameConvexPolygon2D allowableAreaForCoPInFoot = new FrameConvexPolygon2D();
130135

131136
public ErrorBasedStepAdjustmentController(WalkingControllerParameters walkingControllerParameters,
@@ -189,6 +194,7 @@ public ErrorBasedStepAdjustmentController(WalkingControllerParameters walkingCon
189194
DoubleProvider innerLimit = new DoubleParameter(yoNamePrefix + "MinReachabilityWidth", registry, steppingParameters.getMinStepWidth());
190195
DoubleProvider outerLimit = new DoubleParameter(yoNamePrefix + "MaxReachabilityWidth", registry, steppingParameters.getMaxStepWidth());
191196
DoubleProvider inPlaceWidth = new DoubleParameter(yoNamePrefix + "InPlaceWidth", registry, steppingParameters.getInPlaceWidth());
197+
areaToConsiderSwitching.set(AREA_TO_CONSIDER_SWITCHING);
192198

193199
reachabilityConstraintHandler = new StepAdjustmentReachabilityConstraint(soleZUpFrames,
194200
lengthLimit,
@@ -353,6 +359,7 @@ public void initialize(double initialTime, RobotSide supportSide)
353359
previousFootstepSolution.set(footstepSolution.getPosition());
354360
totalStepAdjustment.setToZero();
355361
controlTicksIntoStep.set(0);
362+
selectedReachableRegion.set(null);
356363
}
357364

358365
@Override
@@ -440,7 +447,8 @@ public void compute(double currentTime,
440447
if (environmentallyConstrained)
441448
{
442449
tempPoint2D.set(footstepSolution.getPosition());
443-
FrameConvexPolygon2DReadOnly reachability = getBestReachabilityConstraintToUseWhenNotIntersecting();
450+
computeBestReachabilityConstraintToUseWhenNotIntersecting();
451+
FrameConvexPolygon2DReadOnly reachability = getSelectedRegion();
444452
if (!reachability.isPointInside(tempPoint2D))
445453
reachability.orthogonalProjection(tempPoint2D);
446454
footstepSolution.getPosition().set(tempPoint2D);
@@ -550,11 +558,13 @@ private void projectAdjustedStepIntoCaptureRegion(FramePoint3DReadOnly pointToPr
550558
if (!isTheCaptureRegionReachable())
551559
{
552560
captureRegionInWorld.orthogonalProjection(adjustedSolution);
553-
getBestReachabilityConstraintToUseWhenNotIntersecting().orthogonalProjection(adjustedSolution);
561+
computeBestReachabilityConstraintToUseWhenNotIntersecting();
562+
getSelectedRegion().orthogonalProjection(adjustedSolution);
554563
}
555564
else
556565
{
557-
getBestReachabilityConstraintToUseWhenIntersecting().orthogonalProjection(adjustedSolution);
566+
computeBestReachabilityConstraintToUseWhenIntersecting();
567+
getSelectedRegion().orthogonalProjection(adjustedSolution);
558568
}
559569

560570
footstepAdjustment.set(adjustedSolution);
@@ -594,38 +604,56 @@ private boolean isTheCaptureRegionReachable()
594604
return intersect;
595605
}
596606

597-
private FrameConvexPolygon2DReadOnly getBestReachabilityConstraintToUseWhenNotIntersecting()
607+
private void computeBestReachabilityConstraintToUseWhenNotIntersecting()
598608
{
599609
if (!allowCrossOverSteps.getValue())
600-
return reachabilityConstraintHandler.getReachabilityConstraint();
610+
{
611+
selectedReachableRegion.set(ReachableRegion.BASELINE);
612+
return;
613+
}
601614

602615
double distanceToForward = reachabilityConstraintHandler.getForwardCrossOverPolygon().distance(adjustedSolution);
603616
double distanceToBackward = reachabilityConstraintHandler.getBackwardCrossOverPolygon().distance(adjustedSolution);
604617
double distanceToNominal = reachabilityConstraintHandler.getReachabilityConstraint().distance(adjustedSolution);
605618

606-
boolean forwardIsCloser = distanceToForward < distanceToBackward;
607-
608-
if (forwardIsCloser)
619+
double distanceToPreviouslySelectedRegion = distanceToNominal;
620+
if (selectedReachableRegion.getEnumValue() != null)
609621
{
610-
if (distanceToNominal < distanceToForward)
611-
return reachabilityConstraintHandler.getReachabilityConstraint();
622+
if (selectedReachableRegion.getEnumValue() == ReachableRegion.FORWARD)
623+
distanceToPreviouslySelectedRegion = distanceToForward;
624+
else if (selectedReachableRegion.getEnumValue() == ReachableRegion.BACKWARD)
625+
distanceToPreviouslySelectedRegion = distanceToBackward;
612626
else
613-
return reachabilityConstraintHandler.getForwardCrossOverPolygon();
627+
distanceToPreviouslySelectedRegion = distanceToNominal;
614628
}
615-
else if (distanceToNominal < distanceToBackward)
629+
else
616630
{
617-
return reachabilityConstraintHandler.getReachabilityConstraint();
631+
// initialize this as the default
632+
selectedReachableRegion.set(ReachableRegion.BASELINE);
618633
}
619-
else
634+
635+
boolean forwardIsCloser = distanceToForward < distanceToBackward;
636+
637+
if (forwardIsCloser)
638+
{
639+
if (distanceToForward < distanceToPreviouslySelectedRegion)
640+
{
641+
selectedReachableRegion.set(ReachableRegion.FORWARD);
642+
}
643+
}
644+
else if (distanceToBackward < distanceToPreviouslySelectedRegion)
620645
{
621-
return reachabilityConstraintHandler.getBackwardCrossOverPolygon();
646+
selectedReachableRegion.set(ReachableRegion.BACKWARD);
622647
}
623648
}
624649

625-
private FrameConvexPolygon2DReadOnly getBestReachabilityConstraintToUseWhenIntersecting()
650+
private void computeBestReachabilityConstraintToUseWhenIntersecting()
626651
{
627652
if (!allowCrossOverSteps.getValue())
628-
return reachableCaptureRegion;
653+
{
654+
selectedReachableRegion.set(ReachableRegion.BASELINE);
655+
return;
656+
}
629657

630658
double forwardArea = forwardCrossOverReachableCaptureRegion.getArea();
631659
double backwardArea = backwardCrossOverReachableCaptureRegion.getArea();
@@ -635,23 +663,57 @@ private FrameConvexPolygon2DReadOnly getBestReachabilityConstraintToUseWhenInter
635663
backwardArea = Double.isNaN(backwardArea) ? Double.NEGATIVE_INFINITY : backwardArea;
636664
reachableArea = Double.isNaN(reachableArea) ? Double.NEGATIVE_INFINITY : reachableArea;
637665

666+
forwardReachableArea.set(forwardArea);
667+
backwardReachableArea.set(backwardArea);
668+
baselineReachableArea.set(reachableArea);
669+
670+
double areaOfPreviouslySelectedRegion = reachableArea;
671+
if (selectedReachableRegion.getEnumValue() != null)
672+
{
673+
areaOfPreviouslySelectedRegion = getSelectedArea();
674+
// Don't switch if the area of the previous selection isn't too low
675+
if (getSelectedArea() > areaToConsiderSwitching.getDoubleValue())
676+
return;
677+
}
678+
else
679+
{
680+
// initialize th is as the default
681+
selectedReachableRegion.set(ReachableRegion.BASELINE);
682+
}
683+
638684
boolean forwardIsLargestCrossoverArea = forwardArea > backwardArea;
639685

640686
if (forwardIsLargestCrossoverArea)
641687
{
642-
if (forwardArea > 2.0 * reachableArea)
643-
return forwardCrossOverReachableCaptureRegion;
644-
else
645-
return reachableCaptureRegion;
688+
if (forwardArea > 2.0 * areaOfPreviouslySelectedRegion)
689+
{
690+
selectedReachableRegion.set(ReachableRegion.FORWARD);
691+
}
646692
}
647-
else if (backwardArea > 2.0 * reachableArea)
693+
else if (backwardArea > 2.0 * areaOfPreviouslySelectedRegion)
648694
{
649-
return backwardCrossOverReachableCaptureRegion;
695+
selectedReachableRegion.set(ReachableRegion.BACKWARD);
650696
}
651-
else
697+
}
698+
699+
private double getSelectedArea()
700+
{
701+
return switch (selectedReachableRegion.getEnumValue())
652702
{
653-
return reachableCaptureRegion;
654-
}
703+
case FORWARD -> forwardReachableArea.getDoubleValue();
704+
case BACKWARD -> backwardReachableArea.getDoubleValue();
705+
case BASELINE -> baselineReachableArea.getDoubleValue();
706+
};
707+
}
708+
709+
private FrameConvexPolygon2DReadOnly getSelectedRegion()
710+
{
711+
return switch (selectedReachableRegion.getEnumValue())
712+
{
713+
case FORWARD -> forwardCrossOverReachableCaptureRegion;
714+
case BACKWARD -> backwardCrossOverReachableCaptureRegion;
715+
case BASELINE -> reachableCaptureRegion;
716+
};
655717
}
656718

657719
private boolean deadbandAndApplyStepAdjustment()

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/captureRegion/MultiStepCaptureRegionCalculator.java‎

Lines changed: 19 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import java.awt.*;
44

55
import us.ihmc.commonWalkingControlModules.capturePoint.stepAdjustment.StepAdjustmentReachabilityConstraint;
6+
import us.ihmc.commons.MathTools;
67
import us.ihmc.commons.lists.RecyclingArrayList;
78
import us.ihmc.euclid.geometry.ConvexPolygon2D;
89
import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly;
@@ -35,6 +36,9 @@
3536
*/
3637
public class MultiStepCaptureRegionCalculator implements SCS2YoGraphicHolder
3738
{
39+
private static final double distanceThresholdToFilter = 0.005;
40+
private static final double distanceThresholdToFilterSquared = MathTools.square(distanceThresholdToFilter);
41+
3842
private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
3943

4044
private final YoInteger stepsInQueue = new YoInteger("stepsInQueue", registry);
@@ -162,7 +166,21 @@ public void compute(RobotSide currentStanceSide, FrameConvexPolygon2DReadOnly on
162166
expandCaptureRegion(regionToExpand, reachabilityPolygonsWithOrigin.get(currentStanceSide), multiStepRegion, currentSupportMultiplier);
163167
}
164168

165-
yoMultiStepRegion.setMatchingFrame(multiStepRegion, false);
169+
multiStepRegion.update();
170+
updateInternalFilteringRepeats();
171+
}
172+
173+
private void updateInternalFilteringRepeats()
174+
{
175+
yoMultiStepRegion.clear();
176+
for (int i = 0; i < multiStepRegion.getNumberOfVertices(); i++)
177+
{
178+
FramePoint2DReadOnly previousVertex = multiStepRegion.getPreviousVertex(i);
179+
FramePoint2DReadOnly vertex = multiStepRegion.getVertex(i);
180+
if (vertex.distanceSquared(previousVertex) > distanceThresholdToFilterSquared)
181+
yoMultiStepRegion.addVertexMatchingFrame(multiStepRegion.getVertex(i), false);
182+
}
183+
yoMultiStepRegion.update();
166184
}
167185

168186
public FrameConvexPolygon2DReadOnly getCaptureRegion()

0 commit comments

Comments
 (0)