Cleaned up the ParameterBasedStepExpansion and tunes some parameters,…

… updated a couple tests as well
ihmcrobotics · Oct 21, 2024 · 220ba22 · 220ba22
1 parent f38284f
commit 220ba22
Show file tree

Hide file tree

Showing 3 changed files with 102 additions and 64 deletions.
diff --git a/...ava/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.java b/...ava/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/.../java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedStepExpansion.java b/.../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<FootstepGraphNode> 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<ConvexPolygon2D> footPolygons)
+   public ParameterBasedStepExpansion(DefaultFootstepPlannerParametersReadOnly parameters,
+                                      IdealStepCalculatorInterface idealStepCalculator,
+                                      SideDependentList<ConvexPolygon2D> 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<FootstepGraphNo
          applyMask(fullExpansionToPack, nodeToExpand);
       }
 
-      // sorting is primarily a debug tool for checking proximity to ideal step - skip by default
+      // Sorting is primarily a debug tool for checking proximity to ideal step
+      if (SORT_FULL_EXPANSION)
+      {
+         if (idealStepCalculator != null)
+         {
+            idealStepProximityComparator.update(nodeToExpand, idealStepCalculator);
+            fullExpansionToPack.sort(idealStepProximityComparator);
+         }
+      }
 
-//      if (idealStepCalculator != null)
-//      {
-//         idealStepProximityComparator.update(nodeToExpand, idealStepCalculator);
-//         fullExpansionToPack.sort(idealStepProximityComparator);
-//      }
    }
 
    private void applyMask(List<FootstepGraphNode> listToFilter, FootstepGraphNode stanceNode)
@@ -177,18 +179,6 @@ private void applyMask(List<FootstepGraphNode> listToFilter, FootstepGraphNode s
                             });
    }
 
-   private static int computeMinYawDistance(List<FootstepGraphNode> 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<FootstepGraphNode> listToFilter, DiscreteFootstep idealStep)
    {
       int minXYManhattanDistance = Integer.MAX_VALUE;
@@ -201,6 +191,18 @@ private static int computeMinXYManhattanDistance(List<FootstepGraphNode> listToF
       return minXYManhattanDistance;
    }
 
+   private static int computeMinYawDistance(List<FootstepGraphNode> 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<FootstepGraphNode>
    {
       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/...a/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedNodeExpansionTest.java b/...a/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<FootstepGraphNode> 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<FootstepGraphNode> 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<FootstepGraphNode> 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<FootstepGraphNode> 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<FootstepGraphNode> fullExpansionSorted = new ArrayList<>(fullExpansion);
 
-            ToDoubleFunction<FootstepGraphNode> stepDistance = step -> ParameterBasedStepExpansion.IdealStepProximityComparator.calculateStepProximity(step.getSecondStep(), idealStep);
+            ToDoubleFunction<FootstepGraphNode> stepDistance = step -> ParameterBasedStepExpansion.IdealStepProximityComparator.calculateStepProximity(step.getSecondStep(),
+                                                                                                                                                       idealStep);
             Comparator<FootstepGraphNode> sorter = Comparator.comparingDouble(stepDistance);
             fullExpansionSorted.sort(sorter);