diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.java index 2cabec43e6b..10cc18e2951 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.java @@ -77,8 +77,10 @@ public class DefaultFootstepPlannerParameters extends StoredPropertySet implemen public static final DoubleStoredPropertyKey scaledFootPolygonPercentage = keys.addDoubleKey("Scaled foot polygon percentage"); public static final DoubleStoredPropertyKey cliffHeightThreshold = keys.addDoubleKey("Cliff height threshold"); - - public DefaultFootstepPlannerParameters() // for tests and stuff that's probably not gonna save + /** + * These parameters are used for tests, please don't use on an actual robot, and please don't change these values without checking that all the tests pass + */ + public DefaultFootstepPlannerParameters() { this(null); } diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedStepExpansion.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedStepExpansion.java index 4028bc9681e..abb2c3cbcd4 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedStepExpansion.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedStepExpansion.java @@ -2,7 +2,6 @@ import gnu.trove.list.array.TDoubleArrayList; import gnu.trove.list.array.TIntArrayList; -import us.ihmc.commons.InterpolationTools; import us.ihmc.commons.MathTools; import us.ihmc.euclid.axisAngle.AxisAngle; import us.ihmc.euclid.geometry.ConvexPolygon2D; @@ -20,6 +19,8 @@ public class ParameterBasedStepExpansion implements FootstepExpansion { + private static final boolean SORT_FULL_EXPANSION = false; + private final List fullExpansion = new ArrayList<>(); private final DefaultFootstepPlannerParametersReadOnly parameters; private final IdealStepCalculatorInterface idealStepCalculator; @@ -38,7 +39,9 @@ public class ParameterBasedStepExpansion implements FootstepExpansion private final TIntArrayList xyExpansionMask = new TIntArrayList(); private final TIntArrayList yawExpansionMask = new TIntArrayList(); - public ParameterBasedStepExpansion(DefaultFootstepPlannerParametersReadOnly parameters, IdealStepCalculatorInterface idealStepCalculator, SideDependentList footPolygons) + public ParameterBasedStepExpansion(DefaultFootstepPlannerParametersReadOnly parameters, + IdealStepCalculatorInterface idealStepCalculator, + SideDependentList footPolygons) { this.parameters = parameters; this.idealStepCalculator = idealStepCalculator; @@ -76,12 +79,8 @@ public void initialize() if (reachSquared > maxReachSquared) continue; - double reachFraction = EuclidCoreTools.fastSquareRoot(reachSquared) / parameters.getMaxStepReach(); - double minYawAtFullExtension = parameters.getMinStepYaw(); - double maxYawAtFullExtension = parameters.getMaxStepYaw(); - - double minYaw = InterpolationTools.linearInterpolate(parameters.getMinStepYaw(), minYawAtFullExtension, reachFraction); - double maxYaw = InterpolationTools.linearInterpolate(parameters.getMaxStepYaw(), maxYawAtFullExtension, reachFraction); + double minYaw = parameters.getMinStepYaw(); + double maxYaw = parameters.getMaxStepYaw(); for (double yaw = minYaw; yaw <= maxYaw; yaw += LatticePoint.gridSizeYaw) { @@ -148,13 +147,16 @@ public void doFullExpansion(FootstepGraphNode nodeToExpand, List listToFilter, FootstepGraphNode stanceNode) @@ -177,18 +179,6 @@ private void applyMask(List listToFilter, FootstepGraphNode s }); } - private static int computeMinYawDistance(List listToFilter, DiscreteFootstep idealStep) - { - int minYawDistance = Integer.MAX_VALUE; - for (int i = 0; i < listToFilter.size(); i++) - { - int yawDistance = idealStep.computeYawIndexDistance(listToFilter.get(i).getSecondStep()); - if (yawDistance < minYawDistance) - minYawDistance = yawDistance; - } - return minYawDistance; - } - private static int computeMinXYManhattanDistance(List listToFilter, DiscreteFootstep idealStep) { int minXYManhattanDistance = Integer.MAX_VALUE; @@ -201,6 +191,18 @@ private static int computeMinXYManhattanDistance(List listToF return minXYManhattanDistance; } + private static int computeMinYawDistance(List listToFilter, DiscreteFootstep idealStep) + { + int minYawDistance = Integer.MAX_VALUE; + for (int i = 0; i < listToFilter.size(); i++) + { + int yawDistance = idealStep.computeYawIndexDistance(listToFilter.get(i).getSecondStep()); + if (yawDistance < minYawDistance) + minYawDistance = yawDistance; + } + return minYawDistance; + } + static class IdealStepProximityComparator implements Comparator { private DiscreteFootstep idealStep = null; @@ -239,8 +241,8 @@ private DiscreteFootstep constructNodeInPreviousNodeFrame(double stepLength, dou footstepRotation.transform(footstepTranslation); return new DiscreteFootstep(step.getX() + footstepTranslation.getX(), - step.getY() + footstepTranslation.getY(), - stepYaw + step.getYaw(), + step.getY() + footstepTranslation.getY(), + stepYaw + step.getYaw(), step.getRobotSide().getOppositeSide()); } } diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedNodeExpansionTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedNodeExpansionTest.java index 34b22480447..0e2d492cb0a 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedNodeExpansionTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedNodeExpansionTest.java @@ -1,6 +1,8 @@ package us.ihmc.footstepPlanning.graphSearch.stepExpansion; import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; import us.ihmc.commons.InterpolationTools; import us.ihmc.euclid.geometry.ConvexPolygon2D; @@ -19,81 +21,105 @@ import java.util.*; import java.util.function.ToDoubleFunction; -import static us.ihmc.robotics.Assert.assertTrue; +import static org.junit.jupiter.api.Assertions.*; public class ParameterBasedNodeExpansionTest { private static final double epsilon = 1e-6; + private DefaultFootstepPlannerParameters parameters; + + @BeforeEach + public void setupParameters() + { + // We create default parameters for the tests + parameters = new DefaultFootstepPlannerParameters(); + } @Test - public void testExpansionAlongBoundsFromOriginDefaultParametersWithRight() + public void testExpansionAlongBoundsFromOriginWithRight() { - DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters(); ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, null, PlannerTools.createDefaultFootPolygons()); expansion.initialize(); - double maxYaw = parameters.getMaxStepYaw(); - double minYaw = parameters.getMinStepYaw(); - - List childNodes = new ArrayList<>(); - + // Set up the feet to test moving a right foot DiscreteFootstep stanceStep = new DiscreteFootstep(0.0, 0.0, 0.0, RobotSide.LEFT); DiscreteFootstep startOfSwingStep = new DiscreteFootstep(0.0, 0.3, 0.0, RobotSide.RIGHT); + // Do full expansion of the steps + List childNodes = new ArrayList<>(); expansion.doFullExpansion(new FootstepGraphNode(startOfSwingStep, stanceStep), childNodes); + + // Check the edges of where steps can reach in the x and y directions DiscreteFootstep mostForward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getX())); - DiscreteFootstep furthestReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth()))); DiscreteFootstep mostBackward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getX())); DiscreteFootstep mostInward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getY())); DiscreteFootstep mostOutward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getY())); + + assertTrue(mostForward.getX() <= parameters.getMaxStepReach()); + assertTrue(mostBackward.getX() >= parameters.getMinStepLength()); + assertTrue(mostInward.getY() <= -parameters.getMinStepWidth()); + assertTrue(mostOutward.getY() >= -parameters.getMaxStepWidth()); + + // Check the min and max a step can yaw DiscreteFootstep mostOutwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -snapToCircle(node.getYaw()))); DiscreteFootstep mostInwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> snapToCircle(node.getYaw()))); - assertTrue(mostForward.getX() < parameters.getMaxStepReach() + epsilon); - assertTrue(mostBackward.getX() > parameters.getMinStepLength() - epsilon); - assertTrue(mostInward.getY() < -parameters.getMinStepWidth() + epsilon); - assertTrue(mostOutward.getY() > -parameters.getMaxStepWidth() - epsilon); + // The yaw is a little tricky because we don't have any notion of things being negative. + // So if the yaw is clockwise (which should be negative) it will be ~5.7 or something just less then 2 PI radians + // To account for this, we need to subtract the yaw we got from 2 PI. + double outwardYawFromZero = Math.PI * 2 - mostOutwardYawed.getYaw(); + assertTrue(outwardYawFromZero <= parameters.getMaxStepYaw()); + assertTrue(mostInwardYawed.getYaw() >= parameters.getMinStepYaw()); - assertTrue(getReachAtNode(furthestReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach()); + // Get the footstep closest to the ideal step and ensure that it's less than the max reach + DiscreteFootstep idealReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth()))); + assertTrue(getReachAtNode(idealReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach()); } @Test - public void testExpansionAlongBoundsFromOriginDefaultParametersWithLeft() + public void testExpansionAlongBoundsFromOriginWithLeft() { - DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters(); ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, null, PlannerTools.createDefaultFootPolygons()); expansion.initialize(); - double maxYaw = parameters.getMaxStepYaw(); - double minYaw = parameters.getMinStepYaw(); - + // Set up the feet to test moving a right foot DiscreteFootstep stanceStep = new DiscreteFootstep(0.0, 0.0, 0.0, RobotSide.RIGHT); DiscreteFootstep startOfSwingStep = new DiscreteFootstep(0.0, -0.3, 0.0, RobotSide.LEFT); + // Do full expansion of the steps List childNodes = new ArrayList<>(); expansion.doFullExpansion(new FootstepGraphNode(startOfSwingStep, stanceStep), childNodes); + + // Check the edges of where steps can reach in the x and y directions DiscreteFootstep mostForward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getX())); - DiscreteFootstep furthestReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth()))); DiscreteFootstep mostBackward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getX())); DiscreteFootstep mostInward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getY())); DiscreteFootstep mostOutward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getY())); - DiscreteFootstep mostOutwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> snapToCircle(node.getYaw()))); - DiscreteFootstep mostInwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -snapToCircle(node.getYaw()))); - assertTrue(mostForward.getX() < parameters.getMaxStepReach() + epsilon); - assertTrue(mostBackward.getX() > parameters.getMinStepLength() - epsilon); - assertTrue(mostInward.getY() > parameters.getMinStepWidth() - epsilon); - assertTrue(mostOutward.getY() < parameters.getMaxStepWidth() + epsilon); + assertTrue(mostForward.getX() <= parameters.getMaxStepReach()); + assertTrue(mostBackward.getX() >= parameters.getMinStepLength()); + assertTrue(mostInward.getY() >= parameters.getMinStepWidth()); + assertTrue(mostOutward.getY() <= parameters.getMaxStepWidth()); - double mostOutwardYawedReach = getReachAtNode(mostOutwardYawed, parameters.getIdealFootstepWidth()); - double mostInwardYawedReach = getReachAtNode(mostInwardYawed, parameters.getIdealFootstepWidth()); - assertTrue(getReachAtNode(furthestReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach()); + // Check the min and max a step can yaw + DiscreteFootstep mostOutwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -snapToCircle(node.getYaw()))); + DiscreteFootstep mostInwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> snapToCircle(node.getYaw()))); + + // The yaw is a little tricky because we don't have any notion of things being negative. + // So if the yaw is clockwise (which should be negative) it will be ~5.7 or something just less then 2 PI radians + // To account for this, we need to subtract the yaw we got from 2 PI. + double outwardYawFromZero = Math.PI * 2 - mostOutwardYawed.getYaw(); + assertTrue(outwardYawFromZero <= parameters.getMaxStepYaw()); + assertTrue(mostInwardYawed.getYaw() >= parameters.getMinStepYaw()); + + // Get the footstep closest to the ideal step and ensure that it's less than the max reach + DiscreteFootstep idealReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth()))); + assertTrue(getReachAtNode(idealReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach()); } @Test public void testExpansionAlongBoundsFromOrigin() { - DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters(); ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, null, PlannerTools.createDefaultFootPolygons()); expansion.initialize(); @@ -112,7 +138,8 @@ public void testExpansionAlongBoundsFromOrigin() List childNodes = new ArrayList<>(); expansion.doFullExpansion(new FootstepGraphNode(startOfSwingStep, stanceStep), childNodes); DiscreteFootstep mostForward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getX())); - DiscreteFootstep furthestReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth()))); + DiscreteFootstep furthestReach = getExtremumNode(childNodes, + Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth()))); DiscreteFootstep mostBackward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getX())); DiscreteFootstep mostInward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getY())); DiscreteFootstep mostOutward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getY())); @@ -171,11 +198,11 @@ else if (comparator.compare(node.getSecondStep(), extremumNode) == 1) @Test public void testPartialExpansionSize() { - DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters(); int branchFactor = 100; parameters.setMaxBranchFactor(branchFactor); - IdealStepCalculatorInterface idealStepCalculator = (stance, startOfSwing) -> new DiscreteFootstep(stance.getLatticePoint(), stance.getRobotSide().getOppositeSide()); + IdealStepCalculatorInterface idealStepCalculator = (stance, startOfSwing) -> new DiscreteFootstep(stance.getLatticePoint(), + stance.getRobotSide().getOppositeSide()); ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, idealStepCalculator, PlannerTools.createDefaultFootPolygons()); expansion.initialize(); @@ -202,7 +229,13 @@ public void testPartialExpansionSize() Assertions.assertTrue(expansionList.isEmpty()); } + /** + * This test is meant to check if the full expansion returns a sorted list or not. + * We don't always sort the full expansion, so by default, this test is + * disabled. + */ @Test + @Disabled public void testFullExpansionReturnsSortedOrder() { Random random = new Random(329032); @@ -230,7 +263,8 @@ public void testFullExpansionReturnsSortedOrder() expansion.doFullExpansion(node, fullExpansion); List fullExpansionSorted = new ArrayList<>(fullExpansion); - ToDoubleFunction stepDistance = step -> ParameterBasedStepExpansion.IdealStepProximityComparator.calculateStepProximity(step.getSecondStep(), idealStep); + ToDoubleFunction stepDistance = step -> ParameterBasedStepExpansion.IdealStepProximityComparator.calculateStepProximity(step.getSecondStep(), + idealStep); Comparator sorter = Comparator.comparingDouble(stepDistance); fullExpansionSorted.sort(sorter);